/*******************************************************************************
    OpenAirInterface 
    Copyright(c) 1999 - 2014 Eurecom

    OpenAirInterface is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.


    OpenAirInterface is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with OpenAirInterface.The full GNU General Public License is 
   included in this distribution in the file called "COPYING". If not, 
   see <http://www.gnu.org/licenses/>.

  Contact Information
  OpenAirInterface Admin: openair_admin@eurecom.fr
  OpenAirInterface Tech : openair_tech@eurecom.fr
  OpenAirInterface Dev  : openair4g-devel@eurecom.fr
  
  Address      : Eurecom, Campus SophiaTech, 450 Route des Chappes, CS 50193 - 06904 Biot Sophia Antipolis cedex, FRANCE

 *******************************************************************************/
/*!\brief Phase error compensation*/
///
/// Accomplishes the phase error compensation for CHBCH and SCH channels
///

#ifndef USER_MODE
#define __NO_VERSION__

//#include "from_grlib_softregs.h"

#else
#include <stdio.h>
#include <stdlib.h>

#include <pthread.h>
#endif

#include "PHY/defs.h"
#include "PHY/extern.h"
#include "SCHED/extern.h"
#include "defs.h"
#include "extern.h"

#include "mmintrin.h"


#ifndef USER_MODE
#define openair_get_mbox() (*(unsigned int *)mbox)
#endif //USER_MODE

static __m64 perror64 __attribute__ ((aligned(16)));
static __m64 Rsymb_conj64 __attribute__ ((aligned(16)));

