//////////////////////////////////////////////////////////////////////////
//
//       (C) Copyright 2003 by GATS, Inc.
//           11864 Canon Blvd., STE 101
//           Newport News, VA 23606     
//
//   All Rights Reserved. No part of this software or publication may be
//   reproduced, stored in a retrieval system, or transmitted, in any form
//   or by any means, electronic, mechanical, photocopying, recording, or
//   otherwise without the prior written permission of GATS, Inc.
// 
///////////////////////////////////////////////////////////////////////////
//
// Class: GeoCal
// 
// Filename: GeoCal.cpp      
//
// Description: GeoCal class has member functions to manipulate calculate 
//              all geolocation parameters used in SABER Level1. 
//
// Author:      Y.Wang
//              (757)873-5920
//              <ywang@gats-inc.com>
//
/////////////////////////////////////////////////////////////////////////////

#include <cmath>
#include <math.h>
#include <iostream>
#include <sstream>
#include <cstdlib>

//#include "SCsabertime.h"
#include "DVector.h"
#include "DMatrix.h"
#include "GeoCal.h"
#include "General_Dimension_Math_Consts.h"

#define SECSperDAY 86400.    //seconds per day
#define HOURSperDAY 24.  
#define DEGREESperHOUR 15.  // 360/24=15
#define SPEED_OF_LIGHT  299792.458  //km/s
//Added by CWBrown for Fukushima Algorithm
#define DELTATCONCRIT   1.0e-15  //Convergence Criteria
#define MAXITS 20 // Maximum Iterations (should take less than 5)
#define CHECKQFUNC(x, a, b, c) (a*x*x*x*x + b*x*x*x + c*x - a >= 0)


using namespace std;

static const double RADperARCsec= 4.848136811095360e-6 ;  /* radiance per arc second*/


////////////////////////////////////////////////////////////////////////
// Class constructor
////////////////////////////////////////////////////////////////////////
GeoCal::GeoCal()
{ 
}

////////////////////////////////////////////////////////////////////////
// Class destructor
////////////////////////////////////////////////////////////////////////
GeoCal::~GeoCal()
{ 
}

//////////////////////////////////////////////////////////////////////////
// Method: cal_SC_los_vector(double scanAng)
// Description
//    Calculate line of sight vector in Spacecraft Coords
// Input:
//   scanAng in radians
// Note the following codes work when scan angle is on YZ plane(XZ plane
// is the orbit plane)
///////////////////////////////////////////////////////////////////////////
DVector GeoCal::cal_SC_los_vector(double scanAng)
{
   //scanAng in radians
   DVector losSC(3);
   double rAngle,cosAng,sinAng;
   //rAngle=RADperDEGREE * scanAng;
   rAngle = scanAng;
   losSC[0]= 0.;
   losSC[1]=cos(rAngle);
   losSC[2]=sin(rAngle);

   return losSC;
}

///////////////////////////////////////////////////////////////////////////
// Method: cal_ORB_los_vector
// Description:
//             Calculate line of sight vector in Orbit Coords
//
// Inputs:
//       scanAng- scan angle in radians
//       roll - in degrees
//       pitch - in degrees
//       yaw - in degrees
////////////////////////////////////////////////////////////////////////////
DVector GeoCal::cal_ORB_los_vector(double scanAng,double roll,double pitch,
                double yaw)
{
   DVector losORB(3);
      
   double cosR,cosP,cosY,cosS,sinR,sinP,sinY,sinS;
   double rRoll,rPitch,rYaw,rScan;  
   rRoll = RADperDEGREE * roll;
   rPitch = RADperDEGREE * pitch;
   rYaw =  RADperDEGREE * yaw; 
   rScan = scanAng;    //in radians
   cosR = cos(rRoll);
   cosP =cos (rPitch);
   cosY = cos (rYaw);
   cosS = cos (rScan);
   sinR = sin(rRoll);
   sinP =sin (rPitch);
   sinY = sin (rYaw);
   sinS = sin(rScan);   //sin scan angle 
       

   DVector losSC(3);
   losSC[0] = 0.;
   losSC[1] = cosS;
   losSC[2] = sinS;
   //cout<<"losSC from tran = "<<losSC[0]<<","<<losSC[1]<<","<<losSC[2]<<endl;
   double e[3][3];
   //Rotation around x :  roll
   e[0][0]= 1.;
   e[0][1] =0.;
   e[0][2] = 0.;
   e[1][0]= 0.;
   e[1][1] =cosR;
   e[1][2] =-sinR;
   e[2][0]= 0.;
   e[2][1] =sinR;
   e[2][2] =cosR;
   DMatrix tranRoll(e);

   // Rotation around y: Pitch
   e[0][0]= cosP;
   e[0][1] =0.;
   e[0][2] =sinP;
   e[1][0]= 0.;
   e[1][1] =1.;
   e[1][2] =0.;
   e[2][0]= -sinP;
   e[2][1] =0.;
   e[2][2] =cosP;
   DMatrix tranPitch(e);

   // Rotation around z: Yaw
   e[0][0]= cosY;
   e[0][1] =-sinY;
   e[0][2] = 0.;
   e[1][0]= sinY;
   e[1][1] =cosY;
   e[1][2] =0.;
   e[2][0]= 0.;
   e[2][1] =0.;
   e[2][2] =1.;
   DMatrix tranYaw(e);

   DMatrix tran(3,3);
   tran = tranYaw * tranPitch;
   DMatrix tran2(3,3);
   tran2 = tran * tranRoll;
   losORB = tran2 *losSC;
   //cout<<"losORB from tran = "<<losORB[0]<<","<<losORB[1]<<","<<losORB[2]<<endl;
   /* Same results 
       losORB[0]= (cosY*sinP*sinR-sinY*cosR)*cosS
                +(cosY*sinP*cosR+sinY*sinR)*sinS;
       losORB[1]= (sinY*sinP*sinR+cosY*cosR)*cosS 
                +(sinY*sinP*cosR-cosY*sinR)*sinS;
       losORB[2]= cosP*sinR*cosS + cosP*cosR*sinS;
       cout<<"losORB 2 = "<<losORB[0]<<","<<losORB[1]<<","<<losORB[2]<<endl;
   */
   return losORB;
}

////////////////////////////////////////////////////////////////////////////
// Method:  unitVectorORBtoECF
// Description: 
//             Convert unit vector from ORB to ECF
// Inputs: 
//         matrixORBtoECF: ORB to ECR convertion matrix 
//         unitposORB: unit vector in ORB coords
//         scVelECF: spacecraft velocity factor in ECF
// Outputs: unitposECF: unit vector in ECF coords
////////////////////////////////////////////////////////////////////////////
int GeoCal::unitVectorORBtoECF(DMatrix &matrixORBtoECF,DVector &unitposORB,
DVector &unitposECF,DVector &scVelECF)
{  
   double norm = sqrt(unitposORB *unitposORB);   //the norm of input vector
                                                 // * dot multiplication
   if (norm < 0.99999 || norm > 1.00001)
   {  
      cerr<<" Input vector is not a unit vector !!!!! Stop!!!\n";
      return 1;
   }	 
   DVector tmpECF1(3), tmpECF2(3);
   DVector vDivc(3);
   tmpECF1 = matrixORBtoECF * unitposORB ;
   //double vDivc[3];  //used to correct aberration
   double tmpnorm;
   for(int iXYZ=0; iXYZ<3; iXYZ++)
   {
      vDivc[iXYZ] = scVelECF[iXYZ]/SPEED_OF_LIGHT;
   }	  
   tmpECF2 =tmpECF1  - vDivc;
   tmpnorm = sqrt( tmpECF2 * tmpECF2);
   unitposECF = tmpECF2/ tmpnorm;
   return 0;
}   



/////////////////////////////////////////////////////////////////////////////
// Method: 
// Description Calculate local horizontal elevation angle in LVLH coord
// Inputs:
//         scPosECI: spacecraft postion vector in ECI
//         scanAng:  mirror scan angle in radians
//         roll,pitch,yaw in degrees
// Output: elevation angle in radians
///////////////////////////////////////////////////////////////////////////
void GeoCal::cal_horizontal_elevation_angle(double scanAng,
                                  double roll,double pitch, double yaw,
				  double &elevAngle)
{
   int n=3;
   DVector losORB(n);
   losORB = cal_ORB_los_vector(scanAng,roll,pitch,yaw);
   double losDotlos = losORB *losORB;
   elevAngle = acos (losORB[1]/sqrt(losDotlos)); 
    
   return;
}   

