#include "Gpsnav.h"
/*******************************************************************************
FUNCTION exor(char bit, long parity)
RETURNS  None.

PARAMETERS
			bit     char
			parity  long

PURPOSE
			count the number of bits set in the parameter parity and
			do an exclusive or with the parameter bit

WRITTEN BY
	Clifford Kelley
CHANGED BY
        Yu Lu

*******************************************************************************/
static int exor(char bit, unsigned int parity)
{
  char i;
  int result, tmp_par;
  result=0;
  tmp_par = parity>>6;
  for (i=0;i<24;i++)
  {
    if( tmp_par & 0x1) result++;
    tmp_par = tmp_par>>1;
  }
  result=result%2;

  result=(bit ^ result) & 0x1;
  return(result);
}
/*******************************************************************************
FUNCTION xors(long pattern)
RETURNS  Integer

PARAMETERS
			  pattern  long 32 bit data

PURPOSE
			  count the number of bits set in "pattern"
WRITTEN BY
	Clifford Kelley
CHANGED  by
         Yu Lu

*******************************************************************************/
static int xors(long pattern)
{
  int count,i;
  count=0;
  pattern=pattern>>6;
  for (i=0;i<=25;i++)
  {
	 count+=int(pattern & 0x1);
	 pattern=pattern>>1;
  }
  count=count%2;
  return(count);
}

static int sign(double a)
{
 if( a>0) return 1;
 else return -1;
}

static int bit_sign(double a)
{
 if( a>0 ) return 1;
 else return 0;
}
//---------------------------------------------------------------------------
/*    Routine Description:                                                 */
/*  Constructor of class GPS_Nav_class;                                    */
/***************************************************************************/

GPS_Nav::GPS_Nav(IoInterface* Iohandle)
{
 IoHandle = Iohandle;
 navthrd = NULL;
 newfrmflag = false;
 newfrmcount = 0;
 thrdtermed= true;
 memset((void*)&frm_4ms_6ch, 0, sizeof(one_frame_data));
 count = 0;
 count1 = 0;
 TIC_count =  0;
 ch_status = 0;
 for(char i=0; i<CH_NUM ; i++)
  ch[i].subframe_i = -1;

}

/*  Destructor of class GPS_Nav_class;  */  
GPS_Nav::~GPS_Nav()
{
 if(navthrd) {
  delete navthrd;
 }

#ifdef GPS_NAV_DBG
 debugfile.close();
#endif

#if GPS_NAV_RAW_DATA > 0
 ch_accum_mis_file.close();
 for(int i=0; i<CH_NUM; i++)
  ch_rawdata_file[i].close();
#endif
}
//---------------------------------------------------------------------------
/* Routine Description:
  To add one reg to be written to the gp2021regs
  Argument:
          add:  the address of register to be written;
          hi :  high byte
          lo : low byte
  return:
          no
*/
/***************************************************************************/
void GPS_Nav::add_one_reg( unsigned char add,
                           unsigned char hi,
                           unsigned char lo)
{
 unsigned char reg_count;

 if(gp2021_wr_data.count < 41)
 {
  reg_count = gp2021_wr_data.count++;
  gp2021_wr_data.regs[reg_count].addr = add;
  gp2021_wr_data.regs[reg_count].low  = lo;
  gp2021_wr_data.regs[reg_count].high = hi;
 }
}
void GPS_Nav::clear_all_reg()
{
 memset( (void *)(&gp2021_wr_data), 0, sizeof(gp2021_regs));
}
/***************************************************************************/
/* GP2021  related function *********/
void GPS_Nav::ch_cntl(char ch,  unsigned short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = (ch+CH_OFFSET)<<3;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}

void GPS_Nav::ch_code( char ch,unsigned int freq)
{
 unsigned short high_freq, low_freq;
 high_freq = (freq>>16) & 0xffff;
 low_freq  =  freq & 0xffff;

 // first is high 16-bit freq word
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<3)+5;
 gp2021_wr_data.regs[idx].high = (high_freq>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  high_freq & 0xff;

 // next is low 16-bit freq word
 assert(gp2021_wr_data.count < 41);
 idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<3)+6;
 gp2021_wr_data.regs[idx].high = (low_freq>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  low_freq & 0xff;
}

