/////////////////////////////////////////////////HEADER FILE /////////////////////////////////////////////////////

 #include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"

// Class Definition
class DiffCar{ 
    public:
      void AssignNumber(int);//Startup of Car number
      void AssignRadio(RF24*);//Startup of Radio
      void AssignPines(int,int,int,int); //Startup of Pines
      void AssignKs(float,float,float);
      void AssignTsamp(int);//Startup of Car number

      void DifferencialCarMovement();
      void Indebugmode(bool);
       int Tult=0;
    private:    
      void LisenRadio(); //Lecture of the Radio Emision
        void estimationmodel();
        void fittingmodel();
        void AssignThreshold(int);
        void PrintRadioResult(); //Debug mode
      void CalculateControl(); //Control of the car
        void ControlOrdenador();
        void ControlP();
        void ControlPI();
        void ControlPurePursuit();
        void Standby();
      void Motordrive(); 
      int Number_Car;
      byte DataRecived[7];//Coche-Xact-Yact-thetaact-Xdes-Ydes-ControlSelec-debug
      bool debug;
      int Xactaux,Yactaux;
      float thetaactaux;
      float Xactfloat,Yactfloat,thetaactfloat;  
      int Xact,Yact,Xdes,Ydes,control;
      int Xactup,Yactup,Xdesup,Ydesup;
      int T;
      int Ultimotiempo,Nuevotiempo;
      int accion[2];
      float Klef,Krig,K;
      int threshold,dstar;
      float thetades,thetaactup,thetaact,roh;
      int AIA,AIB,BIA,BIB;
      int r=6;//diameter of tires;
      int L=16;
      int Tsamp;
 
      RF24* Radio;

  
};

// Class initialitazion
void DiffCar::AssignNumber(int n){
 Number_Car=n;
 }
 
void DiffCar::AssignThreshold(int n){
 threshold=n;
 }

void DiffCar::AssignRadio(RF24* radio){
  Radio=radio;
}

void DiffCar::AssignPines(int aia,int aib,int bia,int bib){
  AIA=aia;
  AIB=aib;
  BIA=bia;
  BIB=bib;
}

void DiffCar::AssignKs(float kr,float kl,float k){
  Klef=kl;
  Krig=kr;
  K=k;
}

void DiffCar::AssignTsamp(int tsamp){
  Tsamp=tsamp;
}

void DiffCar::Indebugmode(bool deb){
  debug=deb;
}
// Radio Comunication

void DiffCar::LisenRadio(){
  Radio->read(DataRecived, sizeof(DataRecived));
  
  if (DataRecived[0]==Number_Car){
      Xactup=DataRecived[1];
      Yactup=DataRecived[2];      
      thetaactup=(DataRecived[3])*0.01745329*360/255;//Degrees to Radians.
      Xdesup=DataRecived[4];
      Ydesup=DataRecived[5];
      control=DataRecived[6];     
  }
   if (thetaactup>=3.14){
        thetaactup=thetaactup-3.14*2;
      }
  AssignThreshold(30);
  if (debug) PrintRadioResult();
if (control!=0){
  if ((Xactup==Xactaux)and(Yactup==Yactaux)and(thetaactup==thetaactaux)){
    estimationmodel();
    
  }else{        
        fittingmodel();
  }}
     Serial.print(Xactup);
  Serial.print(" ");
  Serial.print(Yactup);
  Serial.print(" "); 
  Serial.print(Xactaux);
  Serial.print(" ");
  Serial.println(Yactaux);
   Xactaux=Xactup;
   Yactaux=Yactup;
   thetaactaux=thetaactup;
   Xdes=Xdesup;
   Ydes=Ydesup;
   
}
//Debugmode
void DiffCar::PrintRadioResult(){
  if (control==0){
   Serial.print(Xactup);
  Serial.print(" ");
  Serial.print(Yactup);
  Serial.print(" "); 
  Serial.print(accion[0]);
  Serial.print(" ");
  Serial.println(accion[1]);
  }else{
  Serial.print(Xact);
  Serial.print(" ");
  Serial.print(Yact);
  Serial.print(" "); 
  Serial.print(thetaact);
  Serial.print(" ");
  Serial.print(thetades);
  Serial.print(" ");
  Serial.print(Xdes);
  Serial.print(" "); 
  Serial.print(Ydes);
  Serial.print(" ");
  Serial.print(control);
  Serial.print(" ");
  Serial.print(accion[0]);
  Serial.print(" ");
  Serial.println(accion[1]);

  }
}