/////////////////////////////////////////////////////////////////////////////
// Method: 
// Description Calculate trans matrix from ORB coords to ECF coords)
// Inputs: scVelECF: spacecraft velocity vector in ECF
//         scPosECF: spacecraft postion vector in ECF
// Return: conversion matrix
/////////////////////////////////////////////////////////////////////////////
DMatrix GeoCal::matrix_ORBtoECF(DVector &scVelECF, DVector &scPosECF)
{ 
   // !!!! scPosECF and scVelECF should not be zero.
   //cout<<"scVelECF from tran = "<<scVelECF[0]<<","<<scVelECF[1]<<","<<scVelECF[2]<<endl;
   DVector xECF(3),yECF(3),zECF(3); //Orbit coordinate axis x,y and z in ECF
   DVector xTmp0(3), xTmp(3);
   double rdotr; //dot product of position vector with itself
   double vdotr; //dot product of velocity and the postion vector
   rdotr = scPosECF*scPosECF;
   vdotr = scVelECF*scPosECF;
   zECF = scPosECF * (-1./ sqrt(rdotr)) ;
   xTmp0 = scPosECF * (vdotr/rdotr);
   xTmp = scVelECF - xTmp0;
   //X-axis is close to velocity, but normal to P in P-V plane. 
   //Normalize xECF
   double xdotx = xTmp * xTmp ;//dot product of xTmp and itself
   if ( fabs(xdotx) <1.e-10) {
      cout<<" velocity vector is either 0 or is coincident with"<<
            " position vector. Orbital corrdinate system cannot be"
	    <<" usefully determined."<<endl;
      exit(8);	    
   }   
   xECF = xTmp *(1./sqrt(xdotx));
   yECF = zECF ^ xECF;  //cross product of Z axis and X axis
 
   DMatrix result(xECF,yECF,zECF);
   return result;
} 
   
//////////////////////////////////////////////////////////////////////////////
// Method matrix_ORBtoECF2
// Description: The second method to Calculate trans matrix from SC coords to
// ECF coords)
/////////////////////////////////////////////////////////////////////////     
DMatrix GeoCal::matrix_ORBtoECF2(DVector &scVelECF, DVector &scPosECF)
{ 
   DVector xECF(3),yECF(3),zECF(3); //spacecraft coordinate axis x,y and z in ECF
   double rdotr; //dot product of position vector with itself
   double vdotv; //dot product of velocity with itself
   rdotr = scPosECF*scPosECF;
   vdotv = scVelECF*scVelECF;
   xECF = scVelECF * (1./ sqrt(vdotv)) ;
   zECF = scPosECF * (-1./ sqrt(rdotr)) ;
   yECF = zECF ^ xECF;  //cross product of Z axis and X axis
   double ydoty = yECF * yECF;
   //cout<< " yECF * yECF ="<<ydoty<<endl;
   
   DMatrix result(xECF,yECF,zECF);
   return result;
} 

///////////////////////////////////////////////////////////////////////////
// Method: matrix_SCtoECI
// Description: Build rotation matrix(SC coords->ECI coords) from quaternions
//              (SC->ECI)  ECI quatternion provided in PVAT)
// HISTORY: program modified from sc2eci_mat.pro from APL,MDC/shared modules
// Note: In IDL vec(i,j): the first subscript denotes the column (i)
//       and the second subscript is the row.
//////////////////////////////////////////////////////////////////////////// 
DMatrix GeoCal::matrix_SCtoECI(double *q)
{ 
   //cout<<"In matrix_SCtoECI: q[0]="<<q[0]<<"\n";
   //cout<<"In matrix_SCtoECI: q[1]="<<q[1]<<"\n";
   //cout<<"In matrix_SCtoECI: q[2]="<<q[2]<<"\n";
   //cout<<"In matrix_SCtoECI: q[3]="<<q[3]<<"\n";
   double s2emat[3][3];
   s2emat[0][0]=q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
   s2emat[1][1]=-q[0]*q[0]+q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
   s2emat[2][2]=-q[0]*q[0]-q[1]*q[1]+q[2]*q[2]+q[3]*q[3];
   s2emat[1][0]=2.*(q[0]*q[1]+q[3]*q[2]);
   s2emat[2][0]=2.*(q[0]*q[2]-q[3]*q[1]);
   s2emat[0][1]=2.*(q[1]*q[0]-q[3]*q[2]);
   s2emat[2][1]=2.*(q[1]*q[2]+q[3]*q[0]);
   s2emat[0][2]=2.*(q[2]*q[0]+q[3]*q[1]);
   s2emat[1][2]=2.*(q[1]*q[2]-q[3]*q[0]);
   DMatrix result(s2emat);
   return result;
} 

///////////////////////////////////////////////////////////////////////////
//  Method calTpECI
//  Description: Calculate Tangent point in ECI 
//  Inputs: 
//      losECI:  line of sight vector in ECI
//      SCposECI: observer's postion vector in ECI. (spacecraft location vector
//      gmst: greenwich mean sidereal time in radians
//           
//  Outputs:
//     lat: tangent point  geodetic latitude in degrees
//     lon: tangent point  geodetic longitude in degrees
//     altitude: geodetic altitude in km
//
//     notan:  integer indicates the validity of tangent point
//  Returns:
//     Tangent point location vector in ECI
//       
//  HISTORY: program translated and implemented from tangent.pro from MDC/
//           shared modules by Y.Wang
//           tangent.pro only calculate geodetic tangent point latitude and
//           altitude. It has been implemented to calculate longitude.
//       
//  
//////////////////////////////////////////////////////////////////////////     
DVector GeoCal::calTpECI(DVector &losECI, DVector &SCposECI, double gmst,double &lat,double &lon,double &altitude,int &notan)
{
   int i;
   notan =0;
   double f = double(1.)/298.25722356;  //Earth flatness parameter
   double re = 6378.1370;  //mean equitorial radius
   //double eqs; //The square of the major eccentricity e^2=(2f-f^2);
   double one = double(1.);
   double e;
   e = one/((one-f)*(one-f))-one;
   double alarr[3] ={one,one,e+one};
   DVector pv(3),Rs(3),Rsu(3),al(alarr,3),cv(3);
   pv = losECI;
   Rs = SCposECI;
   //cout<<"losECI = "<<losECI[0]<<","<<losECI[1]<<","<<losECI[2]<<endl;
   //cout<<"pv = "<<pv[0]<<","<<pv[1]<<","<<pv[2]<<endl;
   //cout<<"SCposECI = "<<SCposECI[0]<<","<<SCposECI[1]<<","<<SCposECI[2]<<endl;
   //cout<<"Rs = "<<Rs[0]<<","<<Rs[1]<<","<<Rs[2]<<endl;
   //cout<<"vector al="<<al[0] <<al[1] <<al[2] <<"\n";

   Rsu = Rs/sqrt( Rs * Rs);
   DVector buf0(3),buf1(3);
   buf0 = Rsu ^ pv;
   cv = (pv ^ buf0)*(-1.);
   cv = cv/sqrt(cv*cv);
 
   double pop,coc,RoR,poc,Roc,Rop,a,b,c,rad0;
   DVector bufpop(3),bufcoc(3),bufRoR(3),bufpoc(3),bufRoc(3),bufRop(3);
   bufpop = pv % pv ; pop = al * bufpop; 
   bufcoc = cv % cv ; coc = al * bufcoc;
   bufRoR = Rs % Rs ; RoR = al * bufRoR;
   bufpoc = pv % cv;  poc = al * bufpoc;
   bufRoc = Rs % cv;  Roc = al * bufRoc;
   bufRop = Rs % pv;  Rop = al * bufRop;

   a = pop*poc*poc -pop*pop*coc;
   b = pop*(Roc*poc -Rop*coc);
   c = (re*re -RoR)*poc*poc +(2.*Roc*poc -coc*Rop)*Rop;
 
   DVector tp(3);
         
   rad0=b*b-a*c;
   if (rad0 < 0.) {
      notan =1;
      return tp;
   }
         
   double L[2] = {(-b+sqrt(rad0))/a, (-b-sqrt(rad0))/a};
   double ah[2]={coc,coc};
   double bh[2] ={Roc+L[0]*poc, Roc+L[1]*poc};
   double ch[2],h[4];
   double ch0 =RoR -re*re;
   for (i=0;i<2;i++) {
      ch[i]=pop*L[i]*L[i]+2.*L[i]*Rop +ch0;
   }  
   for (i=0;i<2;i++) {
      //double bh0 = sqrt(bh[i]*bh[i]-ah[i]*ch[i]); 
      //Add the following check on Nov03,2003
      double bufv = bh[i]*bh[i]-ah[i]*ch[i];
      if (bufv <0.|| ah[i] ==0.)
      {      notan =1;
              return tp;
      }
      double bh0 =  sqrt(bufv) ;
      h[i]=(-bh[i]+bh0)/ah[i];
      h[i+2]=(-bh[i]-bh0)/ah[i];
   }  
   int minic=0; 
   for (i=0;i<4;i++){
      if (fabs(h[i]) <fabs(h[minic])) minic = i;
   }
   //cout<<"h[0],h[1],h[2],h[3]="<<h[0]<<","<<h[1]<<","<<h[2]<<","<<h[3]<<endl;
   double th,LL,glat;
   th =h[minic];  //tangent height

   LL =L[0];
   if (minic % 2) LL=L[1];
   //cout<<"Slant range LL = "<<LL<<endl;
   // use tp = Rs + pv*LL instead of Rs + pv*LL+cv*th from tangent.pro
   DVector tmp1(3); 
   DVector tmp3(3); 
   tmp1 = pv*LL;
   tmp3 = cv*th;
   //tp = Rs + tmp1;
   tp = Rs + tmp1 + tmp3;
  
   cout << "Tmp1:  " << tmp1.cv[0] << "  " << tmp1.cv[1] << "  " << tmp1.cv[2] << "  " << endl;

 
   //To calculate geocentric,equatorial right ascension of tangent point (added by Y.Wang)
   //This step is added in order to calculate tangent point longitude.
   DVector tpu(3); 
   double rasc_tp;
   double glon; 
   tpu = tp/sqrt(tp*tp);
   rasc_tp = datan0(tpu[1],tpu[0]);   //in radians
   //Calculate tangent point longitude glon in radians.
   //gmst (Greenwich mean sidereal time)
   glon = rasc_tp - gmst;// (if negative, its west of Greenwich meridian)
   if (glon <0) glon += TWO_PI;
   lon = glon * DEGREESperRAD; 
   // To calculate geodectic lat: use tp=Rs + pv*LL+cv*th yw 5/25/01
   DVector tmp2(3), tpp(3),tppu(3);
   tmp2 = cv*th;
   tpp = tp + tmp2; 
   tppu = tpp/sqrt(tpp*tpp);
   glat = 90. -acos(tppu[2])*DEGREESperRAD;
   lat =glat;
   altitude = th;
   //cout << "in Tangent.pro: th ="<<th <<" glat= "<<glat<<"\n";
   //cout << "Tp in ECI from IDL in km= "<< tp[0] << tp[1] <<tp[2] <<"\n";
   return tp;
}	 


