#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "LatLon.h"
#include "NavigationControlLoop.h"




/****************************************************************************************************  
** Code requires LatLon.c for use.

** To compile for testing, define "TEST" in compilatoin command. (Also requires randn.c).  
i.e.:
$ gcc -DTEST randn.c NavigationControlLoop.c LatLon.c -lm -o NavigationControlLoopTest;
*****************************************************************************************************/

#ifdef TEST
#include "randn.h"

#define INITIAL_LATITUDE 36.0
#define INITIAL_LONGITUDE -122.0
#define MEAN_SPEED 0.5
#define SPEED_STDDEV .05

#define GPS_NOISE_STDDEV .00004 

#define HEADING_BIAS  -40
#define HEADING_BIAS_NOISE 5


#define KI 0.05
#define FILTER_LENGTH 20
double TargLatLon[2] = {36.02, -121.98};



/****************************************************************************************************  
TestPlant generates a new GPSLatLon each time it's called, based upon the above defines and a requested
 thrust direction.  On return LatLon returns the actual position and GPSLatLon returns a noisy GPS position.
*****************************************************************************************************/
void TestPlant(double LatLon[2],double GPSLatLon[2],short ThrustRequest)
{
  double dist;
  double NoisyThrustRequest;
  static double LastLatLon[2];
  static int first = 1;
  if(first)
  {
    LastLatLon[0] = INITIAL_LATITUDE;
    LastLatLon[1] = INITIAL_LONGITUDE;
    first = 0;
  }
  dist = randn(MEAN_SPEED,SPEED_STDDEV);
  NoisyThrustRequest = (double) ThrustRequest+randn(HEADING_BIAS,HEADING_BIAS_NOISE);
  while(NoisyThrustRequest > 360)  //Convert Thrust request to +/-180 degrees for NewLatLon
  	NoisyThrustRequest = NoisyThrustRequest-360.0;
  while(NoisyThrustRequest < 0)
  	NoisyThrustRequest = NoisyThrustRequest+360.0;
  //printf("%f\n",NoisyThrustRequest);

  NewLatLon(LastLatLon,dist,NoisyThrustRequest,LatLon);
  LastLatLon[0] = LatLon[0];
  LastLatLon[1] = LatLon[1];
  GPSLatLon[0] = randn(LatLon[0],GPS_NOISE_STDDEV);
  GPSLatLon[1] = randn(LatLon[1],GPS_NOISE_STDDEV);

  return;
}


void main()
{
  int i;
  short thrust_cmd = 0;
  double GPSLatLon[2];
  double LatLon[2];
  double LatLonSum[2]={0,0};
  double BearingToWayPoint;
  double DistanceToTarget;

  TestPlant(LatLon,GPSLatLon,250);
  for(i=0;i<7000;i++)
  {
    thrust_cmd = ThrustCommandLoop(&DistanceToTarget,GPSLatLon,TargLatLon,KI, FILTER_LENGTH);

    if(thrust_cmd >= 0)
      TestPlant(LatLon,GPSLatLon,thrust_cmd);
    else	
      TestPlant(LatLon,GPSLatLon,250);

    BearingToWayPoint = bearing(GPSLatLon,TargLatLon);

    if(BearingToWayPoint < 0)
      BearingToWayPoint = BearingToWayPoint + 360;

    printf("%10.7f %10.7f     %10.7f %10.7f   %10.7f  %10.7f    %d   %f %f\n",LatLon[0],LatLon[1],GPSLatLon[0],GPSLatLon[1],TargLatLon[0],TargLatLon[1],thrust_cmd,BearingToWayPoint, DistanceToTarget);
    if(i == 3000)
    {
      TargLatLon[0] = 36.0;
      TargLatLon[1] = LatLon[1]; 

    } 
  }
}
#endif  //End of Test Code



short ConvertDegreesToFract(double degrees)
{
  return (short) (degrees*32768/180);
}

double ConvertFractToDegrees(short counts)
{
  if(counts >=0)
    return  ((double) counts)*180/32768;
  else
    return ((double) counts)*180/32768 + 360.0;  //Ensure result is always between is greater than zero (0->360)
}

