#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <cpgplot.h>
#include <wcslib/cel.h>
#include "sgdp4h.h"
#include <getopt.h>

#define LIM 80
#define NMAX 256
#define D2R M_PI/180.0
#define R2D 180.0/M_PI
#define XKMPER 6378.135 // Earth radius in km
#define XKMPAU 149597879.691 // AU in km
#define FLAT (1.0/298.257)
#define XKE 0.07436680 // Guassian Gravitational Constant
#define AE 1.0
#define XMNPDA 1440.0

long Isat=0;
long Isatsel=0;
extern double SGDP4_jd0;

struct point {
  int flag,satno;
  char desig[10];
  double mjd,ra,de,rac,dec;
  float st,sr;
  char iod_line[LIM];
  double dx,dy,dr,dt;
  xyz_t obspos;
};
struct doppler {
  double mjd,freq,v;
  xyz_t obspos,obsvel;
};
struct data {
  int n,nsel,m;
  struct point *p;
  struct doppler *q;
  double chisq,rms;
} d;
struct site {
  int id;
  double lng,lat;
  float alt;
  char observer[64];
};
orbit_t orb;
struct site get_site(int site_id);
struct point decode_iod_observation(char *iod_line);
struct doppler decode_doppler_observation(char *line);
int fgetline(FILE *file,char *s,int lim);
double modulo(double x,double y);
double gmst(double mjd);
double dgmst(double mjd);
double date2mjd(int year,int month,double day);
double mjd2doy(double mjd,int *yr);
void mjd2date(double mjd,int *year,int *month,double *day);
void obspos_xyz(double mjd,double lng,double lat,float alt,xyz_t *pos,xyz_t *vel);
void precess(double mjd0,double ra0,double de0,double mjd,double *ra,double *de);
void forward(double ra0,double de0,double ra,double de,double *x,double *y);
struct data read_data(char *iodfname,char *dopfname);
void versafit(int m,int n,double *a,double *da,double (*func)(double *),double dchisq,double tol,char *opt);
double chisq(double a[]);
orbit_t read_tle(char *filename,int satno);
void format_tle(orbit_t orb,char *line1,char *line2);
void highlight(float x0,float y0,float x,float y,int flag);
void time_range(double *mjdmin,double *mjdmax,int flag);
void print_tle(orbit_t orb,char *filename);
void fit(orbit_t orb,int *ia);
void usage();
double nfd2mjd(char *date);
double dot(xyz_t a,xyz_t b);
double magnitude(xyz_t a);
xyz_t cross(xyz_t a,xyz_t b);
orbit_t classel(int ep_year,double ep_day,xyz_t r,xyz_t v);
orbit_t rv2el(int satno,double mjd,xyz_t r0,xyz_t v0);


// Propagate elements
void propagate(double mjd)
{
  int imode;
  xyz_t r,v;
  char desig[20],line1[80],line2[80];

  // Store designation
  strcpy(desig,orb.desig);

  // Propagate
  imode=init_sgdp4(&orb);
  imode=satpos_xyz(mjd+2400000.5,&r,&v);
  
  // Convert
  orb=rv2el(orb.satno,mjd,r,v);

  // Copy designation
  strcpy(orb.desig,desig);

  return;
}

xyz_t get_position(double r0,int i0)
{
  int i;
  double rr,drr,r;
  xyz_t pos;
  double x,y,z;

  // Initial range
  rr=100.0;
  do {
    x=d.p[i0].obspos.x+rr*cos(d.p[i0].ra*D2R)*cos(d.p[i0].de*D2R);
    y=d.p[i0].obspos.y+rr*sin(d.p[i0].ra*D2R)*cos(d.p[i0].de*D2R);
    z=d.p[i0].obspos.z+rr*sin(d.p[i0].de*D2R);
    r=sqrt(x*x+y*y+z*z);
    drr=r-r0;
    rr-=drr;
  } while (fabs(drr)>0.01);
  
  pos.x=x;
  pos.y=y;
  pos.z=z;

  return pos;
}

void period_search(void)
{
  int i,j,i1,i2;
  float dt;
  int nrev,nrevmin,nrevmax;
  char line1[70],line2[70];
  int ia[7];

  // Set fitting parameters
  for (i=0;i<6;i++)
    ia[i]=1;
  ia[6]=0;

  // Select all points
  for (i=0;i<d.n;i++)
    d.p[i].flag=2;
  

  // Print observations
  printf("Present observations:\n");
  for (i=0;i<d.n;i++) 
    printf("%3d: %s\n",i+1,d.p[i].iod_line);
  printf("\nSelect center observations of both arcs: ");
  scanf("%d %d",&i1,&i2);
  dt=d.p[i2].mjd-d.p[i1].mjd;
  printf("\nTime passed: %f days\n",dt);
  printf("Provide revolution range to search over [min,max]: ");
  scanf("%d %d",&nrevmin,&nrevmax);

  for (nrev=nrevmin;nrev<nrevmax+1;nrev++) {
    orb.satno=79000+nrev;
    orb.rev=nrev/dt;
    
    // Set parameters
    for (i=0;i<7;i++) 
      ia[i]=0;
	
    // Loop over parameters
    for (i=0;i<6;i++) {
      if (i==0) ia[4]=1;
      if (i==1) ia[1]=1;
      if (i==2) ia[0]=1;
      if (i==3) ia[3]=1;
      if (i==4) ia[2]=1;
      if (i==5) ia[5]=1;

      for (j=0;j<5;j++)
	fit(orb,ia);
    }

    format_tle(orb,line1,line2);
    printf("%s\n%s\n# %d revs, %f revs/day, %f\n",line1,line2,nrev,nrev/dt,d.rms);
  }


  return;
}