///////////////////////////////////////////////////////////////////////////
//  Method calTpECI
//  Description: Calculate Tangent point in ECI
//  Inputs:
//      losECI:  line of sight vector in ECI
//      SCposECI: observer's postion vector in ECI. (spacecraft location vector
//      gmst: greenwich mean sidereal time in radians
//
//  Outputs:
//     lat: tangent point  geodetic latitude in degrees
//     lon: tangent point  geodetic longitude in degrees
//     altitude: geodetic altitude in km
//
//     notan:  integer indicates the validity of tangent point
//  Returns:
//     Tangent point location vector in ECI
//
//  HISTORY: program translated and implemented from tangent.pro from MDC/
//           shared modules by Y.Wang
//           tangent.pro only calculate geodetic tangent point latitude and
//           altitude. It has been implemented to calculate longitude.
//           Reduced back to not include LLA by C.W.Brown (2006)
//
//////////////////////////////////////////////////////////////////////////
DVector GeoCal::calTpECI(DVector &losECI, DVector &SCposECI, int &notan)
{
   int i;
   notan =0;
   double f = double(1.)/298.25722356;  //Earth flatness parameter
   double re = 6378.1370;  //mean equitorial radius
   //double eqs; //The square of the major eccentricity e^2=(2f-f^2);
   double one = double(1.);
   double e;
   e = one/((one-f)*(one-f))-one;
   double alarr[3] ={one,one,e+one};
   DVector pv(3),Rs(3),Rsu(3),al(alarr,3),cv(3);
   pv = losECI;
   Rs = SCposECI;
   //cout<<"losECI = "<<losECI[0]<<","<<losECI[1]<<","<<losECI[2]<<endl;
   //cout<<"pv = "<<pv[0]<<","<<pv[1]<<","<<pv[2]<<endl;
   //cout<<"SCposECI = "<<SCposECI[0]<<","<<SCposECI[1]<<","<<SCposECI[2]<<endl;
   //cout<<"Rs = "<<Rs[0]<<","<<Rs[1]<<","<<Rs[2]<<endl;
   //cout<<"vector al="<<al[0] <<al[1] <<al[2] <<"\n";

   Rsu = Rs/sqrt( Rs * Rs);
   DVector buf0(3),buf1(3);
   buf0 = Rsu ^ pv;
   cv = (pv ^ buf0)*(-1.);
   cv = cv/sqrt(cv*cv);

   double pop,coc,RoR,poc,Roc,Rop,a,b,c,rad0;
   DVector bufpop(3),bufcoc(3),bufRoR(3),bufpoc(3),bufRoc(3),bufRop(3);
   bufpop = pv % pv ; pop = al * bufpop;
   bufcoc = cv % cv ; coc = al * bufcoc;
   bufRoR = Rs % Rs ; RoR = al * bufRoR;
   bufpoc = pv % cv;  poc = al * bufpoc;
   bufRoc = Rs % cv;  Roc = al * bufRoc;
   bufRop = Rs % pv;  Rop = al * bufRop;

   a = pop*poc*poc -pop*pop*coc;
   b = pop*(Roc*poc -Rop*coc);
   c = (re*re -RoR)*poc*poc +(2.*Roc*poc -coc*Rop)*Rop;

   DVector tp(3);

   rad0=b*b-a*c;
   if (rad0 < 0.) {
      notan =1;
      return tp;
   }

   double L[2] = {(-b+sqrt(rad0))/a, (-b-sqrt(rad0))/a};
   double ah[2]={coc,coc};
   double bh[2] ={Roc+L[0]*poc, Roc+L[1]*poc};
   double ch[2],h[4];
   double ch0 =RoR -re*re;
   for (i=0;i<2;i++) {
      ch[i]=pop*L[i]*L[i]+2.*L[i]*Rop +ch0;
   }
   for (i=0;i<2;i++) {
      //double bh0 = sqrt(bh[i]*bh[i]-ah[i]*ch[i]);
      //Add the following check on Nov03,2003
      double bufv = bh[i]*bh[i]-ah[i]*ch[i];
      if (bufv <0.|| ah[i] ==0.)
      {      notan =1;
              return tp;
      }
      double bh0 =  sqrt(bufv) ;
      h[i]=(-bh[i]+bh0)/ah[i];
      h[i+2]=(-bh[i]-bh0)/ah[i];
   }
   int minic=0;
   for (i=0;i<4;i++){
      //if (fabs(h[i]) <fabs(h[minic])) minic = i;
      if (h[i] < h[minic]) minic = i;
   }
   //cout<<"h[0],h[1],h[2],h[3]="<<h[0]<<","<<h[1]<<","<<h[2]<<","<<h[3]<<endl;
   double th,LL,glat;
   th =h[minic];  //tangent height

   LL =L[0];
   if (minic % 2) LL=L[1];
   //cout<<"Slant range LL = "<<LL<<endl;
   // use tp = Rs + pv*LL instead of Rs + pv*LL+cv*th from tangent.pro
   DVector tmp1(3);
  // DVector tmp2(3);
   tmp1 = pv*LL;
  // tmp2 = cv*th;
   //tp = Rs + tmp1  + tmp2;
   tp = Rs + tmp1;
/*if(fabs(th) < 1.0){   
   cout << "Tp  :  " << tp.cv[0] << "  " << tp.cv[1] << "  " << tp.cv[2] << "  " << LL << " " << th << endl;
   cout << "Tmp1:  " << tmp1.cv[0] << "  " << tmp1.cv[1] << "  " << tmp1.cv[2] << " " << L[0] << " " << L[1] << endl;
   cout << "Tmp2:  " << tmp2.cv[0] << "  " << tmp2.cv[1] << "  " << tmp2.cv[2] << " " << minic << endl;
   cout << "pv  :  " << pv.cv[0] << "  " << pv.cv[1] << "  " << pv.cv[2] << endl;
   cout << "cv  :  " << cv.cv[0] << "  " << cv.cv[1] << "  " << cv.cv[2] << endl;
   cout<<"h[0],h[1],h[2],h[3]="<<h[0]<<","<<h[1]<<","<<h[2]<<","<<h[3]<<endl;
}*/
   return tp;
}




