#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

float DeclinationAngle=0.17; // Radians.  http://www.magnetic-declination.com/

Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

int DriveMode=0;
 // 0=Stationary
 // 1=Turn to face waypoint
 // 2=Travel to waypoint
int StartPos=0;
int EndPos=0;  

int Steer=0; // -10 to 10, left to right
float GotFix=0;
int BiasLeft=0;
int BiasRight=0;
double OriginLat=0;
double OriginLng=0;
double DestLat=0;
double DestLng=0;
double DestinationAngle=0;
String GPSString="";
String GPSSentence="";
String GPSTime="";
double GPSLat=0;
double GPSLng=0;
float Distance=0; // Distance to next waypoint
int WayPoint=0;
int WayPointTotal=4;
double WayPointLat[100];
double WayPointLng[100];
String NS="";
String EW="";
int GPSFix=0;
float TargetBearing=0; // Direction from old to new waypoint
int WheelLVal=0;
int WheelRVal=0;
int WheelRTarget=0;
int WheelLTarget=0;
int SpeedFactor=0;
int Craw=2;
int Walk=4;
int Run=6;
int D=0;
int Button1Pin=A6; // Switch
int Button2Pin=A0; // Left up down
int Button3Pin=A3; // Right up down
int ButtonMode=0; // 0=off. 1=Auto. 2=Manual
int Button1=0; // Switch
int Button2=0; // Left up down
int Button3=0; // Right up down

#define RX1     26  // GPS Data in from GPS module SimpleRTK2B.
#define TX1     25  // Free, Motor controller?

#define RX2     32  // Telemetry In
#define TX2     33  // Telemetry Out

int Drive(int WheelLeft, int WheelRight){
 // Right wheel is 64
 // Left wheel is 192
 // increased number is forward
 // This was for the original Sabretooth controler, which blew up.
 int Motor=0;
 if(WheelLeft>10){WheelLeft=10;}
 if(WheelRight>10){WheelRight=10;}
 if(WheelLeft<-10){WheelLeft=-10;}
 if(WheelRight<-10){WheelRight=-10;}
 
 while(WheelLVal!=WheelLeft || WheelRVal!=WheelRight){
   if(WheelLVal<WheelLeft){WheelLVal=WheelLVal+1;}
   if(WheelLVal>WheelLeft){WheelLVal=WheelLVal-1;}
   if(WheelRVal<WheelRight){WheelRVal=WheelRVal+1;}
   if(WheelRVal>WheelRight){WheelRVal=WheelRVal-1;}
   Motor=(WheelRVal*SpeedFactor)+64;
   Serial1.write(Motor);
   Motor=(WheelLVal*SpeedFactor)+192;
   Serial1.write(Motor);
   delay(50); // Ramping
 }
 
 if(WheelLeft==0 && WheelRight==0){
   // All stop
   Serial1.write(0);
 }
 WheelLVal=WheelLeft;
 WheelRVal=WheelRight;
 //delay(100);
 return 0;
}

int Rotate(int NewBearing){
 Drive(0,0);
 int Bearing=ReadCompas();
 Serial2.println("[<TXT>Turning from " + String(Bearing) + " to " + String(NewBearing) + "</TXT>]");
 if((Bearing+360)>(NewBearing+360)){
    Drive(-10,10);
    while((Bearing+360)>(NewBearing+360)){
     Bearing=ReadCompas();
     delay(20);
    }
 }else{
    Drive(10,-10);
    while((Bearing+360)<(NewBearing+360)){
      Bearing=ReadCompas();
      delay(20);
    }
 }
 Drive(0,0);
 Serial2.println("[<TXT>Turning completed</TXT>]");
 
 return 0;
}
 
int ReadCompas(){
 sensors_event_t event;
 mag.getEvent(&event);
 // Added the +13 and +18 to calibrate the sensor
 double newheading = atan2(event.magnetic.y+13, event.magnetic.x+18);
 newheading+=DeclinationAngle;
 if(newheading < 0){ newheading += 2*PI; }
 if(newheading > 2*PI){ newheading -= 2*PI; }
 int B=int(newheading * 180/M_PI);
 Serial2.println("[<COM>" + String(B) + "</COM>]");
 return B;
}

