#include <stdio.h>
#include <stdlib.h>
#include "rinex.h"
#include "sv.h"

int read_RinexEPP( FILE *RinexEPP_file,SVText *snv );
void WriteSatPosFile(FILE *SatPosFile,int prn,XYZCoor *SVPos);
void GetSVPos(SVText *Text,double t,double *X,double *Y,double *Z);
double Get_atan(double z,double y);
int  GetGPSTime(int year,int month,int day,
		   int hour,int minute,double second,double *gpstime);
int index[32];
int prn[32];
double X[32],Y[32],Z[32];
double code[32],phase[32];
SVText snv[32];
double gpstime;
double dt[32];
void main()
{
  FILE *fo,*fn,*fs;
  double x0=-2377249.0;
  double y0=5387268.0;
  double z0=2442992.25;
  double C=2.997e8;
  double dis,  err;
  int i,j, is;
  char temp[200];
  int year,mon,day,hour,min,flag,num;
  double sec;

  if ((fn = fopen("data.98n", "rt"))== NULL)
   { fprintf(stderr, "Cannot open input file.\n");    exit(1); }
  if ((fo = fopen("data.98o", "rt"))== NULL)
   { fprintf(stderr, "Cannot open input file.\n");    exit(1); }
  if ((fs = fopen("data.out", "wa")) == NULL)
   { fprintf(stderr, "Cannot open outut file.\n");   exit(1);  }

   /* seek to the beginning of the file */
  i=0;
  do { if(read_RinexEPP(fn,&snv[i])) break; index[i++]=snv[i].prn; }while(1);
  /* compute satellite position */

  while(!feof(fo))
  {
    fscanf(fo,"%i%i%i%i%i%lf%i%i",&year,&mon,&day,&hour,&min,&sec,&flag,&num);
    for(i=0;i<num;i++) fscanf(fo,"%i",&prn[i]);
    for(i=0;i<num;i++) fscanf(fo,"  %lf      %lf",&code[i],&phase[i]);

    GetGPSTime(year+1900,mon,day,hour,min,sec, &gpstime);

    fprintf(fs,"%3i%3i%3i%3i%3i%11.7f%3i%3i",
		year,mon,day,hour,min,sec,flag,num);
    for(i=0;i<num;i++) fprintf(fs," %2i",prn[i]);
    fprintf(fs,"\n");

    for(i=0;i<num;i++)
    {
      j=0;
      do{ if(( prn[i] == index[j])||(prn[i] ==0) )  break;
	  else j++;
	  if(j>12) return;
	}while(1);
      gpstime -= code[i]/C;
      dt[i]=snv[j].af0 +snv[j].af1*(gpstime-snv[j].toe)
	   +snv[j].af2*(gpstime-snv[j].toe)*(gpstime-snv[j].toe);
      gpstime +=dt[i];
      code[i] +=dt[i]*C;
      GetSVPos(&snv[j],gpstime,&X[i],&Y[i],&Z[i]);
    }
    for(i=0;i<num;i++)
    {
       fprintf(fs,"%14.3f  %14.3f",code[i],phase[i]);
       fprintf(fs,"%14.3f  %14.3f  %14.3f",  X[i],Y[i],Z[i] );
       dis=sqrt( (X[i]-x0)*(X[i]-x0)
	      + (Y[i]-y0)*(Y[i]-y0)
	      + (Z[i]-z0)*(Z[i]-z0) );
       err=dis - code[i];
       fprintf(fs,"%14.3f  %14.3f\n",  dis,err );
    }
  }
  fclose(fo); fclose(fn);
  fclose(fs);
  free(snv);
}


/*********************** read_EPP() *************************/


int read_RinexEPP( FILE *RinexEPP_file,SVText *snv )

{
   int j;
   char t1[30],t2[30],t3[30],t4[30],t5[30];
   int year,month,day,hour,minute;
   double second, gpstime;

   /* PRN /EPOCH /SV CLK */
   if(fscanf(RinexEPP_file,"%d%d%d%d%d%d%s%s%s%s\n",
		       &snv->prn,&year,&month,&day,
		       &hour,&minute,t1,t2,t3,t4)==EOF) return 1;
   second=atof(t1);
   snv->af0=atof(t2);
   snv->af1=atof(t3);
   snv->af2=atof(t4);
   snv->wn = GetGPSTime(year+1900,month,day,hour,minute,second,
			&gpstime);
   snv->tow = (long) gpstime;

   /* BROADCAST ORBIT -1  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4)
	==EOF) return 1;
   snv->aode=atof(t1);
   snv->crs=atof(t2);
   snv->deltan=atof(t3);
   snv->m0=atof(t4);
   /* BROADCAST ORBIT -2  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4)
	==EOF) return 1;
   snv->cuc=atof(t1);
   snv->e=atof(t2);
   snv->cus=atof(t3);
   snv->roota=atof(t4);
   /* BROADCAST ORBIT -3  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n", t1,t2,t3,t4)
	==EOF) return 1;
   snv->toe=atof(t1);
   snv->cic=atof(t2);
   snv->omega0=atof(t3);
   snv->cis=atof(t4);
   /* BROADCAST ORBIT -4  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4)
	==EOF) return 1 ;
   snv->i0=atof(t1);
   snv->crc=atof(t2);
   snv->omega=atof(t3);
   snv->omegadot=atof(t4);
   /* BROADCAST ORBIT -5  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4)
	==EOF) return 1 ;
   snv->idot=atof(t1);
   snv->wn=atof(t3);
   /* BROADCAST ORBIT -6  */
   if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4)
	==EOF) return 1 ;
   snv->tgd=atof(t3);

   return(0);
}


