/* N3EMO Orbit Simulator routines
   Copyright (C) 1986 Robert W. Berger  */

#include "df0:include/stdio.h"
#include "df0:include/math.h"
#define PI 3.14159265
#define PI2 (PI*2)
#define MinutesPerDay (24*60.0)
#define SecondsPerDay (60*MinutesPerDay)
#define HalfSecond (0.5/SecondsPerDay)
#define EarthRadius 6378.16      /* Kilometers */
#define EarthFlat (1/298.25)      /* Earth Flattening Coeff. */
#define SiderealSolar 1.0027379093
#define DegreesPerRadian (180/PI)
#define RadiansPerDegree (PI/180)
#define ABS(x) ((x) < 0 ? (-(x)) : (x))
#define SQR(x) ((x)*(x))
#define RefYear 79
#define RefSidereal 0.27676777  /* 90 = 0.27610467  */
#define RefDay 0      /* Day of week */
char *MonthNames[12] = { "Jan","Feb","Mar","Apr","May","Jun","Jul",
         "Aug","Sep","Oct","Nov","Dec" };
int MonthDays[12] = {31,28,31,30,31,30,31,31,30,31,30,31};
char *DayNames[7] = { "Sunday","Monday","Tuesday","Wednesday","Thursday",
         "Friday","Saturday"};
/* Solve Kepler's equation               */
/* Inputs:                     */
/*   MeanAnomaly   Time since last perigee, in radians.   */
/*         PI2 = one complete orbit.      */
/*   Eccentricity   Eccentricity of orbit's ellipse.   */
/* Output:                     */
/*   TrueAnomaly   Angle between perigee, geocenter, and   */
/*         current position.         */
double Kepler(MeanAnomaly,Eccentricity)
register double MeanAnomaly,Eccentricity;
{
register double E;      /* Eccentric Anomaly         */
register double Error;
register double TrueAnomaly;
    
    E = MeanAnomaly;   /* Initial guess */
    do
        {
   Error = (E - Eccentricity*sin(E) - MeanAnomaly) 
      / (1 - Eccentricity*cos(E));
   E -= Error;
   }
   while (ABS(Error) >= 0.000001);
    
    if (ABS(E-PI) < 0.000001)
        TrueAnomaly = PI;
      else
        TrueAnomaly = 2*atan(sqrt((1+Eccentricity)/(1-Eccentricity))
            *tan(E/2));
    if (TrueAnomaly < 0)
   TrueAnomaly += PI2;
    return TrueAnomaly;
}
GetSubSatPoint(EpochTime,EpochRAAN,EpochArgPerigee,Inclination,
   Eccentricity,RAANPrecession,PerigeePrecession,Time,TrueAnomaly,
      Latitude,Longitude)
double EpochTime,EpochRAAN,EpochArgPerigee,Inclination,Eccentricity;
double RAANPrecession,PerigeePrecession,Time;
double TrueAnomaly,*Longitude,*Latitude;
{
    double RAAN,ArgPerigee;
    int i;
    ArgPerigee = EpochArgPerigee + (Time-EpochTime)*PerigeePrecession;
    *Latitude = asin(sin(Inclination)*sin(ArgPerigee+TrueAnomaly));
    RAAN = EpochRAAN - (Time-EpochTime)*RAANPrecession;
    *Longitude = -acos(cos(TrueAnomaly+ArgPerigee)/cos(*Latitude));
    if ((Inclination > PI/2 && *Latitude < 0) 
          || (Inclination < PI/2 && *Latitude > 0))
   *Longitude = -*Longitude;
    *Longitude += RAAN;
    /* Convert from celestial to terrestrial coordinates */
    *Longitude -= PI2*(Time*SiderealSolar + RefSidereal);
    /* Make West be positive */
    *Longitude = -*Longitude;
    /* i = floor(Longitude/2*pi)   */
    i = *Longitude/PI2;
    if(i < 0)
   i--;
    *Longitude -= i*PI2;
}

GetPrecession(SemiMajorAxis,Eccentricity,Inclination,
   RAANPrecession,PerigeePrecession)
double SemiMajorAxis,Eccentricity,Inclination;
double *RAANPrecession,*PerigeePrecession;
{
  *RAANPrecession = 9.95*pow(EarthRadius/SemiMajorAxis,3.5) * cos(Inclination)
       / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
  *PerigeePrecession = 4.97*pow(EarthRadius/SemiMajorAxis,3.5)
      * (5*SQR(cos(Inclination))-1)
       / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
}
GetBearings(SiteLat,SiteLong,SiteAltitude,
   SatLat,SatLong,Radius,Azimuth,Elevation,Range)