///////////////////////////////////////////////////////////////////////////
//  Method calCart2LLA_F1999
//  Description: Convert Cartesian to Geodetic Coordinates
//  Inputs:
//      cartXYX:  position vector
//      geoidrf:  Reference Geoid  -- Use Earth Model for Now
//      
//  Outputs:
//     lat: tangent point  geodetic latitude in degrees
//     lon: tangent point  geodetic longitude in degrees
//     altitude: geodetic altitude in km
//     notan:  integer indicates the validity of tangent point
//  Returns:
//     Tangent point location vector in ECI
//
//  HISTORY: program translated and implemented from Fukushima 1999
//		Journal of Geodesy (1999) 73: 603-610
//
//////////////////////////////////////////////////////////////////////////
void GeoCal::calCart2LLA_F1999(DVector &cartXYZ, double gmst, double &lat, double &lon, double &alt, int &notan)
{
   int i=0;
   notan =0;
   double onez = 1.0;
   if(cartXYZ[2] < 0) onez = -1.0; //Algorithm only works for positive z.  
   
   //FUTURE VALUES TO BE REPLACED BY GEOID DATA
   //double f = double(1.)/298.2572235630;  //Earth flatness parameter
   double a_earth = 6378.137;  //mean equitorial radiusi
   double b_earth = 6356.752;
   double e2 = (a_earth * a_earth - b_earth * b_earth) / (a_earth * a_earth);
   //The square of the major eccentricity e^2=(2f-f^2);
   //Note:  The 2nd eccentricity, e', is defined as sqrt((a2 - b2) / b2)
   //       Fukushima defines e' as sqrt(1 - e2)
   double ep = sqrt(1-e2); 
   //double ep2 = (a_earth * a_earth - b_earth * b_earth) / (b_earth * b_earth);
   double c = a_earth * e2;
   double zp, u, v, p, t, tm, deltaT;
   zp = ep * cartXYZ[2] * onez;
   u = 2 * (zp - c);
   v = 2 * (zp + c);
   p = sqrt(cartXYZ[0]*cartXYZ[0]+cartXYZ[1]*cartXYZ[1]);
   tm = (c - zp) / p;
   if(tm <= 0) t = (p - c + zp) / (p - c + 2*zp); // Use Upperbound
   else if (tm >= 1) t = p / (zp + c);            // Use Lowerbound
   else if (CHECKQFUNC(tm, p, u, v)) t =  p / (zp + c); // Use Lowerbound
   else t = (p - c + zp) / (p - c + 2*zp); // Use Upperbound
   deltaT = calDeltaT(t, p, u, v);
   while(fabs(deltaT) > DELTATCONCRIT && i<MAXITS){
	t = t + deltaT;
	deltaT = calDeltaT(t, p, u, v);
	i++;
   }
   lat = atan2((1-t*t) , 2*ep*t) * onez * DEGREESperRAD;  // Calc Lat and make neg if z < 0
   alt = (2*p*ep*t + onez*cartXYZ[2]*(1-t*t) - a_earth*ep*(1+t*t)) / sqrt((1+t*t)*(1+t*t) - 4*e2*t*t);

   //To calculate geocentric,equatorial right ascension of tangent point (added by Y.Wang)
   //This step is added in order to calculate tangent point longitude.
   DVector tpu(3);
   double rasc_tp;
   double glon;
   tpu = cartXYZ/sqrt(cartXYZ*cartXYZ);
   rasc_tp = datan0(tpu[1],tpu[0]);   //in radians
   //Calculate tangent point longitude glon in radians.
   //gmst (Greenwich mean sidereal time)
   glon = rasc_tp - gmst;// (if negative, its west of Greenwich meridian)
   if (glon <0) glon += TWO_PI;
   lon = glon * DEGREESperRAD;
//   cout << "FUKU1999 GMST: "<< gmst <<" lon= "<<lon<<"\n";
   //cout << "in Tangent.pro: th ="<<th <<" glat= "<<glat<<"\n";
   //cout << "Tp in ECI from IDL in km= "<< tp[0] << tp[1] <<tp[2] <<"\n";
   return;
}

/////////////////////////////////////////////////////////////////////////
// Method: calDeltaT
// Description: Calculate deltaT for Fukushima solution
// Input: t, p, u, and v values from Fukushima
// Return: deltaT value
/////////////////////////////////////////////////////////////////////////
double GeoCal::calDeltaT(double t, double p, double u, double v)
{
	double ft = p*t*t*t*t + u*t*t*t +v*t - p;
	double ftp = 4*p*t*t*t +3*u*t*t + v;  //1st derivative of ft
	return -ft/ftp;
}

/////////////////////////////////////////////////////////////////////////
// Method: calTpECF
// Description: Calculate tangent point location in ECF
// Input: losECF -  line of sight vector in ECF
//        posECF - spacecraft postion vector in ECF
// Return: tangent point location in ECF
/////////////////////////////////////////////////////////////////////////
DVector GeoCal::calTpECF(DVector &losECF, DVector &posECF)
{
   DVector TpECF(3);
   double losDotpos; //dot product of losECF and sc position
   //vector from SC to earth center (-posECF) vector 
   losDotpos = (losECF*posECF) *(-1.); 
   //cout<< "Slant range in calTpECF ="<<losDotpos<<endl;
   DVector Rlos(3);
   Rlos = losECF* losDotpos;
   TpECF = posECF + Rlos;
   return TpECF;
}	 
    	 
  
/////////////////////////////////////////////////////////////////////////////
//   Method: cal_greenwich_sidereal_time
//   Description: calculate the greenwich sidereal time in radians
//   Inputs:
//      yr:  year, 4 digit
//      doy:  day of year
//      sec:  seconds of day
//   Outputs: julianCenturies:  Julian centuries since J2000.
//            gast: Greenwich apparent siderial time in radians
//            gmst: Greenwich mean siderial time in radians
//            dpsi: nutations in longitude 
//            deps: nutations in obliquity
//            eps0: apparent obliquity of the ecliptic (radians)
//            obliq: apparent obliquity of the ecliptic (radians)
//   History: translated and modified from GAST.FOR (Orbit Mechanics Toolbox)
//            by Y.Wang
///////////////////////////////////////////////////////////////////////////     
void GeoCal::cal_greenwich_sidereal_time(int yr,int doy,double sec,
              double &julianCenturies,double &gast,double &gmst,
	      double &dpsi, double &deps,double &eps0, double &obliq)
{
   //printf("in cal_greenwich_sidereal_time \n") ;
   
   double fday;   //fraction of day
   double dj;     //Julian day
  
   fday =double(sec/SECSperDAY);  //fraction of a day
   dj = 365.* (yr-1900) + floor((yr-1901)/4.) + doy + fday -0.5; 
   //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
   double t,t2,t3,l,lp,lraan;
   //time arguments
       
   t = dj/36525. -1.0;  //Julian centuries since J2000.
   t2 = t * t;
   t3=t*t2;
   julianCenturies = t;
// fundamental trig arguments
   l = fmod(RADperDEGREE * (280.4665 + 36000.7698 * t), TWO_PI);

   lp = fmod(RADperDEGREE * (218.3165 + 481267.8813 * t), TWO_PI);

   lraan = fmod(RADperDEGREE * (125.04452 - 1934.136261 * t), TWO_PI);

// nutations in longitude and obliquity

   dpsi = RADperARCsec * (-17.2 * sin(lraan) - 1.32 * sin(2.0 * l)
          - 0.23 * sin(2.0 * lp) + 0.21 * sin(2.0 * lraan));

   deps = RADperARCsec * (9.2 * cos(lraan) + 0.57 * cos(2.0 * l)
           + 0.1 * cos(2.0 * lp) - 0.09 * cos(2.0 * lraan));

// mean and apparent obliquity of the ecliptic (radians)

   eps0 = fmod(RADperDEGREE * (23.0 + 26.0/60.0 + 21.448/3600.0)
            + RADperARCsec * (-46.815 * t - 0.00059 * t2 
            + 0.001813 * t3), TWO_PI) ;

   obliq = eps0 + deps;

// greenwich mean sidereal time (radians)

   gmst = fmod(RADperDEGREE * (280.46061837 + 360.98564736629
          * (dj-36525.) + 0.000387933 * t2
          - t3 / 38710000.0), TWO_PI);
    	    
   //gmst = fmod(RADperDEGREE * (100.46061837 + 36000.770053608*t
   //       + 0.000387933 * t2
   //      - t3 / 38710000.0), TWO_PI);
   //cout<<"in GAST.FOR cal_greenwich_sidereal_time ::gmst ="<<gmst<<"\n";

// greenwich apparent sidereal time (radians)
    
   gast = gmst + fmod(dpsi * cos(obliq), TWO_PI);
   /*cout<<"gast ="<<gast<<"\n";
   cout<<"in GAST.FOR: nutation in longitude dspi in radians="<<dpsi<<endl;
   cout<<"in GAST.FOR: nutation in obliquity deps in radians ="<<deps<<endl;
   cout<<"in GAST.FOR: mean obliq eps0 in radians ="<<eps0<<endl;
   cout<<"in GAST.FOR: apparent obliq in radians ="<<obliq<<endl;
   */
}      

//////////////////////////////////////////////////////////////////////////
// Method: cal_mean_greenwich_sidereal_time
// Description: Calculate mean greenwich sidereal time
// Inputs: 
//        yr: 4 digit year 
//        doy: day of year
//        sec: seconds since midnight
// History: This method is translated and modified from 
//          ftp://istp1.gsfc.nasa.gov/oa_routines/unix/
//                ic_grnwch_sidereal.for
// Note: This method is used to compare gmst results with the method 
//       cal_greenwich_sidereal_time
///////////////////////////////////////////////////////////////////////////
void GeoCal::cal_mean_greenwich_sidereal_time(int yr,int doy,double sec)
{
   const double C0=1.7533685592332653 ;
   const double C1=628.33197068884084;
   const double C2=0.67707139449033354E-05;
   const double C3=6.3003880989848915;
   const double C4 = -0.45087672343186841E-09;
   double t;
   double gmst;  //Greenwich mean siderial time in radians
   double fday =double(sec/SECSperDAY);  //fraction of a day
   double dj = 365.* (yr-1900) + floor((yr-1901)/4.) + doy -0.5; 
   //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
   t = (dj-36525.0)/36525.0;  //reference  to J2000 epoch

   gmst = C0 + t*(C1 + t*(C2 + C4*t)) + C3*fday;
   gmst = fmod( gmst,TWO_PI);
   if (gmst < 0.) {
       gmst += TWO_PI;
   }	
   //printf("in ic_grnwch_sidereal.for gmst = %lf \n",gmst);
}    