int GetNewHeading(double La1, double Lo1, double La2,  double Lo2){
 int X= 360 - ( atan2((La2-La1), (Lo2-Lo1)) * 180 / 3.14159265 ) + 90;
 if(X>=360){X=X-360;}
 return X;
}

float GetDistance(double La1, double Lo1, double La2,  double Lo2){
 double R = 6378.137; // Radius of earth in KM
 double dLat = La2 * 3.14159265 / 180 - La1 * 3.14159265 / 180;
 double dLon = Lo2 * 3.14159265 / 180 - Lo1 * 3.14159265 / 180;
 double a = sin(dLat/2) * sin(dLat/2) + cos(La1 * 3.14159265 / 180) * cos(La2 * 3.14159265 / 180) * sin(dLon/2) * sin(dLon/2);
 double c = 2 * atan2(sqrt(a), sqrt(1-a));
 double d = R * c * 1000;
 return d; // meters
}

void GetLocation(){
 while (Serial1.available() > 0){
   GPSString+=char(Serial1.read());
 }
 if(GPSString.indexOf("GNGSA")>0){
   StartPos=GPSString.indexOf("GGA")+4;
   EndPos=GPSString.indexOf("GSA")-4;
   if(EndPos>StartPos){
     GPSSentence=GPSString.substring(StartPos,EndPos);
     GPSTime=GPSSentence.substring(0,GPSSentence.indexOf(","));
     GPSSentence=GPSSentence.substring(GPSSentence.indexOf(",")+1,GPSSentence.length());
     GPSLat=GPSSentence.substring(0,GPSSentence.indexOf(",")).toDouble();
     GPSSentence=GPSSentence.substring(GPSSentence.indexOf(",")+1,GPSSentence.length());
     NS=GPSSentence.substring(0,GPSSentence.indexOf(","));
     GPSSentence=GPSSentence.substring(GPSSentence.indexOf(",")+1,GPSSentence.length());
     GPSLng=GPSSentence.substring(0,GPSSentence.indexOf(",")).toDouble();
     GPSSentence=GPSSentence.substring(GPSSentence.indexOf(",")+1,GPSSentence.length());
     EW=GPSSentence.substring(0,GPSSentence.indexOf(","));
     GPSSentence=GPSSentence.substring(GPSSentence.indexOf(",")+1,GPSSentence.length());
     GPSFix=GPSSentence.substring(0,GPSSentence.indexOf(",")).toInt();
     double Temp1=floor(GPSLat/100); //Degrees
     double Temp2=GPSLat-(Temp1*100); //Minutes
     GPSLat=Temp1 + (Temp2/60);
     Temp1=floor(GPSLng/100); //Degrees
     Temp2=GPSLng-(Temp1*100); //Minutes
     GPSLng=Temp1 + (Temp2/60);
   }
   GPSString="";
   
   if(NS=="S"){GPSLat=0-GPSLat;}
   if(EW=="W"){GPSLng=0-GPSLng;}
 }
 return;
}

void setup() {
 
 WayPoint=1;
 WayPointLat[0]=0;
 WayPointLng[0]=0;

 WayPointLat[1]=;
 WayPointLng[1]=;
 WayPointLat[2]=;
 WayPointLng[2]=;
 WayPointLat[3]=;
 WayPointLng[3]=;
 WayPointLat[4]=;
 WayPointLng[4]=;
 
 DestLat=WayPointLat[1];
 DestLng=WayPointLng[1];


 Serial1.begin(115200, SERIAL_8N1, RX1, TX1);

 // Setup serial for telemetry
 Serial2.begin(56700, SERIAL_8N1, RX2, TX2);
 

 // Get the starting position.
 // Make sure we are in the right country at least while the GSP settles down
 while(GPSLat>-20 && GPSLat<-30 && GPSLng<100 && GPSLng>200 ){
   GetLocation();
 }
 WayPointLat[0]=GPSLat;
 WayPointLng[0]=GPSLng;
 OriginLat=GPSLat;
 OriginLng=GPSLng;
 
}