void GPS_Nav::ch_carrier(char ch,unsigned  int freq)
{
 unsigned short high_freq, low_freq;
 high_freq = (freq>>16) & 0xffff;
 low_freq  =  freq & 0xffff;

 // first is high 16-bit freq word
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<3)+3;
 gp2021_wr_data.regs[idx].high = (high_freq>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  high_freq & 0xff;

 // next is low 16-bit freq word
 assert(gp2021_wr_data.count < 41);
 idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<3)+4;
 gp2021_wr_data.regs[idx].high = (low_freq>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  low_freq & 0xff;
}
void GPS_Nav::ch_epoch_load(char ch, unsigned short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<3)+7;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}
void GPS_Nav::ch_on(char ch)
{
 ch_status=ch_status | bit_pat[ch+CH_OFFSET];
}
void GPS_Nav::ch_off(char ch)
{
 ch_status=ch_status & !bit_pat[ch+CH_OFFSET];
}
void GPS_Nav::set_ch_onoff(void)
{
 reset_cntl(ch_status);
}
void GPS_Nav::test_control(unsigned  short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0x7c;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}
void GPS_Nav::system_setup(unsigned  short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0x7e;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}
void GPS_Nav::io_config(unsigned  short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0xf0;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}
void GPS_Nav::reset_cntl(unsigned  short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0x7f;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}
void GPS_Nav::program_TIC(unsigned  int data)
{
 unsigned short high_16, low_16;
 high_16 = (data>>16) & 0xffff;
 low_16  =  data & 0xffff;

 // first is high 16-bit  word
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0x6d;
 gp2021_wr_data.regs[idx].high = (high_16 >> 8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  high_16 & 0xff;

 // next is low 16-bit  word
 assert(gp2021_wr_data.count < 41);
 idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0x6f;
 gp2021_wr_data.regs[idx].high = (low_16>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  = (low_16) & 0xff;
}
void GPS_Nav::ch_accum_reset(char ch)
{
}
void GPS_Nav::ch_code_slew(char ch, unsigned short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = ((ch+CH_OFFSET)<<2)+0x84;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}

void GPS_Nav::test_data(unsigned  short data)
{
 assert(gp2021_wr_data.count < 41);
 char idx = gp2021_wr_data.count++;
 gp2021_wr_data.regs[idx].addr = 0xf2;
 gp2021_wr_data.regs[idx].high = (data>>8) & 0xff;
 gp2021_wr_data.regs[idx].low  =  data & 0xff;
}

void GPS_Nav::get_newaccumdata(void)
{
 double q_sum, i_sum, tmp_theta;

 epoch_chk  = 1;
 for( int j=0; j<4; j++)
  if((frm_4ms_6ch.all_ch[j].accum_status_b) & 0x2000 ) // TIC happened here
   epoch_chk = 0;      // epoch_check values are not available

 for( int i=0; i< CH_NUM; i++)
  ch[i].valid_data_num = 0;

 for(int i=0; i<CH_NUM; i++)
 {
  for(int j=0; j<4; j++)
  {
   if( (frm_4ms_6ch.all_ch[j].accum_status_a) & (ch_ready_mask[i+CH_OFFSET]) )
   {
    // we have valid data from channel i in jth packet;
    ch[i].prompt_iq[ ch[i].valid_data_num++ ][0] = frm_4ms_6ch.all_ch[j].ch_accm[i].I_prompt;
    ch[i].prompt_iq[ ch[i].valid_data_num -1 ][1] = frm_4ms_6ch.all_ch[j].ch_accm[i].Q_prompt;
    ch[i].track_iq[ ch[i].valid_data_num -1 ][0] = frm_4ms_6ch.all_ch[j].ch_accm[i].I_track;
    ch[i].track_iq[ ch[i].valid_data_num -1 ][1] = frm_4ms_6ch.all_ch[j].ch_accm[i].Q_track;
    if( epoch_chk )
     ch[i].epochchk[ ch[i].valid_data_num -1 ] = *((unsigned short*)(&frm_4ms_6ch.ch_cntl[0])+j*6+ i);

   }   // end of if( (frm_4ms_6ch.all_ch[j
  }    // end of j  for(int j=0; j<4; j++){
  ch[i].mean_p=0.;
  ch[i].mean_t=0.;
  for( int k=0; k< ch[i].valid_data_num; k++)
  {
   ch[i].track_mag[k] = sqrt(pow((float)(ch[i].track_iq[ k ][0]),2)+pow((float)(ch[i].track_iq[ k ][1]),2));
   ch[i].prompt_mag[k]= sqrt(pow((float)(ch[i].prompt_iq[ k ][0]),2)+pow((float)(ch[i].prompt_iq[ k ][1]),2));

   ch[i].mean_p += ch[i].prompt_mag[k];
   ch[i].mean_t += ch[i].track_mag[k];
   q_sum = ch[i].prompt_iq[k][1];// + ch[i].track_iq[k][1];
   i_sum = ch[i].prompt_iq[k][0];// + ch[i].track_iq[k][0];

   if( ch[i].state >= PULLIN)
   {
    if( q_sum != 0. || i_sum != 0.)
     ch[i].theta[k] = atan2( q_sum, i_sum);
    else
    {
     if( k == 0)
      ch[i].theta[k] = ch[i].old_theta;
     else
      ch[i].theta[k] = ch[i].theta[k-1];
    }
   }  //  end of if( ch[i].state

   if( ch[i].state == TRACKING)
   {
    if( i_sum !=0 || q_sum != 0)
     ch[i].ca_phase[k] = ch[i].theta[k]-(PI/2)*sign(q_sum);
//     ch[i].ca_phase[k] = -i_sum*sign(q_sum)/sqrt(i_sum*i_sum + q_sum*q_sum);
   }
  }   // end of k
  ch[i].mean_p = ch[i].mean_p/ch[i].valid_data_num;
  ch[i].mean_t = ch[i].mean_t/ch[i].valid_data_num;

  if( ch[i].state >= PULLIN)
  {
   ch[i].cd_phase1 =
   (ch[i].mean_p - ch[i].mean_t)/(ch[i].mean_p + ch[i].mean_t)*pull_code_k;
  }
 }  // end of for(int i=0;
}
void GPS_Nav::process_channel( int idx )
{
 assert( idx < CH_NUM && idx >=0);

 switch( ch[idx].state){
  case ACQUISITION: ch_acq(idx); break;
  case CONFIRM: ch_confirm(idx); break;
  case PULLIN: ch_pullin(idx); break;
  case TRACKING: ch_track(idx); break;
  default: break;
 }
}

void  GPS_Nav::ch_acq(int idx)
{
 assert( idx < CH_NUM && idx >=0);
 if( ch[idx].mean_p < CONFIRM_THRESHOLD || ch[idx].mean_t < CONFIRM_THRESHOLD )
 {
  if(ch[idx].code > 2044)
  {
   ch[idx].code =0;
   ch[idx].n_freq ++;
   ch_carrier(idx,carrier_ref+(int)(4698*ch[idx].n_freq));
   ch[idx].carrier_freq = carrier_ref+(int)(4698*ch[idx].n_freq);

   if(ch[idx].n_freq >= SEARCH_MAX_F)
   {
    ch[idx].n_freq = SEARCH_MIN_F;
   }
  }
  else
  {
   ch_code_slew(idx,2);
   ch[idx].code += 2;
  }
 }
 else
 {
  ch_code_slew(idx,2042);  // slide back 2 chips , because of frm delay
  ch[idx].state = CONFIRM;
  ch[idx].n_confirm = 0;
  ch[idx].mn_confirm = 0;
 }
}
void  GPS_Nav::ch_confirm(int idx)
{
 assert( idx < CH_NUM && idx >=0);

 ch[idx].n_confirm++;
 if( ch[idx].mean_p > CONFIRM_THRESHOLD)// && ch[idx].mean_t > 500. )
  ch[idx].mn_confirm++;

 if(ch[idx].n_confirm == 7)
 {
  if( ch[idx].mn_confirm < 4 )