int phy_phase_compensation_top (unsigned int pilot_type, unsigned int initial_pilot,
				unsigned int last_pilot, int ignore_prefix)
{

#ifdef PC_TARGET

  // i  - pilot index
  // i2 - I have no idea
  // aa - receive antenna index
  unsigned short i, i2, aa;

  // Temporary buffer or similar
  int *input;

  // Our SCH indices
  unsigned int pilot_offset, sch_index;

  unsigned char frequency_reuse_ind = 1;

  unsigned char log2_avg,log2_perror_amp;
	
  struct complex16 perror, *Rchsch, *Rsymb, *Rsch;
  __m64 *Rchsch64, *Rsch64;
	
  register __m64 mm0, mm1;

  int ind, ind64;
  int chr, chi;
  int rx_energy[NB_ANTENNAS_RX];
  unsigned char *chbch_pdu;
  unsigned int chbch_size;
  unsigned int time_in, time_out;

  unsigned int number_of_pilots = last_pilot - initial_pilot;

#ifdef USER_MODE
  char fname[40], vname[40];
#endif //USER_MODE

#ifdef DEBUG_PHY
  msg ("[openair][PHY][CODING] Correction of phase error\n");
#endif // DEBUG_PHY

  for (aa = 0; aa < NB_ANTENNAS_RX; aa++)
    {
      // Lets check to see if the phase compensation is done over the CHBCH or SCH 
      if (pilot_type == 0)	// Were working with the CHBCH
	{

	  for (i = 0; i < NUMBER_OF_CHBCH_SYMBOLS; i++)
	    {

	      // Review this code... It is NOT WORKING!!!
#ifndef USER_MODE
	      if (openair_daq_vars.mode == openair_NOT_SYNCHED)
		{
		  input = &PHY_vars->rx_vars[aa].
		    RX_DMA_BUFFER[(PHY_vars->rx_vars[0].offset +
				   ((i +
				     SYMBOL_OFFSET_CHBCH) <<
				    LOG2_NUMBER_OF_OFDM_CARRIERS) + (i +
								     1 +
								     SYMBOL_OFFSET_CHBCH)
				   * CYCLIC_PREFIX_LENGTH) %
				  FRAME_LENGTH_SAMPLES];
		}
	      else
		{
#ifdef HW_PREFIX_REMOVAL
		  input = &PHY_vars->rx_vars[aa].
		    RX_DMA_BUFFER[((i +
				    SYMBOL_OFFSET_CHBCH) <<
				   LOG2_NUMBER_OF_OFDM_CARRIERS)];
#else
		  input = &PHY_vars->rx_vars[aa].
		    RX_DMA_BUFFER[((i +
				    SYMBOL_OFFSET_CHBCH) <<
				   LOG2_NUMBER_OF_OFDM_CARRIERS) + (i +
								    SYMBOL_OFFSET_CHBCH
								    +
								    1) *
				  CYCLIC_PREFIX_LENGTH];
#endif //HW_PREFIX_REMOVAL
		}
#else //USER_MODE

	      input = &PHY_vars->rx_vars[aa].
		RX_DMA_BUFFER[(PHY_vars->rx_vars[0].offset +
			       ((i +
				 SYMBOL_OFFSET_CHBCH) <<
				LOG2_NUMBER_OF_OFDM_CARRIERS) + (i + 1 +
								 SYMBOL_OFFSET_CHBCH)
			       * CYCLIC_PREFIX_LENGTH) %
			      FRAME_LENGTH_SAMPLES];

#endif //USER_MODE

	      //    msg("[openair][PHY][CHBCH %d] frame %d: Decoding -> FFT %d\n",frequency_reuse_ind,frame,i);

	      //				dump_chbch_pilots (0);

	      fft ((short *) &input[0],
		   (short *) &PHY_vars->chbch_data[frequency_reuse_ind].
		   rx_sig_f[aa][i << (1 + LOG2_NUMBER_OF_OFDM_CARRIERS)],
		   (short *) twiddle_fft, rev, LOG2_NUMBER_OF_OFDM_CARRIERS, 3,
		   0);



	      // Phase error compensation

	      //				dump_chbch_pilots (0);
	      if (NUMBER_OF_CHBCH_PILOTS)
		{

		  perror.r = 0;
		  perror.i = 0;

		  Rchsch = (struct complex16 *) &PHY_vars->
		    chsch_data[frequency_reuse_ind].rx_sig_f[aa][0];
		  Rsymb = (struct complex16 *) &PHY_vars->
		    chbch_data[frequency_reuse_ind].
		    rx_sig_f[aa][i <<
				 (1 + LOG2_NUMBER_OF_OFDM_CARRIERS)];

		  Rchsch64 =
		    (__m64 *) & PHY_vars->
		    chsch_data[frequency_reuse_ind].rx_sig_f[aa][0];


		  // inner product of received CHSCH in pilot positions and received symbol
		  mm1 = _m_pxor (mm1, mm1);

		  for (i2 = 0; i2 < NUMBER_OF_CHBCH_PILOTS; i2++)
		    {
		      ind = PHY_vars->chbch_data[frequency_reuse_ind].
			pilot_indices[i2] << 1;
		      ind64 = PHY_vars->chbch_data[frequency_reuse_ind].
			pilot_indices[i2];

#ifdef DEBUG_PHY
#ifdef USER_MODE
		      msg ("[OPENAIR][PHY][CHBCH DEMOD]Ant %d symbol %d (%p), pilot %d (%d): RX p (%d,%d), RX s (%d,%d)\n", aa, i, Rsymb, i2, ind64, Rchsch[ind].r, Rchsch[ind].i, Rsymb[ind].r, Rsymb[ind].i);
#endif /* USER_MODE */
#endif /* DEBUG_PHY */
		      //      perror.r += ( ((Rchsch[ind].r*Rsymb[ind].r)>>PERROR_SHIFT) + ((Rchsch[ind].i*Rsymb[ind].i)>>PERROR_SHIFT) );
		      //      perror.i += ( ((Rchsch[ind].i*Rsymb[ind].r)>>PERROR_SHIFT) - ((Rchsch[ind].r*Rsymb[ind].i)>>PERROR_SHIFT) );
		      // MMX version

		      ((short *) &Rsymb_conj64)[0] = Rsymb[ind].r;
		      ((short *) &Rsymb_conj64)[1] = Rsymb[ind].i;
		      ((short *) &Rsymb_conj64)[2] = -Rsymb[ind].i;
		      ((short *) &Rsymb_conj64)[3] = Rsymb[ind].r;

		      mm0 = _mm_madd_pi16 (Rchsch64[ind64], Rsymb_conj64);
		      mm1 = _mm_add_pi32 (mm0, mm1);
		    }

		  perror64 =
		    _mm_srai_pi32 (mm1,
				   PERROR_SHIFT +
				   LOG2_NUMBER_OF_CHBCH_PILOTS);
		  perror.r = ((short *) &perror64)[0];
		  perror.i = ((short *) &perror64)[2];

		  // base shift for rotation on amplitude first OFDM symbol only (might be better to compute the max over the CHBCH 
		  if ((aa == 0) && (i == 0))
		    {
		      log2_perror_amp =
			log2_approx ((unsigned int) ((int) perror.
						     r * perror.r +
						     (int) perror.
						     i *
						     perror.
						     i)) >> 1;
		    }

		  // Apply rotation
		  rotate_cpx_vector ((short *) Rsymb, (short *) &perror,
				     (short *) Rsymb, NUMBER_OF_OFDM_CARRIERS,
				     log2_perror_amp, 0);

#ifdef DEBUG_PHY
#ifdef USER_MODE

		  msg ("[OPENAIR][PHY][CHBCH DEMOD] Ant %d : symbol %d, perror = (%d,%d) , approx amp %d\n", aa, i, perror.r, perror.i, log2_perror_amp);
#endif //USER_MODE
#endif //DEBUG_PHY
		}
	    }
	}
      else		// Were working with the SCH
	{
	  // Foreach PILOT SCH
	  for (pilot_offset = initial_pilot; pilot_offset <= last_pilot; pilot_offset++)
	    {
	      // Set the absolute pilot index wrt the frame:
	      sch_index = EMOS_SCH_INDEX;

	      for (i = 0; i < NUMBER_OF_SCH_SYMBOLS; i++)
		{

		  if (ignore_prefix == 1)
		    {
		      input = &PHY_vars->rx_vars[aa].RX_DMA_BUFFER[(i + pilot_offset) << (LOG2_NUMBER_OF_OFDM_CARRIERS)];
		    }
		  else
		    {
		      input = &PHY_vars->rx_vars[aa].RX_DMA_BUFFER[((i + pilot_offset) << (LOG2_NUMBER_OF_OFDM_CARRIERS)) + (i + 1 + pilot_offset) *
								   CYCLIC_PREFIX_LENGTH];
		    }


		  fft ((short *) &input[0],
		       (short *) &PHY_vars->sch_data[sch_index].rx_sig_f[aa][i << (1 + LOG2_NUMBER_OF_OFDM_CARRIERS)],
		       (short *) twiddle_fft, rev,
		       LOG2_NUMBER_OF_OFDM_CARRIERS, 3, 0);

		  perror.r = 0;
		  perror.i = 0;

		  // Reference SCH
		  Rsch = (struct complex16 *) &PHY_vars->sch_data[sch_index].rx_sig_f[aa][initial_pilot];
		  Rsch64 = (__m64 *) & PHY_vars->chsch_data[sch_index].rx_sig_f[aa][initial_pilot]; //IN MMX format

		  // SCH to be de-rotated
		  Rsymb = (struct complex16 *) &PHY_vars->sch_data[sch_index].rx_sig_f[aa][pilot_offset << (1 + LOG2_NUMBER_OF_OFDM_CARRIERS)];

		  // inner product of received CHSCH in pilot positions and received symbol
		  mm1 = _m_pxor (mm1, mm1);

		  for (i2 = 0; i2 < NUMBER_OF_OFDM_CARRIERS; i2++)
		    {
		      // Indices
		      ind = i2 << 1;
		      ind64 = i2;

#ifdef DEBUG_PHY
#ifdef USER_MODE
		      msg ("[OPENAIR][PHY][CHBCH DEMOD]Ant %d symbol %d (%p), pilot %d (%d): RX p (%d,%d), RX s (%d,%d)\n", aa, i, Rsymb, i2, ind64, Rchsch[ind].r, Rchsch[ind].i, Rsymb[ind].r, Rsymb[ind].i);
#endif /* USER_MODE */
#endif /* DEBUG_PHY */
		      //      perror.r += ( ((Rchsch[ind].r*Rsymb[ind].r)>>PERROR_SHIFT) + ((Rchsch[ind].i*Rsymb[ind].i)>>PERROR_SHIFT) );
		      //      perror.i += ( ((Rchsch[ind].i*Rsymb[ind].r)>>PERROR_SHIFT) - ((Rchsch[ind].r*Rsymb[ind].i)>>PERROR_SHIFT) );
		      // MMX version

		      // Initializing the vector for complex mult
		      ((short *) &Rsymb_conj64)[0] = Rsymb[ind].r;
		      ((short *) &Rsymb_conj64)[1] = Rsymb[ind].i;
		      ((short *) &Rsymb_conj64)[2] = -Rsymb[ind].i;
		      ((short *) &Rsymb_conj64)[3] = Rsymb[ind].r;

		      mm0 = _mm_madd_pi16 (Rchsch64[ind64], Rsymb_conj64);
		      mm1 = _mm_add_pi32 (mm0, mm1);
		    }

		  // this is a bitwise shift
		  perror64 = _mm_srai_pi32 (mm1, PERROR_SHIFT + LOG2_NUMBER_OF_OFDM_CARRIERS);
		  perror.r = ((short *) &perror64)[0];
		  perror.i = ((short *) &perror64)[2];

		  // base shift for rotation on amplitude first OFDM symbol only (might be better to compute the max over the CHBCH 
		  if ((aa == 0) && (i == 0))
		    {
		      // For normalization in the followinf routine
		      log2_perror_amp = log2_approx((unsigned int) ((int) perror.r * perror.r + (int)perror.i * perror.i)) >> 1;
		    }

		  // Apply rotation
		  rotate_cpx_vector ((short *) Rsymb,
				     (short *) &perror,
				     (short *) Rsymb,
				     NUMBER_OF_OFDM_CARRIERS,
				     log2_perror_amp,
				     0);

#ifdef DEBUG_PHY
#ifdef USER_MODE

		  msg ("[OPENAIR][PHY][CHBCH DEMOD] Ant %d : symbol %d, perror = (%d,%d) , approx amp %d\n", aa, i, perror.r, perror.i, log2_perror_amp);
#endif //USER_MODE
#endif //DEBUG_PHY
		}











	      phy_channel_estimation ((short *) &PHY_vars->
				      sch_data[sch_index].
				      rx_sig_f[aa][i <<
						   (1 +
						    LOG2_NUMBER_OF_OFDM_CARRIERS)],
				      (short *) &PHY_vars->
				      sch_data[sch_index].
				      channel[aa][i <<
						  (1 +
						   LOG2_NUMBER_OF_OFDM_CARRIERS)],
				      (short *) &PHY_vars->
				      sch_data[sch_index].
				      channel_f[aa][i <<
						    (1 +
						     LOG2_NUMBER_OF_OFDM_CARRIERS)],
				      (short *) &PHY_vars->
				      sch_data[sch_index].
				      channel_matched_filter_f[aa][i <<
								   (1 +
								    LOG2_NUMBER_OF_OFDM_CARRIERS)],
				      (short *) &PHY_vars->sch_data[0].
				      SCH_conj_f[i <<
						 (1 +
						  LOG2_NUMBER_OF_OFDM_CARRIERS)],
				      15, //LOG2_SCH_RX_F_AMP,
				      (NB_ANTENNAS_RX == 1) ? 1 : 0);

#ifdef USER_MODE
#ifdef DEBUG_PHY

	      sprintf (fname, "sch%d_channelF%d.m", sch_index, aa);
	      sprintf (vname, "sch%d_chanF%d", sch_index, aa);


	      write_output (fname,
			    vname,
			    (short *) &PHY_vars->sch_data[sch_index].
			    channel_f[aa][0], 2 * NUMBER_OF_OFDM_CARRIERS, 2, 1);

	      sprintf (fname, "sch%d_channel%d.m", sch_index, aa);
	      sprintf (vname, "sch%d_chan%d", sch_index, aa);

	      write_output (fname,
			    vname,
			    (short *) &PHY_vars->sch_data[sch_index].
			    channel[aa][0], 2 * NUMBER_OF_OFDM_CARRIERS, 2, 1);

#endif // DEBUG_PHY
#endif // USER_MODE

	    }
	}
    }


#endif //PC_TARGET
}