void DiffCar::estimationmodel(){
  //http://planning.cs.uiuc.edu/ch13.pdf
    Nuevotiempo=millis();
    T=Nuevotiempo-Ultimotiempo;
    float ur=accion[0]/1.21;
    float ul=accion[1];
    ur=ur*0.05;
    ul=ul*0.05;
    //thetaactfloat=0;   
    thetaactfloat=thetaactfloat+(T*r*(ul-ur)/L*0.001);//L = 2*R
    thetaact=thetaactfloat;
    Xactfloat=Xactfloat+((T*4*(ul+ur)*cos(thetaactfloat))*0.001/2);
    Xact=int(Xactfloat);
    Yactfloat=Yactfloat+((T*4*(ul+ur)*sin(thetaactfloat))*0.001/2);
    Yact=int(Yactfloat);
    Serial.print(T);
    Serial.print(" ");
    Serial.print("Estimo:     ");
     Serial.print(thetaactfloat);
     Serial.print(" ");
    Serial.print(thetaact);
    Serial.print("     ");
      Serial.print(Xactfloat);
     Serial.print(" ");
    Serial.print(Xact);
    Serial.print("     ");
       Serial.print(Yactfloat);
     Serial.print(" ");
    Serial.println(Yact);
           Serial.print(Krig);
     Serial.print(" ");
    Serial.println(Klef);
    Ultimotiempo=Nuevotiempo;
}

void DiffCar::fittingmodel(){
    //Krig=(Krig+abs(thetaact-thetaactup))/(abs(Xact-Xactup)+abs(Yact-Yactup));
    //Klef=(Klef-abs(thetaact-thetaactup))/(abs(Xact-Xactup)+abs(Yact-Yactup));
    T=0;
    Xact=Xactup;
    Xactfloat=float(Xact);
    Yact=Yactup;
    Yactfloat=float(Yact);
    thetaact=thetaactup;
    
    thetaactfloat=float(thetaact);
    Serial.println("fit");
}

void DiffCar::CalculateControl(){
if (control==0){
  ControlOrdenador();}else if (control==1){
                     ControlP();}else if  (control==2){
                                ControlPI();}else if  (control==3){
                                              ControlPurePursuit();}else{
   Standby();
}
}


void DiffCar::ControlOrdenador(){

  accion[0]=Xactup;
  accion[1]=Yactup;
  if (debug) PrintRadioResult();
  
}

void DiffCar::ControlP(){
  
  thetades=atan((Yact-Ydes)/(Xact-Xdes));
  
  float Ys=abs((Yact-Ydes)*(Yact-Ydes));
  float Xs=abs((Xact-Xdes)*(Xact-Xdes));
  roh=sqrt(Ys+Xs);
  int large=(int(roh)*K);
  accion[0]=Krig*(255*int(large>255)+large*int(large>0)*int(large<255)-int(2*(thetades-thetaact)))*int(roh>threshold);
  accion[1]=Klef*(255*int(large>255)+large*int(large>0)*int(large<255)+int(2.4*(thetades-thetaact)))*int(roh>threshold);
  if (accion[0]>255){
  accion[1]=accion[1]-(accion[0]-255);
  accion[0]=255;}
  if (debug){ Serial.print("ControlP:   "); 
  Serial.print(roh);
  Serial.print(" ");
  Serial.println(atan((Yact-Ydes)/(Xact-Xdes)));}
}  
void DiffCar::ControlPI(){  
  thetades=atan((Yact-Ydes)/(Xact-Xdes));
  roh=sqrt((Yact-Ydes)*(Yact-Ydes)+(Xact-Xdes)*(Xact-Xdes));
  int large=large+(int(roh)*K);
  int gir=gir+int(thetades-thetaact);
  accion[0]=255*int(large>255)+large*int(large>0)*int(large<255)-gir*int(roh>threshold);
  accion[1]=255*int(large>255)+large*int(large>0)*int(large<255)+gir*int(roh>threshold);
  if (debug) Serial.print("ControlPI");
}
void DiffCar::ControlPurePursuit(){
  float thetades=atan((Xact-Xdes)/(Yact-Ydes));
  float roh=sqrt((Yact-Ydes)*(Yact-Ydes)+(Xact-Xdes)*(Xact-Xdes))-dstar;
  int large=large+(int(roh)*K);
  int gir=gir+int(thetades-thetaact);
  accion[0]=(255*int(large>255)+large*int(large>0)*int(large<255)-gir)*int(roh>threshold);
  accion[1]=(255*int(large>255)+large*int(large>0)*int(large<255)+gir)*int(roh>threshold);
  if (debug) Serial.print("PurePursuit");  
}
void DiffCar::Standby(){
  accion[0]=0;
  accion[1]=0;
  if (debug) Serial.print("STANDBY");
}