/* Using the datas from SV navigation Text to compute the SV position in ECEFCoor */
void GetSVPos(SVText *Text,double t,double *X,double *Y,double *Z)
{
  double mu=3.986005e14;    /* Earth constant , unit m3/s2 */
  double n0,n;             /* mean rate of angle */
  double tk, mk, ek1, ek2, ek;
  double Error=1.0e-12;
  double vk, phik;
  double deltau,deltar,deltai;
  double uk, rk, ik;
  double xk, yk;
  double omegak;
  double omegae=7.292115147e-5;  /* earth self round rate of angle */

  /* compute SV mean rate of angle */
  n0 = sqrt(mu) / (Text->roota * Text->roota * Text->roota);
  n = n0 + Text->deltan ;

  /* compute time tk */
  tk = t - Text->toe;
  if(tk > 302400) tk -= 604800;
  else if(tk < -302400) tk +=604800;

  /* compute mean anomoly at measure time */
  mk = Text->m0  + n * tk;

  /* computer ek */
  ek1 = mk;
  do
  {
    ek2 = mk + Text->e * sin(ek1);
    if(fabs(ek2-ek1)<=Error ) break;
    ek1=ek2;
  }while(1);
  ek=ek1;

  /* computer vk */
//  vk = atan(sqrt(1.0-Text->e *Text->e)*sin(ek)) / (cos(ek)-Text->e);
  vk = Get_atan(cos(ek)-Text->e,sqrt(1.0-Text->e *Text->e)*sin(ek));

  /* compute phik */
  phik = vk + Text->omega ;

  /* compute deltau,deltar,deltai */
  deltau = Text->cuc * cos(2.0*phik) + Text->cus *sin(2.0*phik) ;
  deltar = Text->crc * cos(2.0*phik) + Text->crs *sin(2.0*phik) ;
  deltai = Text->cic * cos(2.0*phik) + Text->cis *sin(2.0*phik) ;

  /* computer uk, rk and ik */
  uk = phik +deltau;
  rk = Text->roota *Text->roota * (1.0-Text->e * cos(ek)) + deltar ;
  ik = Text->i0 + deltai + Text->idot * tk ;

  /* compute xk,yk */
  xk = rk * cos(uk);
  yk = rk * sin(uk);

  /* compute omegak */
  omegak = Text->omega0  + (Text->omegadot  - omegae)*tk -omegae * Text->toe ;

  /* compute ECEF */
  *X = xk * cos(omegak) - yk * cos(ik) *sin(omegak) ;
  *Y = xk * sin(omegak) + yk * cos(ik) *cos(omegak) ;
  *Z = yk*sin(ik) ;
}  /* End of Function */

 /*  Caculate x=atan(y/z)  */
double Get_atan(double z,double y)
{
   double x;
   if (z==0) x=M_PI/2;
   else{
	if (y==0) x=M_PI;
	else{
	      x=atan(fabs(y/z));
	      if ((y>0)&&(z<0)) x=M_PI-x;
	      else if ((y<0)&&(z<0)) x=M_PI+x;
		   else if((y<0)&&(z>0)) x=2*M_PI-x;
	     }
       }
   return x;
}






/**********************************************
     Convert date and time
	to GPS time
***********************************************/

int  GetGPSTime(int year,int month,int day,
		   int hour,int minute,double second,double *gpstime)
{
  int   dayofw,dayofy, yr, ttlday, m, weekno;

/* Check limits of day, month and year */
  if (year < 1981 || month < 1 || month > 12 || day < 1 || day > 31)
     weekno = 0;

/*  Convert day, month and year to day of year */
  if (month == 1)
     dayofy = day;
  else
  {
    dayofy = 0;
    for (m=1; m<=(month-1); m++)
    {
      dayofy += dinmth[m];
      if ( m==2 )
      {
	if (year % 4 == 0 && year % 100 != 0 || year % 400 == 0)
	   dayofy += 1;
      }
    }
    dayofy += day;
  }

/*  Convert day of year and year into week number and day of week */
  ttlday = 360;
  for (yr=1981; yr<=(year-1); yr++)
  {
    ttlday  += 365;
    if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 ==0)
       ttlday  += 1;
  }
  ttlday += dayofy;
  weekno  = ttlday/7;
  dayofw  = ttlday - 7 * weekno;

  *gpstime =  (hour * 3600 + minute * 60 + second + dayofw * 86400);
  return weekno;
}