void loop() {
 


 ButtonMode=1;

 if(ButtonMode==1){

   GetLocation();
   int Bearing=ReadCompas();
   
   delay(100);
   // Hack for testing
   if(GPSFix==4 && GPSLat<-20 && GPSLat>-30 && GPSLng>100 && GPSLng<200 ){GPSFix=5;}
   

   if(GPSFix!=5){ DriveMode=0; }
   
   if(GPSFix==5 && DriveMode==0 ){ DriveMode=1;}
   if(DriveMode==0){
     Serial2.print("[<DM>" + String(DriveMode) + "</DM>]"); //Drive mode
     Drive(0,0);
   }
   
   if(DriveMode==1){ // Turn to face next waypoint
     Serial2.print("[<DM>" + String(DriveMode) + "</DM>]"); //Drive mode
     SpeedFactor=Craw;  // Slow for turning
     TargetBearing=GetNewHeading(GPSLat, GPSLng, DestLat,  DestLng);
     if((Bearing+360-5)>(TargetBearing+360) || (Bearing+360+5)<(TargetBearing+360)){
       Rotate(TargetBearing);
     }
     WheelLVal=0;
     WheelRVal=0;
     DriveMode=2;
   }
   
   if(DriveMode==2){
     Serial2.print("[<DM>" + String(DriveMode) + "</DM>]"); //Drive mode
     Distance=GetDistance(GPSLat,GPSLng,DestLat,DestLng);
     
     if(Distance>0.5){
       
       if(Distance<3){
         // We are getting close. Use a compass to closer
         Bearing=ReadCompas();
         TargetBearing=GetNewHeading(GPSLat, GPSLng, DestLat,  DestLng);
         WheelLTarget=WheelLVal;
         WheelRTarget=WheelRVal;
         if(Distance<=1){ SpeedFactor=Craw;}
         if(Distance>1){SpeedFactor=Walk;}
         if(Distance>2){SpeedFactor=Run;}
         
         if(Bearing+360>TargetBearing+360+10 || Bearing+360<TargetBearing+360-10){
           // More that 10 degrees off, stop and rotate.
           Drive(0,0);
           SpeedFactor=Craw;
           Rotate(TargetBearing);
           Drive(10,10);
         }
         if(Bearing+360>TargetBearing+360+5){
             // Turn left a bit
           if(WheelLVal<8 && WheelRVal<15){WheelRTarget=WheelRVal+1;}
           if(WheelLVal>=8){WheelLTarget=WheelLVal-1;}
           Drive(WheelLTarget,WheelRTarget);
         }
         if(Bearing+360<TargetBearing+360-5){
             // Turn right a bit
           if(WheelRVal<8 && WheelLVal<15){WheelLTarget=WheelLVal+1;}
           if(WheelRVal>=8){WheelRTarget=WheelRVal-1;}
           Drive(WheelLTarget,WheelRTarget);
         }
         if(Bearing+360>TargetBearing+360-5 && Bearing+360<TargetBearing+360+5){
             // Going straight
             Drive(10,10);
         }
       }else{
         
       // Add code to see how far we are off the line

       }  



     }else{

       OriginLat=WayPointLat[WayPoint];
       OriginLng=WayPointLng[WayPoint];
       WayPoint++; // Increment Waypoint Counter
       Serial2.print("[<WP>" + String(WayPoint) + "</WP>]"); // Waypoint number
       if(WayPoint>WayPointTotal){WayPoint=1;}
       DestLat=WayPointLat[WayPoint];
       DestLng=WayPointLng[WayPoint];
       
       DriveMode=1; //Aim to new waypoint
       if(DestLng==OriginLng){
         // Avod a divide by zero error OriginLng=OriginLng+0.0000001}


       }else{
         DestinationAngle=(DestLat-OriginLat)/(DestLng-OriginLng);
         DestinationAngle=atan(1/DestinationAngle)*(180/3.1426);
     }
       
       DestLat=WayPointLat[WayPoint];
       DestLng=WayPointLng[WayPoint];
     }
     

   }


 } // End ButtonMode 1


}