double SiteLat,SiteLong,SiteAltitude,SatLat,SatLong,Radius;
double *Azimuth,*Elevation,*Range;
{
    double SinSiteLat,sinSiteLong,SinSatLat,SinSatLong;
    double CosSiteLat,cosSiteLong,CosSatLat,CosSatLong;
    double CosBeta,SinBeta;
    double LongDiff;
    SinSiteLat = sin(SiteLat); sinSiteLong = sin(SiteLong);
    CosSiteLat = cos(SiteLat); cosSiteLong = cos(SiteLong);    
    SinSatLat = sin(SatLat); SinSatLong = sin(SatLong);
    CosSatLat = cos(SatLat); CosSatLong = cos(SatLong);    
    LongDiff = SiteLong - SatLong;
    CosBeta = SinSiteLat*SinSatLat+CosSiteLat*CosSatLat*cos(LongDiff);
    SinBeta = sqrt(1-SQR(CosBeta));
    *Azimuth = acos((SinSatLat- SinSiteLat*CosBeta)/CosSiteLat/SinBeta);
    if (LongDiff < -PI)
        LongDiff += PI2;
    if (LongDiff > PI)
        LongDiff -= PI2;
    if (LongDiff < 0)
   *Azimuth = PI2 - *Azimuth;
    /* Convert to geocentric radius */
    SiteAltitude += EarthRadius*(1-EarthFlat/2+EarthFlat/2*cos(2*SiteLat));
    *Elevation = atan((Radius*CosBeta-(SiteAltitude))
             /(Radius*SinBeta));
    *Range = sqrt(SQR(Radius) + SQR(SiteAltitude)
              -2*Radius*SiteAltitude*CosBeta);
}

PrintDate(OutFile,Day)
FILE *OutFile;
{
    int Month,Year,DayOfWeek;
    DayOfWeek = (Day-RefDay) % 7;
    Year = RefYear;
    while (Day <= 0)
   {
   Year -= 1;
   if (Year%4 == 0)
       Day += 366;
    else
       Day += 365;
   }
    while ((Year%4 == 0 && Day > 366) || (Year%4 != 0 && Day > 365))
   {
   if (Year%4 == 0)
       Day -= 366;
    else
       Day -= 365;
   Year += 1;
   }
    
    if (Year%4 == 0)
        MonthDays[1] += 1;   /* Leap year */
    
    Month = 0; 
    while (Day > MonthDays[Month])
        {
   Day -= MonthDays[Month];
   Month += 1;
   }
    fprintf(OutFile,"%s  %d %s %d",DayNames[DayOfWeek],Day,
          MonthNames[Month],Year);
    if (Year%4 == 0)
        MonthDays[1] -= 1;   /* Leap year */
}

PrintTime(OutFile,Time)
FILE *OutFile;
double Time;
{
    int day,hours,minutes,seconds;
    day = Time;
    Time -= day;
    if (Time < 0)
        Time += 1.0;   /* Correct for truncation problems with negatives */
    hours = Time*24;
    Time -=  hours/24.0;
    
    minutes = Time*MinutesPerDay;
    Time -= minutes/MinutesPerDay;
    
    seconds = Time*SecondsPerDay;
    seconds -= seconds/SecondsPerDay;
          
    fprintf(OutFile,"%02d%02d:%02d",hours,minutes,seconds);
}    
/* Get the Dayn(SatLong);
    CosSatLat = cos(SatLat); CosSatLong = cos(SatLong);    
    LongDiff = SiteLong - SatLong;
    CosBeta = Sil
 reference is in the future.                  */
double GetDay(Year,Month,Day)
double Day;
{
    int TmpYear;
    TmpYear = Year;
    while (TmpYear > RefYear)
   {
   TmpYear -= 1;
   if (TmpYear%4 == 0)
       Day += 366;
    else
       Day += 365;
   }
    while (TmpYear < RefYear)
   {
   if (TmpYear%4 == 0)
       Day -= 366;
    else
       Day -= 365;
   TmpYear += 1;
   }
    if (Year%4 == 0)
        MonthDays[1] += 1;   /* Leap year */
    
     while (Month > 1)
   {
   Month -= 1;
   Day += MonthDays[Month-1];
   }
    if (Year%4 == 0)
        MonthDays[1] -= 1;   /* Leap year */
    return Day;
}