/*-------------------------------------------------------------------
 File description :
 cpp file for Gps_func class;
 Calculate sat visibility using Almanac file, current time, user's rough location
 Read GPS nav message, get ephemeris data;
 Calculate pseudorange;
 Resolve P.V.T. ( position, velocity, time bias );

--------------------------------------------------------------------*/

#include "Gpsfunc.h"
#include "CMatrix3.h"
#include <time.h>
#include <math.h>
#include <assert.h>
#include <string.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);
}

//------------------------------------------------------------------
// Class constructor and destructor

GPS_Func::GPS_Func()
{
 double pos[3];

 thrdtermed = true;
 for( int i=0; i<SatMax+1; i++)
 {
  Almanac[i].SatId = 0;  // id == 0 means this is not a valid data
  Ephemeris[i].valid = 0; // init epe validity to false
  eph_valid[i] = false;
  Ephemeris[i].iodc = -1; // invalid iodc
  Ephemeris[i].iode = -1; // invalid iode
 }

 for( int i=0; i<CH_NUM; i++)
  for( int j=0; j<5; j++)
  navmsgparity[i].par[j] = 0x1;  // init parity values for 5 frm of all channels

 memset(ch_prn, 0, CH_NUM*sizeof( unsigned char ));

 // default userposition is in Riverside, CA  :=)
 pos[0] = -117.3;  // longi
 pos[1] = 33.4;  // lati
 pos[2] = 500.;  // altitude

 // init user's position to invalid value;

 ECEFuserpos[0] = 1.;
 ECEFuserpos[1] = 1.;
 ECEFuserpos[2] = 1.;
 NEDuserpos[0] = 0.;      //long
 NEDuserpos[1] = 0.;      //lat
 NEDuserpos[2] = 0.;      //alt
 ECEFvel[0] = 0.;
 ECEFvel[1] = 0.;
 ECEFvel[2] = 0.;
 clk_bias = 0.;
 pdop=tdop=hdop=vdop=gdop=0.;

 // init user's accurate time to invalid value;
 AccurateTime = -1.;

 for( int i=0; i<CH_NUM; i++)
 {
  allch_navresult[i].prn = 0;
  allch_navresult[i].valid = false;
  allch_navresult[i].pseudorange = 0.;
  allch_navresult[i].ele = 0.;
  allch_navresult[i].azi = 0.;
 }

 SetUserPos(pos);
#ifdef GPS_FUNC_DBG
 debugfile.open(".\\log\\gps_func.log",ios::out);
#endif
}
GPS_Func::~GPS_Func()
{
#ifdef GPS_FUNC_DBG
 debugfile.close();
#endif
}
//------------------------------------------------------------------
// set longitude, latitude, altitude
// and calculate tangent plane parameters
void GPS_Func::SetUserPos( double* Pos)
{
 double SinLat, CosLat, SinLong, CosLong;
 UserPos[0] = Pos[0];  // longi
 UserPos[1] = Pos[1];  // lati
 UserPos[2] = Pos[2];  // altitude


 SinLat  = sin(Pos[1]*PI/180.);
 CosLat  = cos(Pos[1]*PI/180.);
 SinLong = sin(Pos[0]*PI/180.);
 CosLong = cos(Pos[0]*PI/180.);

 PlaneCE  =  CosLat;
 PlaneET1 = -SinLong;
 PlaneET2 =  CosLong;
 PlaneNT1 = -SinLat * CosLong;
 PlaneNT2 = -SinLat * SinLong;
 PlaneUTx =  CosLat * CosLong;
 PlaneUTy =  CosLat * SinLong;
 PlaneUTz =  SinLat;
 
}
//--------------------------------------------------------------------
//  PosECEF : destination position , in ECEF format
//  Pos:      souce position , in llh format
void GPS_Func::ConvertToECEF( double *PosECEF, double *Pos)
{
  double Dx = 0.0;
  double Dy = 0.0;
  double Dz = 0.0;

  double NormRadius, ActHeight;
  double CosLat, SinLat, CosLong, SinLong;
                 
  CosLong = cos( Pos[0] * PI / 180.0 );
  SinLong = sin( Pos[0] * PI / 180.0 );
  CosLat  = cos( Pos[1] * PI / 180.0 );
  SinLat  = sin( Pos[1] * PI / 180.0 );

  NormRadius = SemiMajorAxis /
    sqrt( 1.0 - ( EccentrSquared * SinLat * SinLat ) );
  ActHeight = NormRadius + Pos[2];

  PosECEF[0] = ActHeight * CosLat * CosLong - Dx;
  PosECEF[1] = ActHeight * CosLat * SinLong - Dy;
  PosECEF[2] = ( OneMinusEccentrSquared * NormRadius + Pos[2]) * SinLat - Dz;
}