int ewsearch(void)
{
  int i,satno=88300;
  double mjdmin,mjdmax;
  int ia[7]={0,0,0,0,0,0,0};
  double ecc,eccmin,eccmax,decc;
  double rev,revmin,revmax,drev;
  double argp,argpmin,argpmax,dargp;
  char line1[70],line2[70];
  FILE *file,*tlefile;
  int year,month;
  double day;

  // Provide 
  printf("Eccentricity [min, max, stepsize]: \n");
  scanf("%lf %lf %lf",&eccmin,&eccmax,&decc);
  printf("Argument of perigee [min, max, stepsize]: \n");
  scanf("%lf %lf %lf",&argpmin,&argpmax,&dargp);

  // Step 2: get time range
  time_range(&mjdmin,&mjdmax,2);

  // Open files
  file=fopen("search.dat","w");
  tlefile=fopen("search.tle","w");

  // Step 4: Loop over eccentricity
  for (ecc=eccmin;ecc<eccmax;ecc+=decc) {
    for (argp=argpmin;argp<argpmax;argp+=dargp) {
      orb.satno=satno++;
      orb.ecc=ecc;
      orb.argp=argp*D2R;
    
      // Set parameters
      for (i=0;i<7;i++) 
	ia[i]=0;
	
      // Step 4: loop over parameters
      for (i=0;i<4;i++) {
	if (i==0) ia[4]=1;
	if (i==1) ia[1]=1;
	if (i==2) ia[0]=1;
	if (i==3) ia[5]=1;
	//	if (i==4) ia[3]=1;
	
	// Do fit
	fit(orb,ia);
      }
      fit(orb,ia);
      fit(orb,ia);
      fit(orb,ia);
      printf("%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);
      fprintf(file,"%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);
      
      format_tle(orb,line1,line2);
      fprintf(tlefile,"OBJ\n%s\n%s\n",line1,line2);
      mjd2date(mjdmin,&year,&month,&day);
      fprintf(tlefile,"# %4d%02d%05.2lf-",year,month,day);
      mjd2date(mjdmax,&year,&month,&day);
      fprintf(tlefile,"%4d%02d%05.2lf, %d measurements, %.5lf deg rms\n",year,month,day,d.n,d.rms);
    }
    fprintf(file,"\n");
  }
  fclose(file);
  fclose(tlefile);

  return orb.satno;
}


int revsearch(void)
{
  int i,satno=88300;
  double mjdmin,mjdmax;
  int ia[7]={0,0,0,0,0,0,0};
  double ecc,eccmin,eccmax,decc;
  double rev,revmin,revmax,drev;
  double argp,argpmin,argpmax,dargp;
  char line1[70],line2[70];
  FILE *file,*tlefile;
  int year,month;
  double day;

  // Provide 
  printf("Mean motion [min, max, stepsize]: \n");
  scanf("%lf %lf %lf",&revmin,&revmax,&drev);

  // Step 1: select all points
  //  for (i=0;i<d.n;i++)
  //    d.p[i].flag=2;

  // Step 2: get time range
  time_range(&mjdmin,&mjdmax,2);

  // Open files
  file=fopen("search.dat","w");
  tlefile=fopen("search.tle","w");

  // Step 4: Loop over eccentricity
  for (rev=revmin;rev<revmax;rev+=drev) {
    orb.satno=satno++;
    orb.rev=rev;
    
    // Set parameters
    for (i=0;i<7;i++) 
      ia[i]=0;
	
    // Step 4: loop over parameters
    for (i=0;i<5;i++) {
      if (i==0) ia[4]=1;
      if (i==1) ia[1]=1;
      if (i==2) ia[0]=1;
      if (i==3) ia[2]=1;
      if (i==4) ia[3]=1;
      
      // Do fit
      fit(orb,ia);
    }
    fit(orb,ia);
    fit(orb,ia);
    fit(orb,ia);
    printf("%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);
    fprintf(file,"%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);

    format_tle(orb,line1,line2);
    fprintf(tlefile,"OBJ\n%s\n%s\n",line1,line2);
    mjd2date(mjdmin,&year,&month,&day);
    fprintf(tlefile,"# %4d%02d%05.2lf-",year,month,day);
    mjd2date(mjdmax,&year,&month,&day);
    fprintf(tlefile,"%4d%02d%05.2lf, %d measurements, %.5lf deg rms\n",year,month,day,d.n,d.rms);
  }
  fclose(file);
  fclose(tlefile);

  return orb.satno;
}

int psearch(void)
{
  int i,satno=99300;
  double mjdmin,mjdmax;
  int ia[7]={0,0,0,0,0,0,0};
  double ecc,eccmin,eccmax,decc;
  double rev,revmin,revmax,drev;
  double argp,argpmin,argpmax,dargp;
  char line1[70],line2[70];
  FILE *file;

  // Provide 
  printf("Mean motion [min, max, stepsize]: \n");
  scanf("%lf %lf %lf",&revmin,&revmax,&drev);
  printf("Eccentricity [min, max, stepsize]: \n");
  scanf("%lf %lf %lf",&eccmin,&eccmax,&decc);
  //  printf("Argument of perigee [min, max, stepsize]: \n");
  //  scanf("%lf %lf %lf",&argpmin,&argpmax,&dargp);


  // Step 1: select all points
  //  for (i=0;i<d.n;i++)
  //    d.p[i].flag=2;

  // Step 2: get time range
  time_range(&mjdmin,&mjdmax,2);

  file=fopen("search.dat","w");

  // Step 4: Loop over eccentricity
  for (rev=revmin;rev<revmax;rev+=drev) {
    for (ecc=eccmin;ecc<eccmax;ecc+=decc) {
      //      for (argp=argpmin;argp<argpmax;argp+=dargp) {
	orb.satno=satno;
	orb.ecc=ecc;
	orb.rev=rev;
	//orb.argp=argp*D2R;
    
	// Set parameters
	for (i=0;i<7;i++) 
	  ia[i]=0;
	
	// Step 4: loop over parameters
	for (i=0;i<5;i++) {
	  if (i==0) ia[4]=1;
	  if (i==1) ia[1]=1;
	  if (i==2) ia[0]=1;
	  //	if (i==3) ia[5]=1;
	  if (i==4) ia[3]=1;
	  
	  // Do fit
	  fit(orb,ia);
	}
	fit(orb,ia);
	fit(orb,ia);
	fit(orb,ia);
	printf("%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);
	fprintf(file,"%8.5lf %8.6lf %8.3lf %8.3lf %8.3lf %8.3lf %8.5lf\n",orb.rev,orb.ecc,orb.argp*R2D,orb.ascn*R2D,orb.mnan*R2D,orb.eqinc*R2D,d.rms);
      }
      fprintf(file,"\n");
      //    }
  }
  fclose(file);

  return orb.satno;
}


int circular_fit(void)
{
  int i;
  double mjdmin,mjdmax;
  int ia[7]={0,0,0,0,0,0,0};

  // Step 1: select all points
  //  for (i=0;i<d.n;i++)
  //    d.p[i].flag=2;

  // Step 2: get time range
  time_range(&mjdmin,&mjdmax,2);

  // Step 3: set initial orbit
  orb.satno=d.p[0].satno;
  orb.eqinc=0.5*M_PI;
  orb.ascn=0.0;
  orb.ecc=0.0001;
  orb.argp=0.0;
  orb.mnan=0.0;
  orb.rev=14.0;
  orb.bstar=0.5e-4;
  orb.ep_day=mjd2doy(0.5*(mjdmin+mjdmax),&orb.ep_year);

  // Step 4: loop over parameters
  for (i=0;i<4;i++) {
    if (i==0) ia[4]=1;
    if (i==1) ia[1]=1;
    if (i==2) ia[0]=1;
    if (i==3) ia[5]=1;

    // Do fit
    fit(orb,ia);
  }
  fit(orb,ia);

  return orb.satno;
}

int adjust_fit(int depth)
{
  int i;
  double mjdmin,mjdmax;
  int ia[6]={0,0,0,0,0,0};

  // Step 2: loop over parameters
  for (i=0;i<depth;i++) {
    if (i==0) ia[4]=1;
    if (i==1) ia[1]=1;
    if (i==2) ia[0]=1;
    if (i==3) ia[5]=1;
    if (i==4) ia[2]=1;
    if (i==5) ia[3]=1;

    // Do fit
    fit(orb,ia);
  }
  fit(orb,ia);

  return orb.satno;
}

void old_circular_fit(void)
{
  int i,j,i0,i1;
  float r0=6500;
  xyz_t pos0,pos1;
  double ang,dt,w,w0;

  // Get end points
  for (i=0,j=0;i<d.n;i++) {
    if (d.p[i].flag==2) {
      if (j==0)
	i0=i;
      i1=i;
      j++;
    }
  }

  // Time difference
  dt=86400.0*(d.p[i1].mjd-d.p[i0].mjd);
  i=0;
  do {
    w0=360.0/(2.0*M_PI*sqrt(r0*r0*r0/398600));
    // Get positions
    pos0=get_position(r0,i0);
    pos1=get_position(r0,i1);
    
    // Compute angle
    ang=acos((pos0.x*pos1.x+pos0.y*pos1.y+pos0.z*pos1.z)/(r0*r0))*R2D;
    
    // Angular motion (deg/sec);
    w=ang/dt;

    r0+=1000.0*(w0-w);
    i++;
  } while (fabs(w0-w)>1e-5 && i<1000);
  printf("%f\n",r0);
  return;
}

int main(int argc,char *argv[])
{
  int i,j,nobs=0;
  int redraw=1,plot_residuals=0,adjust=0,quit=0,identify=0;
  int ia[]={0,0,0,0,0,0,0};
  float dx[]={0.1,0.1,0.35,0.35,0.6,0.6,0.85},dy[]={0.0,-0.25,0.0,-0.25,0.0,-0.25,0.0};
  char c;
  int mode=0,posn=0,click=0;
  float x0,y0,x,y;
  float xmin=0.0,xmax=360.0,ymin=-90.0,ymax=90.0;
  char string[64],bstar[10]=" 50000-4",line0[72],line1[72],line2[72],text[10];
  char filename[64],nfd[32];
  int satno=-1;
  double mjdmin,mjdmax,mjd=0.0;
  int length=864000;
  int arg=0,elset=0,circular=0,tleout=0,noplot=0;
  char *ioddatafile=NULL,*dopdatafile=NULL,*catalog,tlefile[LIM];
  orbit_t orb0;
  FILE *file;

  // Decode options
  while ((arg=getopt(argc,argv,"d:r:c:i:haCo:pIt:l:m:"))!=-1) {
    switch(arg) {

    case 'd':
      ioddatafile=optarg;
      break;

    case 'r':
      dopdatafile=optarg;
      break;

    case 'c':
      catalog=optarg;
      break;

    case 'i':
      satno=atoi(optarg);
      break;

    case 't':
      strcpy(nfd,optarg);
      mjd=nfd2mjd(nfd);
      break;

    case 'm':
      mjd=(double) atof(optarg);
      break;

    case 'l':
      length=atoi(optarg);
      if (strchr(optarg,'h')!=NULL)
	length*=3600;
      else if (strchr(optarg,'m')!=NULL)
	length*=60;
      else if (strchr(optarg,'d')!=NULL)
	length*=86400;
      break;

    case 'C':
      circular=1;
      break;

    case 'p':
      noplot=1;
      break;

    case 'o':
      tleout=1;
      strcpy(tlefile,optarg);
      break;

    case 'h':
      usage();
      return 0;
      break;

    case 'a':
      adjust=1;
      break;

    case 'I':
      identify=1;
      break;

    default:
      usage();
      return 0;
    }
  }

  // Read data
  d=read_data(ioddatafile,dopdatafile);

  // Select observations based on time
  if (mjd>0.0) {
    mjdmin=mjd-length/86400.0;
    mjdmax=mjd;
    for (i=0;i<d.n;i++) {
      if (d.p[i].mjd>mjdmin && d.p[i].mjd<=mjdmax && d.p[i].flag==1)
	d.p[i].flag=2;
      else
	d.p[i].flag=0;
    }
  }
  time_range(&mjdmin,&mjdmax,1);

  // Read TLE
  if (satno>=0) 
    orb=read_tle(catalog,satno);

  // Open catalog
  if (identify==1) {
    file=fopen(catalog,"r");
    if (file==NULL) 
      fatal_error("Failed to open %s\n",catalog);
  }

  // Reloop stderr
  freopen("/tmp/stderr.txt","w",stderr);


  // Fit circular orbit
  if (circular==1) {
    for (i=0;i<d.n;i++)
      d.p[i].flag=2;
    satno=circular_fit();
    plot_residuals=1;
    quit=1;

    // Dump tle
    if (tleout==1) 
      print_tle(orb,tlefile);
  }

  // Identify
  if (identify==1) {
    // Select all points
    for (i=0;i<d.n;i++)
      d.p[i].flag=2;

    // Reset satno
    if (satno==-1)
      satno=0;

    // Loop over file
    while (read_twoline(file,satno,&orb)==0) {
      orb0=orb;
      adjust_fit(2);
      fit(orb,ia);
      printf("%05d %8.3f %8.3f %8.3f %s %8.3f\n",orb.satno,DEG(orb.mnan-orb0.mnan),DEG(orb.ascn-orb0.ascn),d.rms,ioddatafile,mjdmin-(SGDP4_jd0-2400000.5));
    }
    fclose(file);
    
    plot_residuals=1;
    redraw=1;
    quit=1;
    noplot=1;
  }

  // Adjust
  if (adjust==1) {
    // Count observations
    for (i=0,nobs=0;i<d.n;i++) {
      if (d.p[i].flag==1) {
	d.p[i].flag=2;
	nobs++;
      }
    }

    // Not enough observations
    if (nobs<10)
      return 0;

    // Get last point
    for (i=0,j=0;i<d.n;i++) {
      if (d.p[i].flag==2) {
	if (j==0) mjdmax=d.p[i].mjd;
	if (d.p[i].mjd>mjdmax) mjdmax=d.p[i].mjd;
	j++;
      }
    }

    // Propagate orbit to time of last observation
    propagate(mjdmax);
    orb0=orb;

    // Fit
    adjust_fit(16);

    // Print results
    //    printf("%05d %8.3f %8.3f %8.3f %s %8.3f %d\n",satno,DEG(orb.mnan-orb0.mnan),DEG(orb.ascn-orb0.ascn),d.rms,ioddatafile,mjdmin-(SGDP4_jd0-2400000.5),d.nsel);

    // Dump tle
    if (tleout==1) 
      print_tle(orb,tlefile);

    // Set thingies
    plot_residuals=1;
    redraw=1;
    quit=1;
  }

  // Exit before plotting
  if (quit==1 && noplot==1) {
    if (d.n) free(d.p);
    if (d.m) free(d.q);
    fclose(stderr);
    return 0;
  }

  cpgopen("/xs");
  cpgask(0);

  // For ever loop
  for (;;) {
    if (redraw==1) {
      cpgpage();
      cpgsvp(0.1,0.95,0.0,0.18);
      cpgswin(0.0,1.0,-0.5,0.5);

      // Buttons
      cpgtext(0.12,-0.05,"Inclination");
      cpgtext(0.372,-0.05,"Eccentricity");
      cpgtext(0.62,-0.05,"Mean Anomaly");
      cpgtext(0.87,-0.05,"B\\u*\\d");
      cpgtext(0.12,-0.3,"Ascending Node");
      cpgtext(0.37,-0.3,"Arg. of Perigee");
      cpgtext(0.62,-0.3,"Mean Motion");

      // Toggles
      for (i=0;i<7;i++) {
	cpgpt1(dx[i],dy[i],19);
	if (ia[i]==1) {
	  cpgsci(2);
	  cpgpt1(dx[i],dy[i],16);
	  cpgsci(1);
	}
      }

      // Plot map
      cpgsvp(0.1,0.9,0.2,0.9);
      cpgswin(xmax,xmin,ymin,ymax);
      cpgbox("BCTSN",0.,0,"BCTSN",0.,0);
      cpglab("Right Ascension","Declination"," ");

      if (satno>0) {
	// Plot tle
	format_tle(orb,line1,line2);
	cpgmtxt("T",2.0,0.0,0.0,line1);
	cpgmtxt("T",1.0,0.0,0.0,line2);
      }

      // Plot points
      for (i=0;i<d.n;i++) {
	if (d.p[i].flag>=1) {
	  cpgpt1(d.p[i].ra,d.p[i].de,17);
	  sprintf(text," %d",i+1);
	  cpgtext(d.p[i].ra,d.p[i].de,text);
	  if (plot_residuals==1) {
	    cpgmove(d.p[i].ra,d.p[i].de);
	    cpgdraw(d.p[i].rac,d.p[i].dec);
	  }
	  if (d.p[i].flag==2) {
	    cpgsci(2);
	    cpgpt1(d.p[i].ra,d.p[i].de,4);
	    cpgsci(1);
	  }
	}
      }
    }

    // Quit
    if (quit==1)
      break;

    // Get cursor
    cpgband(mode,posn,x0,y0,&x,&y,&c);

    // Help
    if (c=='h' || c=='?') {
      printf("q   Quit\nw   Write TLE file\nP   Search mean motion space\nf   Fit orbit\ns   Select observations in current window\nu   Unselect observations\n1-7 Toggle parameter\nC   Fit circular orbit\nS   Search mean motion/eccentricity space\nc   Change a parameter\nz   Zoom\nr   Unzoom\nt   Toggle starting orbits (LEO, GTO, GEO, HEO)\n");
    }

    // Quit
    if (c=='q' || c=='Q')
      break;
   
    // Period search
    if (c=='P') {
      period_search();
    }

    // Fit
    if (c=='f') {
      // Count points
      for (i=0,nobs=0;i<d.n;i++)
	if (d.p[i].flag==2)
	  nobs++;
      if (satno<0) {
	printf("No elements loaded!\n");
      } else if (nobs==0) {
	printf("No points selected!\n");
      } else {
	fit(orb,ia);
	printf("%d %.5f\n",nobs,d.rms);
	plot_residuals=1;
	redraw=1;
	continue;
      }
    }

    // Write TLE
    if (c=='w') {
      printf("TLE filename to write: ");
      scanf("%s",filename);
      print_tle(orb,filename);
      printf("\n================================================================================\n");
      continue;
    }

    // Highlight
    if (c=='s') {
      highlight(xmin,ymin,xmax,ymax,2);
      time_range(&mjdmin,&mjdmax,2);
      for (i=0,nobs=0;i<d.n;i++)
	if (d.p[i].flag==2)
	  nobs++;
      click=0;
      mode=0;
      redraw=1;
      continue;
    }

    // Unselect
    if (c=='U') {
      for (i=0;i<d.n;i++)
	d.p[i].flag=1;
      time_range(&mjdmin,&mjdmax,1);
      redraw=1;
      continue;
    }

    // Unselect
    if (c=='u') {
      for (i=0;i<d.n;i++) 
	if (d.p[i].flag==2)
	  d.p[i].flag=1;
      redraw=1;
      continue;
    }

    // Toggles
    if (isdigit(c) && c-'0'>=1 && c-'0'<8) {
      if (ia[c-49]==0) 
	ia[c-49]=1;
      else if (ia[c-49]==1) 
	ia[c-49]=0;
      redraw=1;
      continue;
    }

    // Circular fit
    if (c=='C') {
      satno=circular_fit();
      plot_residuals=1;
      printf("%.3f\n",d.rms);
      ia[0]=ia[1]=ia[4]=ia[5]=1;
      redraw=1;
    }

    // Search
    if (c=='S') {
      satno=psearch();
      plot_residuals=1;
      ia[0]=ia[1]=ia[4]=ia[5]=1;
      redraw=1;
    }

    // Search
    if (c=='R') {
      satno=revsearch();
      plot_residuals=1;
      //      ia[0]=ia[1]=ia[4]=ia[5]=ia[2]=1;
      redraw=1;
    }

    // EW search
    if (c=='e') {
      satno=ewsearch();
      plot_residuals=1;
      redraw=1;
    }

    // Change
    if (c=='c') {
      printf("(1) Inclination,     (2) Ascending Node,   (3) Eccentricity,\n(4) Arg. of Perigee, (5) Mean Anomaly,     (6) Mean Motion,\n(7) B* drag,         (8) Epoch,            (9) Satellite ID\n(0) Sat ID\nWhich parameter to change: ");
      scanf("%i",&i);
      if (i>=0 && i<=9) {
	printf("\nNew value: ");
	fgets(string,64,stdin);
	scanf("%s",string);
	if (i==0) strcpy(orb.desig,string);
	if (i==1) orb.eqinc=RAD(atof(string));
	if (i==2) orb.ascn=RAD(atof(string));
	if (i==3) orb.ecc=atof(string);
	if (i==4) orb.argp=RAD(atof(string));
	if (i==5) orb.mnan=RAD(atof(string));
	if (i==6) orb.rev=atof(string);
	if (i==7) orb.bstar=atof(string);
	if (i==8) {
	  orb.ep_year=2000+(int) floor(atof(string)/1000.0);
	  orb.ep_day=atof(string)-1000*floor(atof(string)/1000.0);
	}
	if (i==9) orb.satno=atoi(string);
	redraw=1;
	continue;
      }
      printf("\n================================================================================\n");
    }

    // Zoom
    if (c=='z') {
      click=1;
      mode=2;
    }

    // Execute zoom, or box delete
    if (c=='A') {
      if (click==0) {
	click=1;
      } else if (click==1 && mode==2) {
	xmin=(x0<x) ? x0 : x;
	xmax=(x0>x) ? x0 : x;
	ymin=(y0<y) ? y0 : y;
	ymax=(y0>y) ? y0 : y;

	click=0;
	mode=0;
	redraw=1;
	continue;
      } else {
	click=0;
	mode=0;
	redraw=1;
	continue;
      }
    }

    // Unzoom
    if (c=='r') {
      xmin=0.0;
      xmax=360.0;
      ymin=-90.0;
      ymax=90.0;
      mode=0;
      click=0;
      redraw=1;
      continue;
    }

    // Default tle
    if (c=='t') {
      orb.satno=d.p[0].satno;
      strcpy(orb.desig,d.p[0].desig);
      orb.ep_day=mjd2doy(0.5*(mjdmin+mjdmax),&orb.ep_year);
      satno=orb.satno;
      if (elset==0) {
	orb.eqinc=0.5*M_PI;
	orb.ascn=0.0;
	orb.ecc=0.0001;
	orb.argp=0.0;
	orb.mnan=0.0;
	orb.rev=14.0;
	orb.bstar=0.5e-4;
	printf("LEO orbit\n");
      } else if (elset==1) {
	orb.eqinc=20.0*D2R;
	orb.ascn=0.0;
	orb.ecc=0.7;
	orb.argp=0.0;
	orb.mnan=0.0;
	orb.rev=2.25;
	orb.bstar=0.0;
	printf("GTO orbit\n");
      } else if (elset==2) {
	orb.eqinc=10.0*D2R;
	orb.ascn=0.0;
	orb.ecc=0.0001;
	orb.argp=0.0;
	orb.mnan=0.0;
	orb.rev=1.0027;
	orb.bstar=0.0;
	printf("GSO orbit\n");
      } else if (elset==3) {
	orb.eqinc=63.434*D2R;
	orb.ascn=0.0;
	orb.ecc=0.71;
	orb.argp=270*D2R;
	orb.mnan=0.0;
	orb.rev=2.006;
	orb.bstar=0.0;
	printf("HEO orbit\n");
      }
      elset++;
      if (elset>3)
	elset=0;
      print_orb(&orb);
      printf("\n================================================================================\n");
      click=0;
      redraw=1;
      continue;
    }

    // Save
    x0=x;
    y0=y;
  }

  cpgend();

  if (d.n) free(d.p);
  if (d.m) free(d.q);

  fclose(stderr);
  return 0;
}

// Get observing site
struct site get_site(int site_id)
{
  int i=0;
  char line[LIM];
  FILE *file;
  int id;
  double lat,lng;
  float alt;
  char abbrev[3],observer[64];
  struct site s;
  char *env,filename[LIM];

  env=getenv("ST_DATADIR");
  sprintf(filename,"%s/data/sites.txt",env);

  file=fopen(filename,"r");
  if (file==NULL) {
    printf("File with site information not found!\n");
    return s;
  }
  while (fgets(line,LIM,file)!=NULL) {
    // Skip
    if (strstr(line,"#")!=NULL)
      continue;

    // Strip newline
    line[strlen(line)-1]='\0';

    // Read data
    sscanf(line,"%4d %2s %lf %lf %f",
	   &id,abbrev,&lat,&lng,&alt);
    strcpy(observer,line+38);

    // Change to km
    alt/=1000.0;
    
    // Copy site
    if (id==site_id) {
      s.lat=lat;
      s.lng=lng;
      s.alt=alt;
      s.id=id;
      strcpy(s.observer,observer);
    }

  }
  fclose(file);

  if (id!=site_id)
    s.id==-1;
  
  return s;
}

// Return x modulo y [0,y)
double modulo(double x,double y)
{
  x=fmod(x,y);
  if (x<0.0) x+=y;

  return x;
}

// Greenwich Mean Sidereal Time
double gmst(double mjd)
{
  double t,gmst;

  t=(mjd-51544.5)/36525.0;

  gmst=modulo(280.46061837+360.98564736629*(mjd-51544.5)+t*t*(0.000387933-t/38710000),360.0);

  return gmst;
}

// Greenwich Mean Sidereal Time
double dgmst(double mjd)
{
  double t,dgmst;

  t=(mjd-51544.5)/36525.0;

  dgmst=360.98564736629+t*(0.000387933-t/38710000);

  return dgmst;
}

// Observer position
void obspos_xyz(double mjd,double lng,double lat,float alt,xyz_t *pos,xyz_t *vel)
{
  double ff,gc,gs,theta,s,dtheta;

  s=sin(lat*D2R);
  ff=sqrt(1.0-FLAT*(2.0-FLAT)*s*s);
  gc=1.0/ff+alt/XKMPER;
  gs=(1.0-FLAT)*(1.0-FLAT)/ff+alt/XKMPER;

  theta=gmst(mjd)+lng;
  dtheta=dgmst(mjd)*D2R/86400;

  pos->x=gc*cos(lat*D2R)*cos(theta*D2R)*XKMPER;
  pos->y=gc*cos(lat*D2R)*sin(theta*D2R)*XKMPER; 
  pos->z=gs*sin(lat*D2R)*XKMPER;
  vel->x=-gc*cos(lat*D2R)*sin(theta*D2R)*XKMPER*dtheta;
  vel->y=gc*cos(lat*D2R)*cos(theta*D2R)*XKMPER*dtheta; 
  vel->z=0.0;

  return;
}

// Precess a celestial position
void precess(double mjd0,double ra0,double de0,double mjd,double *ra,double *de)
{
  double t0,t;
  double zeta,z,theta;
  double a,b,c;

  // Angles in radians
  ra0*=D2R;
  de0*=D2R;

  // Time in centuries
  t0=(mjd0-51544.5)/36525.0;
  t=(mjd-mjd0)/36525.0;

  // Precession angles
  zeta=(2306.2181+1.39656*t0-0.000139*t0*t0)*t;
  zeta+=(0.30188-0.000344*t0)*t*t+0.017998*t*t*t;
  zeta*=D2R/3600.0;
  z=(2306.2181+1.39656*t0-0.000139*t0*t0)*t;
  z+=(1.09468+0.000066*t0)*t*t+0.018203*t*t*t;
  z*=D2R/3600.0;
  theta=(2004.3109-0.85330*t0-0.000217*t0*t0)*t;
  theta+=-(0.42665+0.000217*t0)*t*t-0.041833*t*t*t;
  theta*=D2R/3600.0;
  
  a=cos(de0)*sin(ra0+zeta);
  b=cos(theta)*cos(de0)*cos(ra0+zeta)-sin(theta)*sin(de0);
  c=sin(theta)*cos(de0)*cos(ra0+zeta)+cos(theta)*sin(de0);

  *ra=(atan2(a,b)+z)*R2D;
  *de=asin(c)*R2D;

  if (*ra<360.0)
    *ra+=360.0;
  if (*ra>360.0)
    *ra-=360.0;

  return;
}

// Compute Julian Day from Date
double date2mjd(int year,int month,double day)
{
  int a,b;
  double jd;

  if (month<3) {
    year--;
    month+=12;
  }

  a=floor(year/100.);
  b=2.-a+floor(a/4.);

  if (year<1582) b=0;
  if (year==1582 && month<10) b=0;
  if (year==1582 && month==10 && day<=4) b=0;

  jd=floor(365.25*(year+4716))+floor(30.6001*(month+1))+day+b-1524.5;

  return jd-2400000.5;
}

// Decode IOD Observations
struct point decode_iod_observation(char *iod_line)
{
  int year,month,iday,hour,min;
  int format,epoch,me,xe,sign;
  int site_id;
  double sec,ra,mm,ss,de,dd,ds,day,mjd0;
  char secbuf[6],sn[2],degbuf[3],buf1[3],buf2[6];
  struct point p;
  struct site s;
  xyz_t vel;

  // Strip newline
  iod_line[strlen(iod_line)-1]='\0';

  // Copy full line
  strcpy(p.iod_line,iod_line);

  // Set flag
  p.flag=1;

  // Get SSN
  sscanf(iod_line,"%5d",&p.satno);

  // Get desig
  sscanf(iod_line+6,"%s %s",buf1,buf2);
  sprintf(p.desig,"%s%s",buf1,buf2);

  // Get site
  sscanf(iod_line+16,"%4d",&site_id);
  s=get_site(site_id);

  // Skip if site not found
  if (s.id<0) {
    fprintf(stderr,"Site %d not found!\n",site_id);
    p.flag=0;
  }
  
  // Decode date/time
  sscanf(iod_line+23,"%4d%2d%2d%2d%2d%5s",&year,&month,&iday,&hour,&min,secbuf);
  sec=atof(secbuf);
  sec/=pow(10,strlen(secbuf)-2);
  day=(double) iday+(double) hour/24.0+(double) min/1440.0+(double) sec/86400.0;
  p.mjd=date2mjd(year,month,day);

  // Get uncertainty in time
  sscanf(iod_line+41,"%1d%1d",&me,&xe);
  p.st=(float) me*pow(10,xe-8);

  // Get observer position
  obspos_xyz(p.mjd,s.lng,s.lat,s.alt,&p.obspos,&vel);

  // Skip empty observations
  if (strlen(iod_line)<64 || (iod_line[54]!='+' && iod_line[54]!='-')) 
    p.flag=0;

  // Get format, epoch
  sscanf(iod_line+44,"%1d%1d",&format,&epoch);

  // Read position
  sscanf(iod_line+47,"%2lf%2lf%3lf%1s",&ra,&mm,&ss,sn);
  sscanf(iod_line+55,"%2lf%2lf%2s",&de,&dd,degbuf);
  ds=atof(degbuf);
  if (strlen(degbuf)==1)
    ds*=10;
  sign=(sn[0]=='-') ? -1 : 1;
  sscanf(iod_line+62,"%1d%1d",&me,&xe);
  p.sr=(float) me*pow(10,xe-8);
  
  // Decode position
  switch(format) 
    {
      // Format 1: RA/DEC = HHMMSSs+DDMMSS MX   (MX in seconds of arc)
    case 1 : 
      ra+=mm/60+ss/36000;
      de=sign*(de+dd/60+ds/3600);
      p.sr/=3600.0;
      break;
      // Format 2: RA/DEC = HHMMmmm+DDMMmm MX   (MX in minutes of arc)
    case 2:
      ra+=mm/60+ss/60000;
      de=sign*(de+dd/60+ds/6000);
      p.sr/=60.0;
      break;
      // Format 3: RA/DEC = HHMMmmm+DDdddd MX   (MX in degrees of arc)
    case 3 :
      ra+=mm/60+ss/60000;
      de=sign*(de+dd/100+ds/10000);
      break;
      // Format 7: RA/DEC = HHMMSSs+DDdddd MX   (MX in degrees of arc)
    case 7 :
      ra+=mm/60+ss/36000;
      de=sign*(de+dd/100+ds/10000);
      break;
    default :
      printf("IOD Format not implemented\n");
      p.flag=0;
      break;
  }
  // Convert to degrees
  ra*=15.0;

  // Get precession epoch
  if (epoch==0) {
    p.ra=ra;
    p.de=de;
    return p;
  } else if (epoch==4) {
    mjd0=33281.9235;
  } else if (epoch==5) {
    mjd0=51544.5;
  } else {
    printf("Observing epoch not implemented\n");
    p.flag=0;
  }

  // Precess position
  precess(mjd0,ra,de,p.mjd,&p.ra,&p.de);

  return p;
}

// Decode doppler Observations
struct doppler decode_doppler_observation(char *line)
{
  struct doppler q;
  struct site s;
  float flux;
  int site_id;

  // Read line
  sscanf(line,"%lf %lf %f %d",&q.mjd,&q.freq,&flux,&site_id);

  // Get site
  s=get_site(site_id);

  // Get observer position
  obspos_xyz(q.mjd,s.lng,s.lat,s.alt,&q.obspos,&q.obsvel);

  return q;
}


// Read a line of maximum length int lim from file FILE into string s
int fgetline(FILE *file,char *s,int lim)
{
  int c,i=0;

  while (--lim > 0 && (c=fgetc(file)) != EOF && c != '\n')
    s[i++] = c;
  if (c == '\t')
    c=' ';
  if (c == '\n')
    s[i++] = c;
  s[i] = '\0';
  return i;
}

// Read IOD data
struct data read_data(char *iodfname,char *dopfname)
{
  int i=0;
  char line[LIM];
  FILE *file;
  struct data d;

  d.n=0;
  d.m=0;

  // Read IOD data
  if (iodfname!=NULL) {
    // Open file
    file=fopen(iodfname,"r");
    if (file==NULL) {
      fprintf(stderr,"Failed to open %s\n",iodfname);
      exit(1);
    }
    
    // Count lines
    while (fgetline(file,line,LIM)>0) 
      i++;
    d.n=i;

    // Allocate
    d.p=(struct point *) malloc(sizeof(struct point)*d.n);
    
    // Rewind file
    rewind(file);
    
    // Read data
    i=0;
    while (fgetline(file,line,LIM)>0) 
      d.p[i++]=decode_iod_observation(line);
    
    // Close file
    fclose(file);
  }

  // Read doppler data
  if (dopfname!=NULL) {
    // Open file
    file=fopen(iodfname,"r");
    if (file==NULL) {
      fprintf(stderr,"Failed to open %s\n",dopfname);
      exit(1);
    }
    
    // Count lines
    i=0;
    while (fgetline(file,line,LIM)>0) 
      i++;
    d.m=i;
    
    // Allocate
    d.q=(struct doppler *) malloc(sizeof(struct doppler)*d.m);

    // Rewind file
    rewind(file);
    
    // Read data
    i=0;
    while (fgetline(file,line,LIM)>0) 
      d.q[i++]=decode_doppler_observation(line);

    // Close file
    fclose(file);
  }


  return d;
}

// Chi-squared
double chisq(double a[])
{
  int i,imode,nsel;
  double chisq,rms;
  xyz_t satpos,satvel;
  double dx,dy,dz;
  double r;

  // Construct struct
  // a[0]: inclination
  // a[1]: RA of ascending node
  // a[2]: eccentricity
  // a[3]: argument of periastron
  // a[4]: mean anomaly
  // a[5]: revs per day

  if (a[2]<0.0)
    a[2]=0.0;
  if (a[0]<0.0) {
    a[0]*=-1;
    a[1]+=180.0;
  } else if (a[0]>180.0) {
    a[0]=180.0;
  }
  if (a[5]>20.0)
    a[5]=20.0;
  if (a[5]<0.05)
    a[5]=0.05;
  

  // Set parameters
  orb.eqinc=RAD(a[0]);
  orb.ascn=RAD(modulo(a[1],360.0));
  orb.ecc=a[2];
  orb.argp=RAD(modulo(a[3],360.0));
  orb.mnan=RAD(modulo(a[4],360.0));
  orb.rev=a[5];
  orb.bstar=a[6];

  // Initialize
  imode=init_sgdp4(&orb);
  if (imode==SGDP4_ERROR)
    printf("Error\n");

  // Loop over points
  for (i=0,nsel=0,chisq=0.0,rms=0.0;i<d.n;i++) {
    // Get satellite position
    satpos_xyz(d.p[i].mjd+2400000.5,&satpos,&satvel);
      
    // compute difference vector
    dx=satpos.x-d.p[i].obspos.x;  
    dy=satpos.y-d.p[i].obspos.y;
    dz=satpos.z-d.p[i].obspos.z;
      
    // Celestial position
    r=sqrt(dx*dx+dy*dy+dz*dz);
    d.p[i].rac=modulo(atan2(dy,dx)*R2D,360.0);
    d.p[i].dec=asin(dz/r)*R2D;

    
    // Compute offset
    forward(d.p[i].ra,d.p[i].de,d.p[i].rac,d.p[i].dec,&d.p[i].dx,&d.p[i].dy);
    d.p[i].dr=sqrt(d.p[i].dx*d.p[i].dx+d.p[i].dy*d.p[i].dy);

    if (d.p[i].flag==2) {
      // Compute chi-squared
      chisq+=pow(d.p[i].dr/d.p[i].sr,2);

      // Compute rms
      rms+=pow(d.p[i].dr,2);

      // Count selected points
      nsel++;
    }
  }
  if (nsel>0) 
    rms=sqrt(rms/(float) nsel);
  
  d.chisq=chisq;
  d.rms=rms;
  d.nsel=nsel;

  return chisq;
}

// Read tle
orbit_t read_tle(char *filename,int satno)
{
  int i;
  FILE *file;
  orbit_t orb;

  file=fopen(filename,"r");
  if (file==NULL) 
    fatal_error("Failed to open %s\n",filename);

  // Read TLE
  read_twoline(file,satno,&orb);
  fclose(file);

  return orb;
}

// MJD to DOY
double mjd2doy(double mjd,int *yr)
{
  int year,month,k=2;
  double day,doy;
  
  mjd2date(mjd,&year,&month,&day);

  if (year%4==0 && year%400!=0)
    k=1;

  doy=floor(275.0*month/9.0)-k*floor((month+9.0)/12.0)+day-30;

  *yr=year;

  return doy;
}

// Compute Date from Julian Day
void mjd2date(double mjd,int *year,int *month,double *day)
{
  double f,jd;
  int z,alpha,a,b,c,d,e;

  jd=mjd+2400000.5;
  jd+=0.5;

  z=floor(jd);
  f=fmod(jd,1.);

  if (z<2299161)
    a=z;
  else {
    alpha=floor((z-1867216.25)/36524.25);
    a=z+1+alpha-floor(alpha/4.);
  }
  b=a+1524;
  c=floor((b-122.1)/365.25);
  d=floor(365.25*c);
  e=floor((b-d)/30.6001);

  *day=b-d-floor(30.6001*e)+f;
  if (e<14)
    *month=e-1;
  else
    *month=e-13;

  if (*month>2)
    *year=c-4716;
  else
    *year=c-4715;

  return;
}

// Format TLE
void format_tle(orbit_t orb,char *line1,char *line2)
{
  int i,csum;
  char sbstar[]=" 00000-0",bstar[13];

  // Format Bstar term
  if (fabs(orb.bstar)>1e-9) {
    sprintf(bstar,"%11.4e",10*orb.bstar);
    sbstar[0] = bstar[0];  sbstar[1] = bstar[1];  sbstar[2] = bstar[3];  sbstar[3] = bstar[4];
    sbstar[4] = bstar[5];  sbstar[5] = bstar[6];  sbstar[6] = bstar[8];  sbstar[7] = bstar[10];  sbstar[8] = '\0';
  }
  // Print lines
  sprintf(line1,"1 %05dU %-8s %2d%012.8f  .00000000  00000-0 %8s 0    0",orb.satno,orb.desig,orb.ep_year-2000,orb.ep_day,sbstar);
  sprintf(line2,"2 %05d %8.4f %8.4f %07.0f %8.4f %8.4f %11.8f    0",orb.satno,DEG(orb.eqinc),DEG(orb.ascn),1E7*orb.ecc,DEG(orb.argp),DEG(orb.mnan),orb.rev);

  // Compute checksums
  for (i=0,csum=0;i<strlen(line1);i++) {
    if (isdigit(line1[i]))
      csum+=line1[i]-'0';
    else if (line1[i]=='-')
      csum++;
  }
  sprintf(line1,"%s%d",line1,csum%10);
  for (i=0,csum=0;i<strlen(line2);i++) {
    if (isdigit(line2[i]))
      csum+=line2[i]-'0';
    else if (line2[i]=='-')
      csum++;
  }
  sprintf(line2,"%s%d",line2,csum%10);

  return;
}

// Highlight
void highlight(float x0,float y0,float x,float y,int flag)
{
  int i;
  float xmin,xmax,ymin,ymax;

  xmin=(x0<x) ? x0 : x;
  xmax=(x0>x) ? x0 : x;
  ymin=(y0<y) ? y0 : y;
  ymax=(y0>y) ? y0 : y;
  for (i=0;i<d.n;i++) 
    if (d.p[i].ra>xmin && d.p[i].ra<xmax && d.p[i].de>ymin && d.p[i].de<ymax && d.p[i].flag!=0)
      d.p[i].flag=flag;

  return;
}

// Select time range
void time_range(double *mjdmin,double *mjdmax,int flag)
{
  int i,n;
  float c;

  for (i=0,n=0;i<d.n;i++) {
    if (d.p[i].flag==flag) {
      if (n==0) {
	*mjdmin=d.p[i].mjd;
	*mjdmax=d.p[i].mjd;
      }
      if (d.p[i].mjd< *mjdmin) *mjdmin=d.p[i].mjd;
      if (d.p[i].mjd> *mjdmax) *mjdmax=d.p[i].mjd;
      n++;
    }
  }
  c=0.1*(*mjdmax- *mjdmin);
  *mjdmin-=c;
  *mjdmax+=c;

  return;
}

// Print TLE
void print_tle(orbit_t orb,char *filename)
{
  int i,n;
  FILE *file;
  double mjdmin,mjdmax;
  int year,month;
  double day;
  char line1[70],line2[70];

  // Count number of points
  for (i=0,n=0;i<d.n;i++) {
    if (d.p[i].flag==2) {
      if (n==0) {
	mjdmin=d.p[i].mjd;
	mjdmax=d.p[i].mjd;
      }
      if (d.p[i].mjd<mjdmin) mjdmin=d.p[i].mjd;
      if (d.p[i].mjd>mjdmax) mjdmax=d.p[i].mjd;
      n++;
    }
  }

  // Write TLE
  file=fopen(filename,"a");
  format_tle(orb,line1,line2);
  fprintf(file,"OBJ\n%s\n%s\n",line1,line2);

  mjd2date(mjdmin,&year,&month,&day);
  fprintf(file,"# %4d%02d%05.2lf-",year,month,day);
  mjd2date(mjdmax,&year,&month,&day);
  fprintf(file,"%4d%02d%05.2lf, %d measurements, %.3lf deg rms\n",year,month,day,n,d.rms);
  fclose(file);

  return;
}

// Fit
void fit(orbit_t orb,int *ia)
{
  int i,n;
  double a[7],da[7];
  //  double db[7]={5.0,5.0,0.1,5.0,5.0,0.5,0.0001};
  //  double db[7]={1.0,1.0,0.02,1.0,1.0,0.1,0.0001};
  double db[7]={0.1,0.1,0.002,0.1,0.1,0.01,0.00001};

  a[0]=orb.eqinc*R2D;
  da[0]=da[0]*R2D;
  a[1]=orb.ascn*R2D;
  da[1]=da[1]*R2D;
  a[2]=orb.ecc;
  a[3]=orb.argp*R2D;
  da[3]=da[3]*R2D;
  a[4]=orb.mnan*R2D;
  da[4]=da[4]*R2D;
  a[5]=orb.rev;
  a[6]=orb.bstar;

  for (i=0;i<7;i++) {
    if (ia[i]==1)
      da[i]=db[i];
    else
      da[i]=0.0;
  }

  // Construct struct
  // a[0]: inclination
  // a[1]: RA of ascending node
  // a[2]: eccentricity
  // a[3]: argument of periastron
  // a[4]: mean anomaly
  // a[5]: revs per day
  // a[6]: bstar

  // Count highlighted points
  for (i=0,n=0;i<d.n;i++)
    if (d.p[i].flag==2)
      n++;

  if (n>0)
    versafit(n,7,a,da,chisq,0.0,1e-6,"n");

  // Return parameters
  orb.eqinc=RAD(a[0]);
  orb.ascn=RAD(modulo(a[1],360.0));
  orb.ecc=a[2];
  orb.argp=RAD(modulo(a[3],360.0));
  orb.mnan=RAD(modulo(a[4],360.0));
  orb.rev=a[5];
  orb.bstar=a[6];

  return;
}

void usage()
{
  printf("satfit d:c:i:haCo:p\n\n");
  printf("d    IOD observations\n");
  printf("c    TLE catalog\n");
  printf("i    Satellite ID (NORAD)\n");
  printf("C    Fit circular orbit\n");
  printf("p    No plotting\n");
  printf("o    Output TLE file\n");
  printf("a    Adjust MA and RAAN\n");
  printf("h    This help\n");

  return;
}

// nfd2mjd
double nfd2mjd(char *date)
{
  int year,month,day,hour,min,sec;
  double mjd,dday;

  sscanf(date,"%04d-%02d-%02dT%02d:%02d:%02d",&year,&month,&day,&hour,&min,&sec);
  dday=day+hour/24.0+min/1440.0+sec/86400.0;

  mjd=date2mjd(year,month,dday);

  return mjd;
}

// Dot product
double dot(xyz_t a,xyz_t b)
{
  return a.x*b.x+a.y*b.y+a.z*b.z;
}

// Magnitude
double magnitude(xyz_t a)
{
  return sqrt(dot(a,a));
}

// Cross product
xyz_t cross(xyz_t a,xyz_t b)
{
  xyz_t c;

  c.x=a.y*b.z-a.z*b.y;
  c.y=a.z*b.x-a.x*b.z;
  c.z=a.x*b.y-a.y*b.x;

  return c;
}

// Clasical elements
orbit_t classel(int ep_year,double ep_day,xyz_t r,xyz_t v)
{
  int i;
  double rm,vm,vm2,rvm,mu=1.0;;
  double chi,xp,yp,sx,cx,b,ee;
  double a,ecc,incl,node,peri,mm,n;
  xyz_t h,e,kk,nn;
  orbit_t orb;

  r.x/=XKMPER;
  r.y/=XKMPER;
  r.z/=XKMPER;
  v.x/=(XKE*XKMPER/AE*XMNPDA/86400.0);
  v.y/=(XKE*XKMPER/AE*XMNPDA/86400.0);
  v.z/=(XKE*XKMPER/AE*XMNPDA/86400.0);

  rm=magnitude(r);
  vm2=dot(v,v);
  rvm=dot(r,v);
  h=cross(r,v);
  chi=dot(h,h)/mu;

  e.x=(vm2/mu-1.0/rm)*r.x-rvm/mu*v.x;
  e.y=(vm2/mu-1.0/rm)*r.y-rvm/mu*v.y;
  e.z=(vm2/mu-1.0/rm)*r.z-rvm/mu*v.z;

  a=pow(2.0/rm-vm2/mu,-1);
  ecc=magnitude(e);
  incl=acos(h.z/magnitude(h))*R2D;
  
  kk.x=0.0;
  kk.y=0.0;
  kk.z=1.0;
  nn=cross(kk,h);
  if (nn.x==0.0 && nn.y==0.0)
    nn.x=1.0;
  node=atan2(nn.y,nn.x)*R2D;
  if (node<0.0)
    node+=360.0;

  peri=acos(dot(nn,e)/(magnitude(nn)*ecc))*R2D;
  if (e.z<0.0)
    peri=360.0-peri;
  if (peri<0.0)
    peri+=360.0;

  // Elliptic motion
  if (ecc<1.0) {
    xp=(chi-rm)/ecc;
    yp=rvm/ecc*sqrt(chi/mu);
    b=a*sqrt(1.0-ecc*ecc);
    cx=xp/a+ecc;
    sx=yp/b;
    ee=atan2(sx,cx);
    n=XKE*sqrt(mu/(a*a*a));
    mm=(ee-ecc*sx)*R2D;
  } 
  if (mm<0.0)
    mm+=360.0;

  // Fill
  orb.satno=0;
  orb.eqinc=incl*D2R;
  orb.ascn=node*D2R;
  orb.argp=peri*D2R;
  orb.mnan=mm*D2R;
  orb.ecc=ecc;
  orb.rev=XKE*pow(a,-3.0/2.0)*XMNPDA/(2.0*M_PI);
  orb.bstar=0.0;
  orb.ep_year=ep_year;
  orb.ep_day=ep_day;
  orb.norb=0;

  return orb;
}

// Position and velocity to elements
orbit_t rv2el(int satno,double mjd,xyz_t r0,xyz_t v0)
{
  int i,imode;
  orbit_t orb[5],orb1[5];
  xyz_t r,v;
  kep_t kep;
  char line1[70],line2[70];
  int ep_year;
  double ep_day;

  // Epoch
  ep_day=mjd2doy(mjd,&ep_year);

  // Initial guess
  orb[0]=classel(ep_year,ep_day,r0,v0);
  orb[0].satno=satno;
  
  for (i=0;i<4;i++) {
    // Propagate
    imode=init_sgdp4(&orb[i]);
    imode=satpos_xyz(mjd+2400000.5,&r,&v);

    // Compute initial orbital elements
    orb1[i]=classel(ep_year,ep_day,r,v);

    // Adjust
    orb[i+1].rev=orb[i].rev+orb[0].rev-orb1[i].rev;
    orb[i+1].ascn=orb[i].ascn+orb[0].ascn-orb1[i].ascn;
    orb[i+1].argp=orb[i].argp+orb[0].argp-orb1[i].argp;
    orb[i+1].mnan=orb[i].mnan+orb[0].mnan-orb1[i].mnan;
    orb[i+1].eqinc=orb[i].eqinc+orb[0].eqinc-orb1[i].eqinc;
    orb[i+1].ecc=orb[i].ecc+orb[0].ecc-orb1[i].ecc;
    orb[i+1].ep_year=orb[i].ep_year;
    orb[i+1].ep_day=orb[i].ep_day;
    orb[i+1].satno=orb[i].satno;
    orb[i+1].norb=orb[i].norb;
    orb[i+1].bstar=orb[i].bstar;

    // Keep in range
    if (orb[i+1].ecc<0.0)
      orb[i+1].ecc=0.0;
    if (orb[i+1].eqinc<0.0)
      orb[i+1].eqinc=0.0;
  }

  orb[i].mnan=modulo(orb[i].mnan,2.0*M_PI);
  orb[i].ascn=modulo(orb[i].ascn,2.0*M_PI);
  orb[i].argp=modulo(orb[i].argp,2.0*M_PI);

  return orb[i];
}


// Get a x and y from a RA and Decl
void forward(double ra0,double de0,double ra,double de,double *x,double *y)
{
  int i,status;
  double phi,theta;
  struct celprm cel;

  // Initialize Reference Angles
  celini(&cel);
  cel.ref[0]=ra0;
  cel.ref[1]=de0;
  cel.ref[2]=999.;
  cel.ref[3]=999.;
  cel.flag=0.;
  strcpy(cel.prj.code,"STG");

  if (celset(&cel)) {
    printf("Error in Projection (celset)\n");
    return;
  }
  cels2x(&cel,1,0,1,1,&ra,&de,&phi,&theta,x,y,&status);

  return;
}