void DiffCar::Motordrive()
{      
  /*     int Spe;
       int theta;
       Spe=accion[0];
       theta=accion[1];
     // Si la velocidad es positiva los motores se moveran hacia delante
     // Si es negativa hacia atras
     // Si es cero los motores no avanzan
       bool forw=(Spe>0);
       bool back=(Spe<0);
      // Theta positivo giro en el sentido de las agujas del reloj
      // Theta negativo giro en sentido antihorario
       bool right = (theta<0);
       bool left = (theta>0);
  analogWrite(AIA, int(forw)*abs(Spe)+int(left)*abs(theta));
  analogWrite(AIB, int(back)*abs(Spe)+int(right)*abs(theta));
  analogWrite(BIA, int(back)*abs(Spe)+int(left)*abs(theta));
  analogWrite(BIB, int(forw)*abs(Spe)+int(right)*abs(theta));*/
  //ACCION DE 0 a 255
  //Serial.println("motordrive in");

  Serial.println((millis()-Tult));
  //if (((Tsamp-(micros()-Tult))>0)and(control!=0)) delay(Tsamp-(micros()-Tult));
  
  analogWrite(AIA, int(accion[0]*int(accion[0]>=60))*int(control!=4));
  analogWrite(AIB, 20*int(accion[0]<=60)*int(control!=4));//To avoid dumping
  analogWrite(BIA, 20*int(accion[1]<=50)*int(control!=4));//To avoid dumping
  analogWrite(BIB, accion[1]*int(accion[1]>=50)*int(control!=4));
  Tult=millis();
   // Serial.println("motordrive out");

}

void DiffCar::DifferencialCarMovement(){
    LisenRadio(); 
    CalculateControl();
    Motordrive();

}

/////////////////////////////////////////////////HERE THE PROGRAM START/////////////////////////////////////////////////////
const int AIA = 5;  // (pwm) pin 9 connected to pin A-IA 
const int AIB = 6;  // (pwm) pin 5 connected to pin A-IB 
const int BIA = 3;  // (pwm) pin 10 connected to pin B-IA  
const int BIB = 9;  // (pwm) pin 6 connected to pin B-IB 

const float Kri=1.21;
const float Kle=1;
const float K=1.2;
const int tsamp=100;
RF24 radio(17,10);
const uint64_t pipes[2] = { 0xF0F0F0F010LL, 0xF0F0F0F008LL };
DiffCar diffCar;

void setup() {
  Serial.begin(4800);
  printf_begin();
  radio.begin();
  radio.setRetries(15, 15);
  radio.openWritingPipe(pipes[1]);
  radio.openReadingPipe(1, pipes[0]);
  radio.startListening();
  radio.printDetails();
  diffCar.AssignNumber(1);//Startup of Car number
  diffCar.AssignRadio(&radio);//Startup of Radio
  diffCar.AssignPines(AIA,AIB,BIA,BIB);
  diffCar.AssignKs(Kri,Kle,K);
  diffCar.AssignTsamp(tsamp);
  diffCar.Indebugmode(true);
  diffCar.Tult=0;
}

void loop() {
  diffCar.DifferencialCarMovement();
}