////////////////////////////////////////////////////////////////////////////
// Method: cal_mean_greenwich_sidereal_time
// Description: Calculate mean greenwich sidereal time
// Inputs: 
//      yr: 4 digid year
//      doy: day of the year
//      sec: seconds since midnight
// Outputs:
//      sdecl: apparent declination of the sun in radians in ECI
//      srasn:  apparent right ascension of the sun in radians in ECI
//      gst_mean: greenwich mean sidereal time in radians
// History:
//         This method is translated and modified from sun.f from 
//         C.T.Russell,Electrodynamics,1971. It is good for years 
//         1901 through 2099.
//////////////////////////////////////////////////////////////////////////////
void GeoCal::cal_mean_greenwich_sidereal_time(int yr,int doy,double sec,
     double &sdecl,double &srasn,double &gst_mean)
{
    double fday,dj;
    double gmst;  //Greenwich mean siderial time in radians
       
    //cout<<"fday in sun ="<<sec/86400. <<"\n";
    fday =double(sec/SECSperDAY);  //fraction of a day
    dj = 365.* (yr-1900) + floor((yr-1901)/4.) + doy + fday -0.5; 
    //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
    gmst = fmod((279.690983 + 0.9856473354*dj + 360.*fday + 180.), 360.);
    gmst = gmst * RADperDEGREE;
    //cout<<"in SUN::gmst in rad="<<gmst<<endl;
    gst_mean = gmst;
    double t,vl,g,slong,obliq,slp,sind,cosd,sdec,sra;
    t =dj/36525.;
    vl =fmod(279.696678+0.9856473354*dj,360.);
    g =fmod(358.475845 + 0.985600267*dj,360.)*RADperDEGREE;
    //Ecliptic longitude slong in degrees
    slong = vl + (1.91946-0.004789*t)*sin(g) +0.020094*sin(2.*g);
    //cout<<"slong in sun.f ="<<slong*RADperDEGREE<<endl;
    obliq = (23.45229 -0.0130125*t) * RADperDEGREE;
    //cout<<"obliq in sun.f ="<<obliq<<endl;
    slp = (slong -0.005686) *RADperDEGREE;
    //cout<<"slp in sun.f="<<slp<<endl;
    sind = sin (obliq)*sin (slp);
    cosd = sqrt(1.-sind*sind);
    sdec =  atan (sind/cosd);
    sra = PI- atan2(((1.)/tan(obliq))*sind/cosd, -cos(slp)/cosd);
    //cout<<"sra in sun.f ="<<sra<<"\n";
    //cout<<"sdec in sun.f ="<<sdec<<endl;

    // x,y,z : position of the sun in GEI (ECI)
   // double x,y,z;  //  
   // x=cos(sra)*cos(sdec);
   // y=sin(sra)*cos(sdec);
   // z=sin(sdec);
    sdecl =sdec;   //radians
    srasn =sra;    //radians
}

/////////////////////////////////////////////////////////////////////////
// Method: cal_solar_zenith
// Description: Calculate solar zenith angle in degrees
// 
// Input: 
//	gmst: greenwich mean sidereal time in radians
//	sdec: declination of the sun in ECI in radians.
//	srasn: right ascension srasn of the sun in ECI in radians.
//	dlat:  observer's geographical latitude in degrees
//	dlong: observer's geographical longitude in degrees. (East positive)
// Output:
//	solarZenith: solar Zenith angle in degrees
///////////////////////////////////////////////////////////////////////////  
void GeoCal::cal_solar_zenith(double gmst,double sdec,double srasn,
                                   double dlat,double dlong,
                                   double &solarZenith)
{ 
    //cout<<"dlong="<<dlong<<"\n";	
    double rlat,rlong;
    rlong =dlong *RADperDEGREE;
    rlat = dlat * RADperDEGREE;
    //Calculate hour angle rh :  rh = LST - srasn, where LST is the local
    //sidereal time.  LST = gmst + rlong, rlong is east longitude
    //double rh = gmst +rlong - srasn;
    double rh = srasn - (gmst +rlong);
    //cout<<"in solo :srasn ="<<srasn<<"\n";
    //cout<<"gmst ="<<gmst<<"\n";
    //cout<<"rlong="<<rlong<<"\n";
    //cout<<"rh = sran -(gmst+rlong) "<<rh<<"\n";
      
    //cout<<"sdec in solo ="<<sdec<<"\n";
    //Calculate solar zenith angle
    double cossza =sin(sdec)*sin(rlat)+cos(sdec)*cos(rlat)*cos(rh);
    double sza = acos(cossza)* DEGREESperRAD;
    solarZenith =sza;
}  


/////////////////////////////////////////////////////////////////////////////
// Method: cal_solar_zenith_method2
// Description: The second method to calculate solar zenith angle 
// 
//  HISTORY: modified from cozena.f Woolf H.M.
//Parameters:
//	epsiln: eccentricity of earth orbit
//	sinop: sine of obliquit of ecliptic
//INPUTS:
//	day: days of year(jan 1 is day1)
//	hour: hours of greenwich time
//	rlong: degree longitude (0 to 360 from east to west)
//	rlat: degree latitude counted positive northern hemisphere.
/////////////////////////////////////////////////////////////////////////////
void GeoCal::cal_solar_zenith_method2(int day, double hour,
                                   double rlat,double rlong,
                                   double &solarZenith)
{  
   //cout<< "day ="<<day<<"\n";
   //cout<< "hour ="<<hour<<"\n";
   static const double dpy = 365.242; //days per year
   static const double dph = 15.;    //degree per hour
   static const double epsiln=0.016733; //eccentricity of earth orbit:
   static const double sinob=0.3978;   //sine of obliquit of ecliptic
   double dlon,dang,homp,hang,ang,sigma,sindlt,cosdlt,cozena;
   if(rlong >180) {
       dlon = 360.0 -rlong ; //counted positive west of greenwich
   }else
   {  
      dlon = -rlong;
   }
   //cout<<"rlong,dlon="<<rlong<<" "<<dlon<<"\n";	
   //cout<<"rlat="<<rlat<<"\n";	
   dang = TWO_PI*(day-1)/dpy;
   homp = 12.0 +0.123570*sin(dang)-0.004289*cos(dang)
       +0.153809*sin(2.*dang)+0.060783*cos(2.*dang);
   hang =dph*(hour-homp)-dlon;   
   //cout<<"hour in rad = "<<hour*dph*RADperDEGREE<<"\n";
   //cout<<"homp in rad="<<homp*dph*RADperDEGREE<<"\n";
   //cout<<"dlon in rad="<<dlon*RADperDEGREE<<"\n";
   //cout<<"hang =hour-home-dlon ="<<hang*RADperDEGREE<<"\n";
	
   ang = 279.9348*RADperDEGREE +dang;
   sigma =(ang*DEGREESperRAD+ 0.4087*sin(ang)+1.8724*cos(ang)
            -0.0182*sin(2.*ang)+0.0083*cos(2.0*ang))*RADperDEGREE;
   sindlt =sinob*sin(sigma);
    //cout<<" dec in wool ="<<asin(sindlt)<<"\n";
   cosdlt =sqrt(1.-sindlt*sindlt);
   cozena = sindlt*sin(RADperDEGREE*rlat)+cosdlt*cos(RADperDEGREE*rlat)*cos(RADperDEGREE*hang);
   solarZenith =acos(cozena)* DEGREESperRAD;	
   //cout<<"from cozena.f Woolf H.M. solarZenith in deg="<<solarZenith<<endl;
} 			 

void GeoCal::cal_solarLT(double sec,double dlong,double &SolarLT)
//////////////////////////////////////////////////////////////////////////////
//
// Description: Calculate local civil time
// 
// Input:
//       
//      dlong: observer's geographical longitude in degrees. (East positive)
//      sec: seconds since midnight (UTC)
//      double &SolarLT
//
////////////////////////////////////////////////////////////////////////////
{
   double flong; //from -180 to 180; dlong is from 0 to 360
   if (dlong<=180) flong = dlong;
   else flong = dlong - 360.;
   SolarLT =  sec/3600. +flong/15.; //hours
   if (SolarLT >24.) SolarLT -=24;   //Reduce to the range of 0-24 if necessary
   if (SolarLT <0) SolarLT +=24;   //Reduce to the range of 0-24 if necessary
   SolarLT = SolarLT * 3600000.;  //Convert to milisec 
}   

/////////////////////////////////////////////////////////////////////////////
double GeoCal::julianCenturyEpoch1900(int yr,int doy,double sec){
       double fday,dj,djd,t;
       fday =double(sec/SECSperDAY);  //fraction of a day
       dj = 365.* (yr-1900) + floor((yr-1901)/4.) + doy + fday -0.5; 
       //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
       djd = dj -36525.;
       t =dj/36525.;
       return t;
   }  