//--------------------------------------------------------------------
// Pos:   Desitination position , in llh format
// PosECEF: Source position, in ECEF format

void GPS_Func::ConvertToLongLatAlt( double *Pos, double *PosECEF)
{
  double P, Q, R, D, SinL, SinL2;
  double X, Y, Z;
  double Long, Lat, Alt;

  X = PosECEF[0];
  Y = PosECEF[1];
  Z = PosECEF[2];

  Long = atan2( Y, X );

  P = sqrt( (X * X) + (Y * Y) );
  Q = Z / P;
  Alt = 0.0;

  Lat = atan2( Q, OneMinusEccentrSquared );

  do {
    SinL = sin(Lat);
    SinL2 = SinL * SinL;
    R = SemiMajorAxis / sqrt(1.0 - (EccentrSquared * SinL2));
    D = Alt;
    Alt = (P / cos(Lat)) - R;
    Lat = atan2(Q * (R + Alt), (OneMinusEccentrSquared * R) + Alt);
  }  while ( fabs(Alt - D) > 0.2 );

  Pos[0] = Long * 180.0 / PI;
  Pos[1] = Lat  * 180.0 / PI;
  Pos[2] = Alt;

}
//--------------------------------------------------------------------
//  sec : in 0-59;
//  min : in 0-59;
//  hr  : in 0-23;
//  day : in 1-31;
//  mon : in 1-12;
// calculate Julian day using sec,min,hr,day,mon,yr
long double GPS_Func::CalcJulianDay(int sec,int min,int hr,int day,int mon,int year)
{
 int e_year,e_mon;
 long double jd,h_jd, tmp_d;


 if( mon <=2){
  e_mon = mon +12;
  e_year = year -1;
 }
 else
 {
  e_mon = mon;
  e_year = year;
 }
 h_jd = (hr + min/60. + sec/3600.)/24.;
 tmp_d = (long double)(floor(365.25*e_year)) + (long double)(floor(30.6001*(e_mon+1)))+ (long double)day;
 jd =  tmp_d +  h_jd + 1720981.5L;
 return jd;
}
// calculate julian day using tm variable
long double GPS_Func::CalcJulianDay(struct tm * gt)
{
 int sec,min,hr,day,mon,year;

 sec = gt->tm_sec;
 min = gt->tm_min;
 hr  = gt->tm_hour;
 day = gt->tm_mday;
 mon = gt->tm_mon+1;
 year = gt->tm_year+1900;

 return CalcJulianDay(sec,min,hr,day,mon,year);
}
//--------------------------------------------------------------------
// calculate GPS week using Julian day
int GPS_Func::CalcGpsWeek(long double jd )
{
  int gw;
  gw = floor((jd-2444244.5)/7.); // 2444244.5 is JD of Jan,6,1980
  return (gw%1024);
}
// use current system time to calc gps time
void GPS_Func::get_gpsweek(void)
{
 struct tm *gmt;
 time_t curtime;
 double jd;

 time(&curtime);
 gmt = gmtime(&curtime);
 jd = CalcJulianDay(gmt);
 GpsWeek = CalcGpsWeek(jd);
}
// calculate time(in sec) of week using julian day
int GPS_Func::CalcToW( long double jd)
{
 int gw ;
 gw = floor((jd-2444244.5L)/7.);
 return( jd -2444244.5L - gw*7.)*SecPerDay ;
}
//--------------------------------------------------------------------
// read Almanac data for one satellite