/****************************************************************************************************  
NavigationControlLoop:  Returns a desired thrust direction in integer degrees (relative to true north)
Arguments:  - DistToTarg, the distanct from the current location estimate to the target is returned in this variable.
            - GPSLatLon, Lat/Lon from GPS, passed into function. 
	    - TargetLatLon, Lat/Lon target, passed into function.
            - MaxIntGain, maximum applied integral gain, ramps up to this from zero after target direction changes.
	    - Number of GPS positions to average to determine location estimates. 
Returns: -  Requested Thrust direction in degrees as an integer between 0 and 359. 
*****************************************************************************************************/
short ThrustCommandLoop(double *DistToTarg, double GPSLatLon[2], double TargetLatLon[2],
                        double MaxIntGain, int N_Update)
{
   static double LastTargetLatLon[2] = {0, 0};
   static double IntGain = 0;
   static int n = 0;
   static double LatLonSum[2]={0,0};
   static double FilteredLatLon_Last[2]={0,0};
   static double IntegralTerm = 0;
   static int ThrustCommand = -1;
   static int LastLatLonValid = 0;

   double FilteredLatLon[2]={0,0};
   double COG_actual;
   double COG_targ;
   short f_COG_actual;
   short f_COG_targ;
   short e;

   //Use most recent GPS hit if filtered values are not yet available
   if(!LastLatLonValid)
     *DistToTarg = distance(GPSLatLon,TargetLatLon);

   //Target Lat/Lon has been updated.
   if((TargetLatLon[0] != LastTargetLatLon[0])  || (TargetLatLon[1] != LastTargetLatLon[1]))
   {
     LastTargetLatLon[0] = TargetLatLon[0];
     LastTargetLatLon[1] = TargetLatLon[1];
     IntGain = 0;
   }

   LatLonSum[0] += GPSLatLon[0];
   LatLonSum[1] += GPSLatLon[1];

   // Trigger every N_Update calls
   //
   printf("ThrustCommandLoop::update %d of %d\n", n+1, N_Update);
   if(++n == N_Update)  //BufferRollOver
   {
     n = 0;

     FilteredLatLon[0] = LatLonSum[0]/N_Update;
     FilteredLatLon[1] = LatLonSum[1]/N_Update;
     LatLonSum[0] = 0.0;
     LatLonSum[1] = 0.0;
 
     printf("ThrustCommandLoop::target lat:%.2f lon:%.2f\n",
            FilteredLatLon[0], FilteredLatLon[1]);
     if(LastLatLonValid)
     {
       *DistToTarg = distance(FilteredLatLon,TargetLatLon);
       COG_actual = bearing(FilteredLatLon_Last,FilteredLatLon);
       COG_targ = bearing(FilteredLatLon,TargetLatLon);
       printf("ThrustCommandLoop::distance:%.2f m @ %.2f degrees\n",
              *DistToTarg, COG_targ);

       if(COG_actual < 0) COG_actual = COG_actual + 360;
       if(COG_targ < 0)   COG_targ = COG_targ + 360;

       f_COG_targ = ConvertDegreesToFract(COG_targ);
       f_COG_actual = ConvertDegreesToFract(COG_actual);

       e = f_COG_targ-f_COG_actual;
       IntegralTerm += (IntGain*e);
       ThrustCommand = ConvertFractToDegrees(f_COG_targ + (short) IntegralTerm);
        
       while(ThrustCommand < 0)
          ThrustCommand +=360;
       while(ThrustCommand > 360)
          ThrustCommand -=360;

       if(IntGain < MaxIntGain) 
          IntGain += MaxIntGain/(N_Update);
       if(IntGain > MaxIntGain) 
          IntGain -= MaxIntGain/(N_Update);
    }

    FilteredLatLon_Last[0] = FilteredLatLon[0];
    FilteredLatLon_Last[1] = FilteredLatLon[1];
    LastLatLonValid = 1;
  }
  
  return ThrustCommand;
}