///////////////////////////////////////////////////////////////////
// Calculate lunar vector in ECI  Coords  (from earth to moon)
//  Input:
//     Julian Date
//  Output:
//     right ascension and declination
//     earth-moon distance
//  Return:
//    lunar vector in ECI
//  History:
//    Translated and modified from MOON.FOR (Orbital Mechanics Fortran 
//    ToolBox) by Y.Wang  2001
//////////////////////////////////////////////////////////////////
     void GeoCal::cal_ECI_lunar_vector(int year,int doy,double sec, double &rasc, double &decl, DVector &lunarV)
     {
     //lunar position vector subroutine
      double rmoon[3]; 
      double  djd,t,gm,gm2,gm3,fm,fm2,em,em2,em4,gs,lv, lm,rm, ls, l;
      double plon,plat,obliq,r,rmmoon,a,b;
      static const double atr =4.848136811095360E-006;
      double fday,dj;
      fday =double(sec/SECSperDAY);  //fraction of a day
      dj = 365.* (year-1900) + floor((year-1901)/4.) + doy + fday -0.5; //
       //dj: Julian days past since 1900 epoch
      //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
      djd = dj -36525.; // Julian days past since 2000 epoch
      t =dj/36525.;   // Julian centrary since 1900 epoch
						
      
     // djd = xjdate - 2451545.0;  //Julian days past since 2000 epoch
     // t = (djd / 36525.0) + 1.0; //Julian centrary


      gm = r2r(.374897  + .03629164709  * djd);
      gm2 = 2.0  * gm;
      gm3 = 3.0  * gm;
      fm = r2r(.259091  + .0367481952  * djd);
      fm2 = 2.0  * fm;
      em = r2r(.827362  + .03386319198  * djd);
      em2 = 2.0  * em;
      em4 = 4.0  * em;
      gs = r2r(.993126  + .0027377785  * djd);
      lv = r2r(.505498  + .00445046867  * djd);
      lm = r2r(.606434  + .03660110129  * djd);
      ls = r2r(.779072  + .00273790931  * djd);
      rm = r2r(.347343  - .00014709391  * djd);

      l = 22640.0 *sin(gm)-4586.0 *sin(gm-em2)+2370.0 *sin(em2)
           +769.0 *sin(gm2)-668.0 *sin(gs)-412.0 *sin(fm2)
           -212.0 *sin(gm2-em2)-206.0 *sin(gm-em2+gs)
           +192.0 *sin(gm+em2)+165.0 *sin(em2-gs)
           +148.0 *sin(gm-gs)-125.0 *sin(em)-110 *sin(gm+gs)
           -55.0 *sin(fm2-em2)-45.0 *sin(gm+fm2)+40 *sin(gm-fm2)
           -38.0 *sin(gm-em4)+36.0 *sin(gm3)-31 *sin(gm2-em4)
           +28.0 *sin(gm-em2-gs)-24.0 *sin(em2+gs)+19.0 *sin(gm-em)
           +18.0 *sin(em+gs)+15.0 *sin(gm+em2-gs)+14 *sin(gm2+em2)
           +14.0 *sin(em4)-13.0 *sin(gm3-em2)-17.0 *sin(rm)
           -11.0 *sin(gm+16.0 *ls-18.0 *lv)+10.0 *sin(gm2-gs)
           +9.0 *sin(gm-fm2-em2)
           +9.0 *(cos(gm+16.0 *ls-18.0 *lv)-sin(gm2-em2+gs))
           -8.0 *sin(gm+em)
           +8.0 *(sin(2.0 *(em-gs))-sin(gm2+gs))
           -7.0 *(sin(2.0 *gs)
           +sin(gm-2.0 *(em-gs))-sin(rm))
           -6.0 *(sin(gm-fm2+em2)+sin(fm2+em2))-4.0 *(sin(gm-em4+gs)
           -t*cos(gm+16.0 *ls-18.0 *lv))
           -4.0 *(sin(gm2+fm2)-t*sin(gm+16.0 *ls-18.0 *lv))
           +3.0 *(sin(gm-3.0 *em)-sin(gm+em2+gs)-sin(gm2-em4+gs)
           +sin(gm-2.0 *gs)+sin(gm-em2-2.0 *gs))
           -2.0 *(sin(gm2-em2-gs)+sin(fm2-em2+gs)-sin(gm+em4))
           +2.0 *(sin(4.0 *gm)+sin(em4-gs)+sin(gm2-em));

//     geocentric, ecliptic longitude (radians)
      plon = lm + atr*l;

      b = 18461.0 *sin(fm)+1010.0 *sin(gm+fm)+1000.0 *sin(gm-fm)
           -624.0 *sin(fm-em2)-199.0 *sin(gm-fm-em2)
           -167.0 *sin(gm+fm-em2)
           +117.0 *sin(fm+em2)+62 *sin(gm2+fm)+33 *sin(gm-fm+em2)
           +32.0 *sin(gm2-fm)-30 *sin(fm-em2+gs)
           -16.0 *sin(gm2-em2+fm)
           +15.0 *sin(gm+fm+em2)+12 *sin(fm-em2-gs)
           -9.0 *sin(gm-fm-em2+gs)
           -8.0 *(sin(fm+rm)-sin(fm+em2-gs))-7.0 *sin(gm+fm-em2+gs)
           +7.0 *(sin(gm+fm-gs)-sin(gm+fm-em4))
           -6.0 *(sin(fm+gs)+sin(3.0 *fm)-sin(gm-fm-gs))
           -5.0 *(sin(fm+em)+sin(gm+fm+gs)+sin(gm-fm+gs)
           -sin(fm-gs)-sin(fm-em))
           +4.0 *(sin(gm3+fm)-sin(fm-em4))-3.0 *(sin(gm-fm-em4)
           -sin(gm-3.0 *fm))
           -2.0 *(sin(gm2-fm-em4)+sin(3.0 *fm-em2)-sin(gm2-fm+em2)
           -sin(gm-fm+em2-gs));

//     geocentric, ecliptic latitude (radians)

      plat = atr * (b + 2.0  * (sin(gm2-fm-em2) + sin(gm3-fm)));
      
//     obliquity of the ecliptic (radians)

      obliq = atr * (84428.0  - 47.0  * t + 9.0  * cos(rm));
      //cout<<"obliq in MOON.FOR ="<<obliq <<endl;

      r = 60.36298 -3.27746 *cos(gm)-.57994 *cos(gm-em2)
            -.46357 *cos(em2)-8.904e-02*cos(gm2)
            +.03865 *cos(gm2-em2)
            -.03237 *cos(em2-gs)-.02688 *cos(gm+em2)
            -.02358 *cos(gm-em2+gs)
            -.0203 *cos(gm-gs)+.01719 *cos(em)+.01671 *cos(gm+gs)
            +.01247 *cos(gm-fm2)+.00704 *cos(gs)
            +.00529 *cos(em2+gs)
            -.00524 *cos(gm-em4)+.00398 *cos(gm-em2-gs)
            -.00366 *cos(gm3)
            -.00295 *cos(gm2-em4)-.00263 *cos(em+gs)
            +.00249 *cos(gm3-em2)
            -.00221 *cos(gm+em2-gs)+.00185 *cos(fm2-em2)
            -.00161 *cos(2.0 *(em-gs))
            +.00147 *cos(gm+fm2-em2)-.00142 *cos(em4)
            +.00139 *cos(gm2-em2+gs);

//     geocentric distance (km)

      rmmoon = 6378.14  * (r -.00118  * cos(gm-em4+gs) 
                 - .00116  * cos(gm2+em2)
                 - .0011  * cos(gm2-gs));
      //cout<<"geocentric moon earth distance (km)="<<rmmoon<<endl;
      a = sin(plon) * cos(obliq) - tan(plat) * sin(obliq);
      b = cos(plon);

//     geocentric, equatorial right ascension (radians)

      rasc = datan0(a, b);

//     geocentric, equatorial declination (radians)

      decl = asin(sin(plat) * cos(obliq) + cos(plat) * sin(obliq)
               * sin(plon));

//     geocentric, equatorial lunar position vector (km)

      rmoon[0] = rmmoon * cos(rasc) * cos(decl);
      rmoon[1] = rmmoon * sin(rasc) * cos(decl);
      rmoon[2] = rmmoon * sin(decl);
      /*
      //cout<<"rmoon[0] = "<<rmoon[0] <<endl;
      //cout<<"rmoon[1] = "<<rmoon[1] <<endl;
      //cout<<"rmoon[2] = "<<rmoon[2] <<endl;
      */
      lunarV[0] =rmoon[0];
      lunarV[1] =rmoon[1];
      lunarV[2] =rmoon[2];

      return; 
    }
    
//////////////////////////////////////////////////////////////////////////////
//   cal_ECIvector_SCtoMoon(int year,int doy,double sec, DVector &SCposECI,
//                       DVector &lunarSC)
//   INPUT:
//     year:  year eg 2001
//     doy:   day of the year 
//     sec:   sec since UT midnight
//     SCposECI: spacecraft position vector in ECI
//   OUTPUT:
//     lunarSC:  vector from spacecraft to center of moon in ECI 
/////////////////////////////////////////////////////////////////////////////

    void GeoCal:: cal_ECIvector_SCtoMoon(int year,int doy,double sec, DVector &SCposECI,DVector &lunarSC)
    {
        DVector lunarECI(3);
        double rasc,decl;
        cal_ECI_lunar_vector(year,doy,sec,rasc,decl,lunarECI);
        lunarSC = lunarECI - SCposECI;
	return;
     }	

				      