void phy_phase_compensation (short *ref_sch, short *tgt_sch, short *out_sym, int ignore_prefix, int aa, struct complex16 *perror_out)
{
  struct complex16 perror, *Rsymb, *Rsch;
	
  struct complex32 perror32;
	
  __m64 perror64, Rsymb_conj64, Rsch64;
  __m64 Rsymb64[NUMBER_OF_OFDM_CARRIERS];
  __m64 *tgt_sch64 = (__m64 *)tgt_sch;
	
  register __m64 mm0, mm1, mm2;
	
  int i,i2;
	
  unsigned char log2_avg,log2_perror_amp;
	
  int ind, ind64;
	
  perror.r = 0;
  perror.i = 0;
	
  perror32.r = 0;
  perror32.i = 0;
	
  // Reference SCH
  Rsch = (struct complex16 *) ref_sch;
	
  // SCH to be de-rotated
  Rsymb = (struct complex16 *) tgt_sch;
	
	
  // The following code copies the data from *tgt_sch to *Rsymb64 and 
  // converts it into the following format: | Re0 Im0 Re0 Im0 || Re1 Im1 Re1 Im1 |
  i2=0;
  for(i=0;i<((NUMBER_OF_OFDM_CARRIERS)>>1);i++)      // reverse 2 complex samples at a time => does the loop size>>1 times
    {

      mm0 = tgt_sch64[i];

      mm1 = mm0;
      mm2 = mm0;

      mm1 = _m_punpckldq(mm1,mm0);
      mm2 = _m_punpckhdq(mm2,mm0);

      Rsymb64[i2++]=mm1;             // get the output index (reverse bit ordering index)
      Rsymb64[i2++]=mm2;
	
    }
		
  // inner product of received CHSCH in pilot positions and received symbol
  mm1 = _m_pxor (mm1, mm1);
	
  for (i = 0; i < NUMBER_OF_OFDM_CARRIERS; i++)
    {
      // Indices
      ind = i;
      //ind64 = i;

      // Pure C version 
      // 		perror32.r += ( (((int)Rsch[ind].r*(int)Rsymb[ind].r) >> PERROR_SHIFT) + (((int)Rsch[ind].i*(int)Rsymb[ind].i) >> PERROR_SHIFT) );
      // 		perror32.i += ( (((int)Rsch[ind].i*(int)Rsymb[ind].r) >> PERROR_SHIFT) - (((int)Rsch[ind].r*(int)Rsymb[ind].i) >> PERROR_SHIFT) );
		
      // MMX version
      ((short *) &Rsch64)[0] = Rsch[ind].r;
      ((short *) &Rsch64)[1] = Rsch[ind].i;
      ((short *) &Rsch64)[2] = Rsch[ind].r;
      ((short *) &Rsch64)[3] = Rsch[ind].i;
		
      // Initializing the vector for complex mult
      ((short *) &Rsymb_conj64)[0] = Rsymb[ind].r;
      ((short *) &Rsymb_conj64)[1] = Rsymb[ind].i;
      ((short *) &Rsymb_conj64)[2] = -Rsymb[ind].i;
      ((short *) &Rsymb_conj64)[3] = Rsymb[ind].r;
		
      //Complex multiplication 16 -> 32 bits
      mm0 = _mm_madd_pi16 (Rsch64, Rsymb_conj64);
      mm1 = _mm_add_pi32 (mm0, mm1);
    }
	
  // Believe me... this is a bitwise shift! (bringing back to 16 bits)
  perror64 = _mm_srai_pi32 (mm1, PERROR_SHIFT + LOG2_NUMBER_OF_OFDM_CARRIERS);
  perror.r = ((short *) &perror64)[0];
  perror.i = ((short *) &perror64)[2];
			
  // 	perror.r = (short)(perror32.r >> LOG2_NUMBER_OF_OFDM_CARRIERS);
  // 	perror.i = (short)(perror32.i >> LOG2_NUMBER_OF_OFDM_CARRIERS);
	
  // base shift for rotation on amplitude first OFDM symbol only (might be better to compute the max over the CHBCH)
  // 	if (aa == 0)
  // 	{
  // For normalization in the following routine
  log2_perror_amp = log2_approx((unsigned int) ((int)(perror.r * perror.r) + (int)(perror.i * perror.i))) >> 1;
  // 	}

  *perror_out = perror;

  // Apply rotation
  rotate_cpx_vector ((short *) Rsymb64, (short *) &perror, (short *) out_sym, NUMBER_OF_OFDM_CARRIERS, log2_perror_amp,0);
}