#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;

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

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

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

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;
 
  if(LastLatLonValid)
    {
      *DistToTarg = distance(FilteredLatLon,TargetLatLon);
      COG_actual = bearing(FilteredLatLon_Last,FilteredLatLon);
      COG_targ = bearing(FilteredLatLon,TargLatLon);
      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;
}