///////////////////////////////////////////////////////////////////////////
//  Calculate earth sun distance //////////////////////////////////////////
//  Input:
//     yr: year
//     doy:  days of the year
//     sec:  second since midnight
//  Output:
//     rsun_e:  earth sun distance in km
//  History:
//     Translated and modified from SUN.FOR (orbit mechanics FORTRAN toolbox)
//////////////////////////////////////////////////////////////////////////////
void GeoCal::cal_sun_earth(int yr, int doy, double sec,double &rsun_e)
{
   //   solar ephemeris subroutine
   double atr =4.848136811095360E-6;
   double t,gs,lm,ls,g2,g4,g5,rm,pl,plat,plon,obliq,a,b,rasc,decl,rmsun;
   double fday,dj,djd;
   fday =double(sec/SECSperDAY);  //fraction of a day
   dj = 365.* (yr-1900) + floor((yr-1901)/4.) + doy + fday -0.5;
   //cout<<"!!!!!!Julday dj ="<<dj<<"\n";
   djd = dj -36525.;
   t =dj/36525.;
   //djd = xjdate - 2451545.0;
   //t = (djd / 36525.) + 1.0;


   gs = r2r(.993126 + .0027377785*djd);
   lm = r2r(.606434 + .03660110129 * djd);
   ls = r2r(.779072 + .00273790931 * djd);
   g2 = r2r(.140023 + .00445036173 * djd);
   g4 = r2r(.053856 + .00145561327 * djd);
   g5 = r2r(.056531 + .00023080893 * djd);
   rm = r2r(.347343 - .00014709391 * djd);

   pl = 6910.0*sin(gs)+72.0*sin(2.0*gs)-17.0*t*sin(gs)
        -7.0*cos(gs-g5)+6.0*sin(lm-ls)
        +5.0*sin(4.0*gs-8.0*g4+3.0*g5)
        -5.0*cos(2.0*(gs-g2))
        -4.0*sin(gs-g2)+4.0*cos(4.0*gs-8.0*g4+3.0*g5)
        +3.0*sin(2.0*(gs-g2))-3.0*sin(g5)
        -3.0*sin(2.0*(gs-g5));
   //cout<<"pl in SUN.FOR="<<pl<<endl;	     

   //     geocentric, ecliptic longitude

   plon = ls + atr * (pl - 17.0 * sin(rm));
   //cout<<"plon in SUN.FOR ="<<plon<<endl;

   //     geocentric, ecliptic latitude

   plat = 0.0;
      
   obliq = atr * (84428.0 - 47.0 * t + 9.0 * cos(rm));
   //cout<<"obliq in SUN.FOR ="<<obliq<<endl;

   //    geocentric distance (km)

   rmsun = 149597870.66 * (1.00014 - 0.01675*cos(gs)
           -0.00014 * cos(2.0 * gs));
   rsun_e = rmsun;
   a = sin(plon) * cos(obliq) - tan(plat) * sin(obliq);
   b = cos(plon);
// geocentric, equatorial right ascension (radians)
   rasc = datan0(a,b);
// geocentric, declination (radians)
   decl = asin(sin(plat) * cos(obliq) + cos(plat) * sin(obliq) * sin(plon));
   //cout<<"rasc in SUN.FOR ="<<rasc<<endl;
   //cout<<"decl in SUN.FOR ="<<decl<<endl;

}    

/////////////////////////////////////////////////////////////////////////////
//  Method r2r(double x)
////////////////////////////////////////////////////////////////////////////
double GeoCal::r2r(double x)
{
   double results = TWO_PI * (x - floor(x));
   return results;
}  


/*
/////////////////////////////////////////////////////////////////////////////
//Convert ECI vector to ECF vector
//
//     DVector GeoCal::convert_ECItoECF(SABER_TIME &utcTime,DVector &posECI)
//////////////////////////////////////////////////////////////////////////// 
DVector GeoCal::convert_ECItoECF(SABER_TIME &utcTime,DVector &posECI)
{ 
   DVector vec(3);
   int year;
   int doy;
   double timeSec;
   year = utcTime.year;
   doy = utcTime.doy;
   timeSec = utcTime.msec/1000.;
   vec = convert_ECItoECF(year,doy,timeSec,posECI);
   return vec;
} 
*/
 	
/////////////////////////////////////////////////////////////////////////////
// Method: convert_ECItoECF(int year,int doy,double timeSec,DVector &posECI)
// Description: Convert ECI vector to ECF vector
//
//////////////////////////////////////////////////////////////////////////// 
DVector GeoCal::convert_ECItoECF(int year,int doy,double timeSec,DVector &posECI )
{
   //cout<<" in convert_ECItoECF: year,day,timeSec ="<<year<<","<<doy<<"," <<timeSec<<endl;
   double zero = double(0.0);
   double one = double (1.);
   const double EARTH_ROT_RATE = 7.2921151467e-5 ; //radians/sec
   int i,j;
   double t,gst_rad,gmst_rad,deps,dpsi,eps0,eps;  //t=julian centuries
   DMatrix mtran(3,3),dmtran(3,3),cmatrix(3,3),tranmatrix(3,3),dtranmatrix(3,3);
   DVector posECF(3);
   cal_greenwich_sidereal_time(year,doy,timeSec,t,
               gst_rad,gmst_rad,dpsi,deps,eps0,eps);

   double cos_gst,sin_gst;
   cos_gst = cos(gmst_rad) ;
   sin_gst = sin(gmst_rad);
      
   mtran(0,0) = cos_gst;
   mtran(0,1) = sin_gst;
   mtran(0,2) = zero;
   mtran(1,0) = -sin_gst;
   mtran(1,1) = cos_gst;
   mtran(1,2) = zero;
   mtran(2,0) = zero;
   mtran(2,1) = zero;
   mtran(2,2) = one;
/*
       if (Velocity_req ==1){
         dmtran(0,0) = cos_gst*EARTH_ROT_RATE;
         dmtran(0,1) = sin_gst*EARTH_ROT_RATE;
         dmtran(0,2) = zero;
         dmtran(1,0) = -dmtran(0,1);
         dmtran(1,1) = dmtran(0,0);
         dmtran(1,2) = zero;
         dmtran(2,0) = zero;
         dmtran(2,1) = zero;
         dmtran(2,2) = one;
       }
       else
       {
         for (i=0;i<3;i++) {
	  for (j =0;j<3;j++) {
	    dmtran(i,j)=zero;
	  }
	 }
       }	
*/
   DMatrix nutmat(3,3),premat(3,3);       
   //double epso,zeta,theta,zee,sinzet

	double	    EPSO;	    //Mean obliquity of date
	double	    ZETA;	    //Precession angle
	double	    THETA;	    //Precession angle
	double	    ZEE	;	    //Precession angle
	double	    sinZET;	    //Sine of ZETA
	double	    cosZET;	    //Cosine of ZETA
	double	    sinZEE;	    //Sine of ZEE
	double	    cosZEE;	    //Cosine of ZEE
	double	    sinTHE;	    //Sine of THETA
	double	    cosTHE;	    //Cosine of THETA
	double	    DELEPS;	    //DELTA EPSILON, Nutation in obliquity
	double	    DELPSI;	    //DELTA PSI, Nutation in longitude
	double	    EPS	;	    //EPSILON  apparent obliquity
	double	    cosEP;	    //Cosine of EPS (apparent obliquity)
	double	    cosEPO;	    //Cosine of OBLIQUITY(mean obliquity)
	double	    cosPSI;	    //Cosine of PSI--- cos(DELPSI) 
	double	    sinEP;	    //Sine of EPS---   sin(EPS)
	double	    sinEPO;	    //Sine of OBLIQUITY---    sin(EPSO)
	double	    sinPSI;	    //Sine of PSI ---  sin(DELPSI)
	double TIME,T2,T3;
        EPSO =eps0;
	DELEPS =deps;
	DELPSI =dpsi;
	EPS = eps;
      TIME = t;

      T2 = TIME*TIME;
      T3 = TIME*T2;

//     CALCULATE PRECESSION ANGLES
      ZETA   = 0.11180860865024398e-01*TIME
            + 0.14635555405334670e-05*T2
            + 0.87256766326094274e-07*T3;
	    //cout<<"ZETA ="<<ZETA <<"\n";
      THETA  = 0.97171734551696701e-02*TIME
            - 0.20684575704538352e-05*T2
            - 0.20281210721855218e-06*T3;
	   // cout<<"THETA ="<<THETA <<"\n";
      ZEE    = 0.11180860865024398e-01*TIME
            + 0.53071584043698687e-05*T2
            + 0.88250634372368822e-07*T3;
	   // cout<<"ZEE ="<<ZEE <<"\n";
      sinZET = sin(ZETA);                                               
      cosZET = cos(ZETA);                                               
      sinZEE = sin(ZEE) ;  
      cosZEE = cos(ZEE) ;  
      sinTHE = sin(THETA); 
      cosTHE = cos(THETA); 
                                                                       
//     COMPUTE THE TRANSFORMATION MATRIX BETWEEN MEAN EQUATOR AND        
//     EQUINOX OF 1950 AND MEAN EQUATOR AND EQUINOX OF DATE. THIS        
//     MATRIX IS CALLED premat.                                          
//                                                                       
      premat(0,0) = -sinZET*sinZEE  + cosZET*cosZEE*cosTHE ;             
      premat(1,0) =  cosZEE*sinZET  + sinZEE*cosTHE*cosZET ;            
      premat(2,0) =  sinTHE*cosZET ;           
      premat(0,1) = -sinZEE*cosZET  - cosZEE*cosTHE*sinZET ;          
      premat(1,1) =  cosZEE*cosZET  - sinZEE*cosTHE*sinZET ;         
      premat(2,1) = -sinTHE*sinZET ;        
      premat(0,2) = -cosZEE*sinTHE ;       
      premat(1,2) = -sinZEE*sinTHE ;      
      premat(2,2) =  cosTHE;     
      /*
       cout << "Test results of premat(0,0)=" << premat(0,0)<<"\n";
       cout << "Test results of premat(0,1)=" << premat(0,1)<<"\n";
       cout << "Test results of premat(0,2)=" << premat(0,2)<<"\n";
       cout << "Test results of premat(1,0)=" << premat(1,0)<<"\n";
       cout << "Test results of premat(1,1)=" << premat(1,1)<<"\n";
       cout << "Test results of premat(1,2)=" << premat(1,2)<<"\n";
       cout << "Test results of premat(2,0)=" << premat(2,0)<<"\n";
       cout << "Test results of premat(2,1)=" << premat(2,1)<<"\n";
       cout << "Test results of premat(2,2)=" << premat(2,2)<<"\n";
       */
//     CALCULATE MEAN OBLIQUITY OF DATE (EPSO). WHERE TIME IS MEASURED IN
//     JULIAN CENTURIES FROM 2000.0.
//
      EPSO=(1.813e-3*T3-5.9e-4*T2
          -4.6815e+1*TIME+8.4381448e+4)*RADperARCsec;

      //cout<<"from GAST.for eps0 ="<<eps0<<"\n";
      //cout<<"in ECItoECF  EPSO ="<<EPSO<<"\n";  //same results
      //cout<<"in ECItoECF DELPSI*DEGREESperRAS="<<DELPSI*180.0/PI<<endl;

      cosEP=cos(EPS);                               
      cosEPO=cos(EPSO);                             
      cosPSI=cos(DELPSI);                           
      sinEP=sin(EPS);                               
      sinEPO=sin(EPSO);                             
      sinPSI=sin(DELPSI);                           
      nutmat(0,0)=cosPSI;                            
      nutmat(0,1)=-sinPSI*cosEPO;                    
      nutmat(0,2)=-sinPSI*sinEPO;                    
      nutmat(1,0)=sinPSI*cosEP;                      
      nutmat(1,1)=cosPSI*cosEP*cosEPO+sinEP*sinEPO;  
      nutmat(1,2)=cosPSI*cosEP*sinEPO-sinEP*cosEPO ; 
      nutmat(2,0)=sinPSI*sinEP;                      
      nutmat(2,1)=cosPSI*sinEP*cosEPO-cosEP*sinEPO;  
      nutmat(2,2)=cosPSI*sinEP*sinEPO+cosEP*cosEPO;  
  /*     cout << "Test results of nutmat(0,0)=" << nutmat(0,0)<<"\n";
       cout << "Test results of nutmat(0,1)=" << nutmat(0,1)<<"\n";
       cout << "Test results of nutmat(0,2)=" << nutmat(0,2)<<"\n";
       cout << "Test results of nutmat(1,0)=" << nutmat(1,0)<<"\n";
       cout << "Test results of nutmat(1,1)=" << nutmat(1,1)<<"\n";
       cout << "Test results of nutmat(1,2)=" << nutmat(1,2)<<"\n";
       cout << "Test results of nutmat(2,0)=" << nutmat(2,0)<<"\n";
       cout << "Test results of nutmat(2,1)=" << nutmat(2,1)<<"\n";
       cout << "Test results of nutmat(2,2)=" << nutmat(2,2)<<"\n";
  */                                                              
//     CALCULATE ELEMENTS OF nutmat * premat.  THIS MATRIX IS THE 
//     ANALYTICALLY CALCULATED TRANSFORMATION MATRIX, WHICH WILL  
//     TRANSFORM THE MEAN EARTH EQUATOR AND EQUINOX OF J2000 INTO
//     THE TRUE EARTH EQUATOR AND EXQUINOX OF DATE.
                                                                
       cmatrix = nutmat * premat;
        
       tranmatrix =mtran *cmatrix; 
       dtranmatrix =dmtran * cmatrix;
       DVector tmp1(3), tmp2(3);
       //DVector velECF(3);
       //velECF= dtranmatrix * velECI;
       //cout<<"velECF="<<velECF[0]<<","<<velECF[1]<<","<<velECF[2]<<endl;
       posECF = tranmatrix * posECI;
       //cout<<"posECI="<<posECI[0]<<","<<posECI[1]<<","<<posECI[2]<<endl;
       //cout<<"posECF="<<posECF[0]<<","<<posECF[1]<<","<<posECF[2]<<endl;
       return posECF;
}       


