/* Orbit Determination v1.4 - SSP 2000!
   * Tiffany K. Ng
   * Aug 29, 2000
   */

  #include<stdio.h>
  #include<math.h>

  #define K .01720209895
  #define E 0.4090928023
  #define PI 3.1415926535897932384626

  // declare global variables
  int c;    // counter
  double ra_input[3][3], ra[3], dec_input[3][3], dec[3], t[3], big_R[6][3];   // variables for input
  double phat[3][3], phatdot0[3], phat2dot0[3], big_R_1[3], big_R0[3], big_R1[3], p0_mag, p0dot_mag; // formerly main() local vars made global
  double r0_mag = 2.5, new_r0_mag, big_R0_mag, cproduct[3], cproduct2[3];     // formerly iterate_r0() local vars made global
  double big_R0_dot[3];     // step C
  double r0[3], r0dot[3];      // step D
  double i, Om, w;       // step F
  double i_ec, Om_ec, w_ec; // step G

  // function prototypes
  // double input();                  // request 3 sets of RAs, Decs, Julian dates, and R-vectors from user
  double hms2deg(double ra[3]);    // convert hours, minutes, seconds -> degrees
  double dms2deg(double deg[3]);      // convert degrees, minutes, seconds -> degrees
  double deg2rad(double deg);         // convert degrees -> radians
  double rad2deg(double rad);         // convert radians -> degrees
  void interpolate_R();            // interpolate three R vectors from JPL equatorial coordinates
  void iterate_r0( void );         // iterate to find magnitude of r-nought and rho-nought
  void stepC( void );              // compute magnitude of rho-nought-dot and R-nought-dot
  void stepD( void );              // compute components of r-nought and r-nought-dot
  void f_g( void );                // f & g series
  double f(double tau, double r0mag, double r0dot);
  double g(double tau, double r0);
  void stepF ( void );             // convert rectangular (r-vector, r-dot-vector) -> orbital elements
  void stepG ( void );             // convert Equatorial -> Ecliptic
  double magnitude(double vector[3]); // compute magnitude of a given vector
  double cross_product(double v1[3], double v2[3], double cproduct[3]);// compute cross product of two vectors
  double dot_product(double v1[3], double v2[3]);                      // compute dot product of two vectors
  void pause( void );              // prompt user to continue

  /*************************************** begin main ***************************************/
  int main( void ) {
     double tJD[3];
     tJD[0] = 2451759.80567;          // interpolated Julian dates for 3 observations
     tJD[1] = 2451765.79931;
     tJD[2] = 2451773.78993;

     big_R[0][0] = -.6690210121;      // Aug 3, 7 UT
     big_R[0][1] = .6999716435;
     big_R[0][2] = .3034472269;
     big_R[1][0] = -.6695482029;      // Aug 3, 8 UT
     big_R[1][1] = .6995400413;
     big_R[1][2] = .3032601227;

     big_R[2][0] = -.7413937753;      // Aug 9, 7 UT
     big_R[2][1] = .6343928992;
     big_R[2][2] = .2750151449;
     big_R[3][0] = -.7418705912;      // Aug 9, 8 UT
     big_R[3][1] = .6339141718;
     big_R[3][2] = .2748075722;

     big_R[4][0] = -.8255701095;      // Aug 17, 6 UT
     big_R[4][1] = .5375942601;
     big_R[4][2] = .233043349;
     big_R[5][0] = -.8259730721;      // Aug 17, 7 UT
     big_R[5][1] = .5370605235;
     big_R[5][2] = .2328119313;

     ra_input[0][0] = 21;          // RAs and decs for Juno
     ra_input[0][1] = 23;
     ra_input[0][2] = 7.7155;
     dec_input[0][0] = -3;
     dec_input[0][1] = -24;
     dec_input[0][2] = -31.485;

     ra_input[1][0] = 21;
     ra_input[1][1] = 18;
     ra_input[1][2] = 14.347;
     dec_input[1][0] = -4;
     dec_input[1][1] = -8;
     dec_input[1][2] = -29.02;

     ra_input[2][0] = 21;
     ra_input[2][1] = 11;
     ra_input[2][2] = 32.375;
     dec_input[2][0] = -5;
     dec_input[2][1] = -15;
     dec_input[2][2] = -5.2;

     printf("=*= Orbit Determination =*= Tiffany K. Ng =*= SSP 2000!\n");
     printf("\nJulian days:  t(-1) = %.3lf  t(0) = %.3lf  t(+1) = %.3lf\n", tJD[0], tJD[1], tJD[2]);

     // input();  // request user input

     // convert RA, dec -> degrees -> radians
     printf("\nRadians: ");
     for(c=0; c<3; c++)  {
        ra[c] = hms2deg(ra_input[c]);
        ra[c] = deg2rad(ra[c]);
        dec[c] = dms2deg(dec_input[c]);
        dec[c] = deg2rad(dec[c]);
        printf("RA(%d) = %.10lf  Dec(%d) = %.10lf\n", c-1, ra[c], c-1, dec[c]);
     }

     // convert Julian dates -> Gaussian days
     t[0] = K * (tJD[0] - tJD[1]);
     t[1] = K * (tJD[2] - tJD[0]);
     t[2] = K * (tJD[2] - tJD[1]);
     printf("\nGaussian: tau(-1) = %.10lf  tau(0) = %.10lf  tau(+1) = %.10lf", t[0], t[1], t[2]);


     for(c=0; c<3; c++)  {
        // rho-hat for 3 observations
        phat[c][0] = cos(ra[c])*cos(dec[c]);
        phat[c][1] = sin(ra[c])*cos(dec[c]);
        phat[c][2] = sin(dec[c]);
     }

     for(c=0; c<3; c++)  {
        // rho-hat-dot and rho-hat double-dot
        phatdot0[c] = (pow(t[2],2)*(phat[0][c] - phat[1][c]) - pow(t[0],2)*(phat[2][c] - phat[1][c])) / (t[0]*t[1]*t[2]);
        phat2dot0[c] = -2*(t[2]*(phat[0][c] - phat[1][c]) - t[0]*(phat[2][c] - phat[1][c])) / (t[0]*t[1]*t[2]);
     }
     printf("\n");
     for(c=0; c<3; c++)  {
        printf("phat(%d) = %.10lfi + %.10lfj + %.10lfk\n", c-1, phat[c][0], phat[c][1], phat[c][2]);
     }
     printf("\nphatdot0 = %.10lfi + %.10lfj + %.10lfk\n", phatdot0[0], phatdot0[1], phatdot0[2]);
     printf("phat2dot0 = %.10lfi + %.10lfj + %.10lfk\n", phat2dot0[0], phat2dot0[1], phat2dot0[2]);

     interpolate_R(); // interpolate 3 R-vectors from JPL equatorial coordinates
     iterate_r0();    // iterate to find magnitude of r-nought
     stepC();         // magnitude of rho-nought-dot and R-nought-dot
     pause();         // prompt user to continue
     stepD();         // compute components of r-nought and r-nought-dot
     f_g();           // f & g series
     pause();         // prompt user to continue
     stepF();         // convert rectangular (r-vector, r-dot-vector) -> orbital elements

     return 0;
  }
  /**************************************** end main ****************************************/

  double input()  {
     for(c=0; c<3; c++)  {
        printf("\nEnter RA %d: ", c+1);
        scanf("%lf %lf %lf", &ra_input[c][0], &ra_input[c][1], &ra_input[c][2]);
        printf("\nEnter Dec %d: ", c+1);
        scanf("%lf %lf %lf", &dec_input[c][0], &dec_input[c][1], &dec_input[c][2]);
        printf("\nEnter Julian date %d: ", c+1);
        scanf("%lf", &t[c]);
        printf("\nEnter components of vector R %d: ", c+1);
        scanf("%lf %lf %lf", &big_R[c][0], &big_R[c][1], &big_R[c][2]);
     }
  }

  double hms2deg(double ra_input[3])  {
     return((((ra_input[0] + ra_input[1]/60.0 + ra_input[2]/3600.0)) / 24.0) * 360);
  }

  double dms2deg(double dec_input[3]) {
     return(dec_input[0] + dec_input[1]/60.0 + dec_input[2]/3600.0);
  }

  double deg2rad(double deg){
     return(deg * PI / 180.0);
  }

  void interpolate_R()   {
     double f, min[3];
     min[0] = 121 / 6.0;
     min[1] = 11;
     min[2] = 57.5;

     // interpolate R(-1) vector
     for(c=0; c<3; c++)  {
        f = min[0] / 60.0;
        big_R_1[c] = big_R[0][c] + f*(big_R[1][c] - big_R[0][c]);
     }
     printf("\nR-vector(-1) = %.10lfi + %.10lfj + %.10lfk\n", big_R_1[0], big_R_1[1], big_R_1[2]);

     // interpolate R(0) vector
     for(c=0; c<3; c++)  {
        f = min[1] / 60.0;
        big_R0[c] = big_R[2][c] + f*(big_R[3][c] - big_R[2][c]);
     }
     printf("R-vector(0)  = %.10lfi + %.10lfj + %.10lfk\n", big_R0[0], big_R0[1], big_R0[2]);

     // interpolate R(+1) vector
     for(c=0; c<3; c++)  {
        f = min[2] / 60.0;
        big_R1[c] = big_R[4][c] + f*(big_R[5][c] - big_R[4][c]);
     }
     printf("R-vector(+1) = %.10lfi + %.10lfj + %.10lfk\n", big_R1[0], big_R1[1], big_R1[2]);
  }

  void iterate_r0( void )   {
     double dr;                          // dr = delta r0_mag
     big_R0_mag = magnitude(big_R0);
     cross_product(phat[1], phatdot0, cproduct);      // cross product of rho-hat and rho-hat-dot
     do {
        p0_mag = (1/pow(r0_mag,3) - (1 + 1/328900.5) / pow(big_R0_mag,3))
            * (dot_product(cproduct,big_R0) / dot_product(cproduct,phat2dot0));
        new_r0_mag = sqrt(pow(p0_mag,2) + pow(magnitude(big_R0),2) - 2*p0_mag*dot_product(big_R0,phat[1]));
        dr = new_r0_mag - r0_mag;
        r0_mag = new_r0_mag;
     }  while((dr > .000001) || (dr < -0.000001));
     printf("\n|r(0)| = %.10lf  |p(0)| = %.10lf  ", r0_mag, p0_mag);
  }

  void stepC( void )  {
     cross_product(phat[1], phat2dot0, cproduct2);    // cross product of rho-hat and rho-hat double-dot
     p0dot_mag = -.5 * (1/pow(r0_mag,3) - (1 + 1/328900.5) / pow(big_R0_mag,3))
            * (dot_product(cproduct2,big_R0) / dot_product(cproduct,phat2dot0));
     printf("|p(0)dot| = %.10lf", p0dot_mag);
     for(c=0; c<3; c++)  {
        big_R0_dot[c] = (big_R[3][c] - big_R[2][c]) / (K / 24.0);
     }
     printf("\nR0dot = %.10lfi + %.10lfj + %.10lfk\n", big_R0_dot[0], big_R0_dot[1], big_R0_dot[2]);
  }

  void stepD( void )  {
     for(c=0; c<3; c++)  {
        r0[c] = p0_mag * phat[1][c] - big_R0[c];
        r0dot[c] = p0dot_mag * phat[1][c] + p0_mag * phatdot0[c] - big_R0_dot[c];
     }
     printf("\nr0    = %.10lfi + %.10lfj + %.10lfk", r0[0], r0[1], r0[2]);
     printf("\nr0dot = %.10lfi + %.10lfj + %.10lfk\n", r0dot[0], r0dot[1], r0dot[2]);
  }

  void f_g( void ){
     double avp_1[3], avp1[3], phat_1[3], phat1[3];
     double f_1, f1, g_1, g1, r_1[3], r1[3];    // variables included in output (except r)
     printf("\n\n* f & g series *\n");

     // f and g for first observation
     f_1 = f(t[0], magnitude(r0), dot_product(r0,r0dot));
     g_1 = g(t[0], magnitude(r0));

     // f and g for third observation
     f1 = f(t[2], magnitude(r0), dot_product(r0,r0dot));
     g1 = g(t[2], magnitude(r0));

     // output f and g values
     printf("\nf(-1) = %.10lf   g(-1) = %.10lf", f_1, g_1);
     printf("\nf(+1) = %.10lf   g(+1) = %.10lf", f1, g1);

     for(c=0; c<=2; c++) {
        r_1[c] = f_1 * r0[c] + g_1 * r0dot[c];  // components of vector r for first observation
        r1[c] = f1 * r0[c] + g1 * r0dot[c];     // components of vector r for third observation
     }

     /* output components of vectors r(-1) and r(+1)
     printf("\n\nr(-1): %.10lf + %.10lf + %.10lf", r_1[0], r_1[1], r_1[2]);
     printf("\nr(+1): %.10lf + %.10lf + %.10lf", r1[0], r1[1], r1[2]);
     */

     // approximate rho-vector components
     for(c=0; c<=2; c++) {
        avp_1[c] = r_1[c] + big_R_1[c];  // t(-1)
        avp1[c] = r1[c] + big_R1[c];  // t(+1)
     }

     // approximated rho-hat components
     for(c=0; c<=2; c++) {
        phat_1[c] = avp_1[c] / magnitude(avp_1);
        phat1[c] = avp1[c] / magnitude(avp1);
     }

     // output initial and approximated rho-hat components
     printf("\n\nFirst rho-hat: %.10lfi + %.10lfj + %.10lfk", phat[0][0], phat[0][1], phat[0][2]);
     printf("\nApproximated rho-hat: %.10lfi + %.10lfj + %.10lfk", phat_1[0], phat_1[1], phat_1[2]);
     printf("\n\nThird rho-hat: %.10lfi + %.10lfj + %.10lfk", phat[2][0], phat[2][1], phat[2][2]);
     printf("\nApproximated rho-hat: %.10lfi + %.10lfj + %.10lfk\n", phat1[0], phat1[1], phat1[2]);
  }

  double f(double t, double r0mag, double dot_prod){
     return(1 - pow(t,2) / (2 * pow(r0mag,3)) + pow(t,3) * dot_prod / (2 * pow(r0mag,5)));
  }

  double g(double t, double r0mag) {
     return(t - pow(t,3) / (6 * pow(r0mag, 3)));
  }

  void stepF ( void ) {  // convert Ecliptic rectangular (r-vector, v-vector) -> Ecliptic orbital elements
     double a, e, h[3], hmag, sin_Om, rmag, U, sin_U, gnu, sin_gnu, Ekepler, en, M, M0;
     double t0 = 2451800.5;

     for(c=0; c<3; c++)  {
        r0dot[c] = r0dot[c]*K;
     }

     // compute a
     a = 1 / ( 2 / magnitude(r0) - (dot_product(r0dot, r0dot))/pow(K,2));

     // compute e
     h[0] = r0[1]*r0dot[2] - r0[2]*r0dot[1]; // cross product of r0 and r0dot
     h[1] = -(r0[0]*r0dot[2] - r0[2]*r0dot[0]);
     h[2] = r0[0]*r0dot[1] - r0[1]*r0dot[0];
     hmag = magnitude(h);
     e = sqrt(1 - (pow(hmag, 2) / (pow(K, 2)*a)));

     // compute i
     // printf("\nh = %lf %lf %lf", h[0], h[1], h[2]);
     i = atan( sqrt(pow(h[0],2) + pow(h[1],2)) / h[2] );

     // compute Omega
     Om = acos( -h[1] / (hmag * sin(i)) );
     sin_Om = h[0] / (hmag * sin(i));        // quadrant check
     if(sin_Om < 0)   {  Om = 2*PI - Om; }

     // compute w
     rmag = magnitude(r0);
        // find U
     U = acos( (r0[0]*cos(Om) + r0[1]*sin(Om)) / rmag);
     sin_U = r0[2] / (rmag * sin(i));    // quadrant check
     if(sin_U < 0){   U = 2*PI - U;}
        // find v (gnu)
     gnu = acos((( (a - a*pow(e, 2)) / rmag) - 1) / e);
     sin_gnu = ((a * (1 - pow(e, 2)) / hmag) * (dot_product(r0,r0dot) / rmag)) / e; // quadrant check
     if(sin_gnu < 0)  {  gnu = 2*PI - gnu;   }
        // find w from U and gnu
     w = U - gnu;
     if(w < 0) {  w += 2*PI;   }      // set 0 <= w <= 360

     stepG();     // convert Equatorial -> Ecliptic

     // compute M0
     Ekepler = acos((1 - rmag/a) / e);
     if(gnu > PI) {   Ekepler = 2*PI - Ekepler; }  // quadrant check
     M = Ekepler - e*sin(Ekepler);
     en = K / pow(a, 1.5);
     M0 = en*(t0 - 2451765.79931) + M;                // compute M0 from M (t0 - t[1])
     if(M0 < 0)   {   M0 += 2*PI;  }            // set 0 <= Mo <= 360

     // output orbital elements
     printf("\n\n* Orbital elements for Ecliptic and Equinox 2000 *");
     printf("\na     = %.7lf\ne     = %.7lf\ni     = %.5lf\nNode  = %.5lf\nw     = %.5lf\nMo    = %lf\n",
        a, e, rad2deg(i_ec), rad2deg(Om_ec), rad2deg(w_ec), rad2deg(M0));
     printf("\n* JPL Horizons orbital elements *");
     printf("\na     = 2.6674628\ne     = 0.2585759\ni     = 12.96833\nNode  = 170.16748\nw     = 248.08453\nM     = 298.0211395");
  }

  void stepG( void )  {
     double sin_Om_ec, dw, sin_dw;

     // inclination
     i_ec = acos( cos(E)*cos(i) + sin(E)*sin(i)*cos(Om) );
     if(i_ec > PI){   i_ec = i_ec - PI;  }   // quadrant check

     // uppercase omega
     Om_ec = acos((cos(E)*cos(i_ec) - cos(i)) / (sin(E)*sin(i_ec)));
     sin_Om_ec = sin(Om)*sin(i)/sin(i_ec);      // quadrant check
     if(sin_Om_ec < 0)   {  Om_ec = 2*PI - Om_ec;  }

     // lowercase omega
     dw = acos(cos(Om)*cos(Om_ec) + sin(Om)*sin(Om_ec)*cos(E));
     sin_dw = sin(E)*sin(Om) / sin(i_ec);    // quadrant check
     if(sin_dw < 0)   {  dw = 2*PI - dw;     }
     w_ec = w - dw;
     if(w_ec < 0) {   w_ec = w_ec + 2*PI;}
  }

  double magnitude(double vector[3])  {
     return(sqrt(pow(vector[0], 2) + pow(vector[1], 2) + pow(vector[2], 2)));
  }

  double cross_product(double v1[3], double v2[3], double cproduct[3]) {
     cproduct[0] = v1[1]*v2[2] - v1[2]*v2[1];
     cproduct[1] = -(v1[0]*v2[2] - v1[2]*v2[0]);
     cproduct[2] = v1[0]*v2[1] - v1[1]*v2[0];
  }

  double dot_product(double v1[3], double v2[3]){
     double sum = 0, component;
     for(c = 0; c < 3; c++) {      // component formula loop
        component = v1[c] * v2[c];
        sum += component;
     }
     return (sum);
  }

  double rad2deg(double rad){  return(rad * 180 / PI);    }

  void pause( void )  {
     char dummy;
     printf("\nPress Enter to continue...");
     scanf("%c", &dummy);
  }