void GPS_Func::read_almparam(AlmInfo *almpt, FILE *almfile)
{
  char info[1024],*begin_info;
  int tmp_int;
  double tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_HEALTH_STR)) ) break;
  }
  // we find health string 
  sscanf(begin_info, "Health:%d", &tmp_int);
  almpt->Health = tmp_int;
  
  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_ECCEN_STR)) ) break;
  }
  // we find eccentricity str 
  sscanf(begin_info, "Eccentricity: %lg", &tmp_double);
  almpt->E = tmp_double;


  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_TIME_STR)) ) break;
  }
  // we find time of availablity str 
  sscanf(begin_info, "Time of Applicability(s): %lg", &tmp_double);
  almpt->Toa = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_ORB_STR)) ) break;
  }
  // we find ORB str
  sscanf(begin_info, "Orbital Inclination(rad): %le", &tmp_double);
  almpt->DeltaI = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_RRA_STR)) ) break;
  }
  // we find RRA str 
  sscanf(begin_info, "Rate of Right Ascen(r/s): %le", &tmp_double);
  almpt->OmegaDot = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_SQRTA_STR)) ) break;
  }
  // we find SQRTA str 
  sscanf(begin_info, "SQRT(A)  (m 1/2): %le", &tmp_double);
  almpt->SqrtA = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_RAAW_STR)) ) break;
  }
  // we find RAAW str 
  sscanf(begin_info, "Right Ascen at Week(rad): %le", &tmp_double);
  almpt->Omega0 = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_AOP_STR)) ) break;
  }
  // we find AOP str 
  sscanf(begin_info, "Argument of Perigee(rad): %le", &tmp_double);
  almpt->Omega = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_MA_STR)) ) break;
  }
  // we find MA str 
  sscanf(begin_info, "Mean Anom(rad): %le",  &tmp_double);
  almpt->M0 = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_AF0_STR)) ) break;
  }
  // we find AF0 str 
  sscanf(begin_info, "Af0(s): %le", &tmp_double);
  almpt->Af0 = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_AF1_STR)) ) break;
  }
  // we find AF1 str 
  sscanf(begin_info, "Af1(s/s): %le", &tmp_double);
  almpt->Af1 = tmp_double;

  while( (fgets(info, 1024, almfile)) != NULL){
    if( (begin_info=strstr(info, ALM_WK_STR)) ) break;
  }
  // we find Week str 
  sscanf(begin_info, "week: %d", &tmp_int);
  almpt->WeekNo = tmp_int;
}
//--------------------------------------------------------------------
// read Almanac data for all satellites
void GPS_Func::ReadAlm(FILE *almfile)
{
  char info[1024],*begin_info, dummy[1024];
  int satid;

  AlmInfo  *alm;

  while( (fgets(info,1024,almfile)) != NULL){
    begin_info = strstr(info, ALM_ID_STR);  // find the beginning of ID str
    if( begin_info ){
      sscanf(begin_info, "%s %d", dummy, &satid); // read the id
      assert(satid <32);
      assert(satid >=0);
      alm = &Almanac[satid];
      alm->SatId = satid;
      read_almparam(alm, almfile);
    } // end of if( begin_info )
  }// end of while
}
//--------------------------------------------------------------------
// calculate sat position using ephemeris data and time of week
void GPS_Func::SatPosECEFEphinfo(EphInfo *EphPtr, double Time, int WeekNo, double *Pos)
{
 double d_toc,bclk,tc, d_toe, ei, ea, diff, aol, ta;
 double delr, delal, delinc, r, inc, la, xp, yp;

 d_toc = Time - EphPtr->toc;
 if(d_toc>302400.0)
  d_toc = d_toc - 604800.0;
 else if(d_toc<-302400.0)
  d_toc = d_toc + 604800.0;

 bclk = EphPtr->af0 + EphPtr->af1*d_toc + EphPtr->af2*d_toc*d_toc
	-EphPtr->tgd;
 tc = Time - bclk;

 d_toe = tc - EphPtr->toe;
 if(d_toe>302400.0)
  d_toe = d_toe-604800.0;
 else if(d_toe<-302400.0)
  d_toe = d_toe+604800.0;

 ei = EphPtr->ma + d_toe*(EphPtr->wm + EphPtr->dn);
 ea = ei;
 do
 {
   diff = (ei - (ea - EphPtr->ety*sin(ea)))/(1.0E0 - EphPtr->ety*cos(ea));
   ea = ea + diff;
 } while (fabs(diff) > 1.0e-12 );

 bclk = bclk - 4.442807633E-10*EphPtr->ety*EphPtr->sqra*sin(ea);
 EphPtr->bclk = bclk;

//     ea is the eccentric anomaly
 ta = atan2(sqrt(1.00-pow(EphPtr->ety,2))*sin(ea),cos(ea)-EphPtr->ety);

//     TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)

 aol = ta + EphPtr->w;

//     AOL IS THE ARGUMENT OF LATITUDE OF THE SATELLITE

//     calculate the second harmonic perturbations of the orbit

 delr  = EphPtr->crc*cos(2.0*aol) + EphPtr->crs*sin(2.0*aol);
 delal = EphPtr->cuc*cos(2.0*aol) + EphPtr->cus*sin(2.0*aol);
 delinc= EphPtr->cic*cos(2.0*aol) + EphPtr->cis*sin(2.0*aol);

 //     R IS THE RADIUS OF SATELLITE ORBIT AT TIME T

 r = pow(EphPtr->sqra,2)*(1.00 - EphPtr->ety*cos(ea)) + delr;
 aol = aol + delal;