///////////////////////////////////////////////////////////////////////////
// Method: calECF2Geodetic(DVector &cECF, double &lat, double &lon,
//                           double &alt)
// Description: 			   
//	Convert coordinates from the ECF cartesian corrdinates to geodetic
//	latitude,east longitude and spheroid height
//	Input: vector in ECF coords.  
// History:
//   Convert from Fortran Function XYZPLH(ARG1,ARG2)
//   NOAA KLM USER's GUIDE Appendix B
//   Add in geodetic altitude alt
///////////////////////////////////////////////////////////////////////////
void GeoCal::calECF2Geodetic(DVector &cECF, double &lat, double &lon,
                           double &alt)
{
   double x,y,z,rlat,rlon,he; //he: spheroid height
   x = cECF[0];
   y = cECF[1];
   z = cECF[2];
   double xysq,t,zt,h1,sinphi,qsp,h2,t1,esqsp;
   int i,j,ierr;	
   double f = double(1.)/298.25722356;  //Earth flatness parameter
   double re = 6378.1370;  //mean equitorial radius
   double eqs; //The square of the major eccentricity e^2=(2f-f^2);
   eqs=(2.*f-f*f);
   //cout<<"eqs ="<<eqs<<"\n";
   ierr=0;
   t=eqs*z;
   xysq = x*x+y*y;
   if (fabs(z)>= 1.e-15) goto Five;
   he = sqrt(xysq) - re;
   rlat = 0.;
   goto twoone; 
   Five:
   for(i=0; i<24;i++){
      zt =z + t;
      h1 = sqrt(xysq+ zt*zt);
      sinphi = zt/h1;
      esqsp =eqs*sinphi;
      h2=re/sqrt(double(1.)-esqsp*sinphi);
      //cout<<"zt,sinphi,epsqsp ="<<zt<<sinphi<<esqsp<<"\n";
      //cout<<"h1,h2,t1="<<h1<<","<<h2<<","<<t1<<"\n";
      t1=h2*esqsp;
      if(fabs((t1-t)/t1) < 1e-14) goto twoo;
      t=t1;
   }
   ierr =1;
   goto endthis;
   twoo:  he=h1-h2;
   //cout << "he=h1-h2 where h1,h2="<<h1<<", "<<h2<<"\n";
   rlat= asin(sinphi);
   twoone:	
   if (x == 0.) {
      if (y == 0.) 
      {
         ierr+= 2;
	 cerr <<"Longitude is undefined \n";
	 goto endthis;
      } else if (y < 0.) 
      {
          rlon = PI *1.5;
      } else
      {
        rlon = PI/2.0;
      }
   } else
   { 
     rlon = datan0(y,x);
   }
   endthis:
   lat =rlat * DEGREESperRAD;
   lon =rlon * DEGREESperRAD;
   alt =he;  //altitude
}			   


//////////////////////////////////////////////////////////////////////////
//double GeoCal::datan0(double y, double x)			 
//Return  a value for the atan2 between 0 and 2 pi where the tangent
// is defined by the two input arguments as y/x
// uses C math subroutine atan2 with returens a value between -pi and pi, 
// given two arguments
// 
// input y =  sine of angle;  x =cosine of angle.
// output(return)   four quadrant inverse tangent (radians)
// 
// History:
//   Convert from Fortran Function DATAN0(ARG1,ARG2)
//   NOAA KLM USER's GUIDE Appendix B
// This Routine is equavalent to ATAN3.FOR from Orbital Mechanics Fortran
// Toolbox
/////////////////////////////////////////////////////////////////////
double GeoCal::datan0(double y, double x)
{
   double datan0;
   datan0 = atan2(y,x);
   if(datan0 < 0.){
      datan0 =datan0 + TWO_PI;
   }
   return datan0;
}	 

/////////////////////////////////////////////////////////////////////////////
// Method: double Angle_los_moonScan(DVector &losECI,DVector &lunarECI)
// return the angle between los and moon vector in degrees
//
//////////////////////////////////////////////////////////////////////////// 
double GeoCal:: Angle_los_moonScan(DVector &losECI,DVector &lunarECI)
{
   
     double losDotlunar; //dot product of los and lunar vector
     losDotlunar = (losECI*lunarECI);
     double losdotlos = losECI * losECI ;
     double lunardotlunar = lunarECI * lunarECI;
     double theta = acos(losDotlunar/sqrt(losdotlos * lunardotlunar))*DEGREESperRAD;
     //double moon_radius = 1.738; //km
     //double maxAng =  asin(moon_radius/sqrt(lunardotlunar))*DEGREESperRAD;
     //cout<<"maxAng moon = "<<maxAng<<endl;
     return theta;
     
}