#include <RotaryEncoder.h>
#include <SPI.h>
#include "RF24.h"
#include <TimerOne.h>
#include <math.h>
#include <PID_v1.h>

void Odometry();

//  Variables for Odometry
float Wheel_L_size = 78.7;
float Wheel_R_size = 78.7;
//jak nie dojeżdża to zwiększyc wartosc Encoder_scope_ , jak przejeżdza to zmniejszyć
//float Encoder_scope_L = 970.5;
//float Encoder_scope_R = 960.5;
float Encoder_scope_L = 960;
float Encoder_scope_R = 960;
float Distance_between_wheels = 137;

//  Definition of left and right encoder PINs
RotaryEncoder encoder_L(A4, A5);
RotaryEncoder encoder_R(A3, A2);

//  RF communication PIN declarations
RF24 radio(7,8);

//  PID controller
double kp_L = 5 , ki_L = 1 , kd_L = 0.01 ,input_L = 0, output_L = 0, setpoint_L = 0;   // modify kp, ki and kd for optimal performance
double kp_R = 5 , ki_R = 1 , kd_R = 0.01 ,input_R = 0, output_R = 0, setpoint_R = 0;   // modify kp, ki and kd for optimal performance
PID myPID_L(&input_L, &output_L, &setpoint_L, kp_L, ki_L, kd_L, DIRECT);  // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'
PID myPID_R(&input_R, &output_R, &setpoint_R, kp_R, ki_R, kd_R, DIRECT);  // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'


//  Motor left PIN declaration
int dir1PinA = 2;
int dir2PinA = 9;
int speedPinA = 5; // Needs to be a PWM pin to be able to control motor speed == 5

//  Motor right PIN declaration
int dir1PinB = 4;
int dir2PinB = 10;
int speedPinB = 3; // Needs to be a PWM pin to be able to control motor speed == 3

//  Motors rotate speed
int Motors_Rotate_Speed = 20;

//  Motors line speed
int Motors_Line_Speed = 20;

//  Innterrupt variables( Odometry and motors PID/Speed variables )
float   X_Global = 400;
float   Y_Global = 400;
//float   X_Global = 0;
//float   Y_Global = 0;
float  Angle_Global=0;
float Angle_Desired = 0;
float Angle_Desired_temp=Angle_Desired;
boolean Flag_Angle=true;
long int ticks2_L = 0;
long int ticks2_R = 0;
long temp_L = 0;
long temp_R = 0;
int Sp_L=0;
int Sp_R=0;


//  Function initializing RF24 module
void init_NRF(){
  radio.begin();
  radio.setPALevel(RF24_PA_LOW);
  radio.setDataRate(RF24_1MBPS);
  radio.setCRCLength(RF24_CRC_16);
  radio.setPayloadSize(32);
  radio.setChannel(1);
  radio.setAutoAck(true);
  radio.openWritingPipe(0x314e6f6465);
  radio.openReadingPipe(1,0x324e6f6465);
  radio.startListening();
}

//  Function setting up motors speed
void motors_L_R(int Speed_L,int Speed_R){

  Sp_L=Speed_L;
  Sp_R=Speed_R;

}

//  Function calculating global X and Y position based on encoder values and also actually setting up motors speed
//  This function is called every 0,0015 sec
void Odometry(){

    if (Sp_L >= 0){
      temp_L+=Sp_L;
    }
    else if (Sp_L < 0){
      temp_L-=-Sp_L;
    }

    if (Sp_R >= 0){
      temp_R+=Sp_R;
    }
    else if (Sp_R < 0){
      temp_R-=-Sp_R;
    }

    setpoint_R=temp_R/99.3;                             // modify division to fit motor and encoder characteristics
    setpoint_L=temp_L/100;                             // modify division to fit motor and encoder characteristics

    input_R=encoder_R.getPosition();
    input_L=encoder_L.getPosition();

    myPID_R.Compute();                                    // calculate new output
    myPID_L.Compute();                                    // calculate new output


      if (output_L >= 0){
        digitalWrite(dir1PinA, HIGH);
        digitalWrite(dir2PinA, LOW);
        analogWrite(speedPinA,output_L);
      }
      else {
        digitalWrite(dir1PinA, LOW);
        digitalWrite(dir2PinA, HIGH);
        analogWrite(speedPinA,-output_L);
      }

      if (output_R >= 0){
        digitalWrite(dir1PinB, HIGH);
        digitalWrite(dir2PinB, LOW);
        analogWrite(speedPinB, output_R);
      }
      else {
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      analogWrite(speedPinB,-output_R);
      }


    long int ticks1_L=input_L;
    long int ticks1_R=input_R;

    long int L_ticks=ticks1_L - ticks2_L;
    long int R_ticks=ticks1_R - ticks2_R;

    ticks2_L=ticks1_L;
    ticks2_R=ticks1_R;

    float L_mm= (float)L_ticks * ( ( Wheel_L_size * PI ) / Encoder_scope_L);
    float R_mm= (float)R_ticks  *( ( Wheel_R_size * PI ) / Encoder_scope_R);

    float MM=( L_mm + R_mm ) / 2;

      if(Flag_Angle){
    Angle_Global  += ((L_mm - R_mm)/ Distance_between_wheels )*(180.0/PI);


    while (  Angle_Global < 0 ||  Angle_Global  >= 360){
      if (  Angle_Global  < 0){
         Angle_Global  =  Angle_Global  + 360;
      }
      if ( Angle_Global  >= 360){
         Angle_Global  =  Angle_Global  - 360;
      }
    }

while (  Angle_Desired_temp < 0 ||  Angle_Desired_temp  >= 360){
      if (  Angle_Desired_temp  < 0){
         Angle_Desired_temp  =  Angle_Desired_temp  + 360;
      }
      if ( Angle_Desired_temp  >= 360){
         Angle_Desired_temp  =  Angle_Desired_temp  - 360;
      }
    }




      }else{
   Y_Global += MM*cos( Angle_Global /(180/PI));
   X_Global += MM*sin( Angle_Global /(180/PI));
      }
}


void setup() {

//  Initialize serial communication @ 115200 baud:
  Serial.begin(57600);

  init_NRF();

  Timer1.initialize(1500);
  Timer1.attachInterrupt(Odometry);

  myPID_L.SetMode(AUTOMATIC);
  myPID_L.SetSampleTime(1);
  myPID_L.SetOutputLimits(-255, 255);

  myPID_R.SetMode(AUTOMATIC);
  myPID_R.SetSampleTime(1);
  myPID_R.SetOutputLimits(-255, 255);

  pinMode(dir1PinA,OUTPUT);
  pinMode(dir2PinA,OUTPUT);
  pinMode(speedPinA,OUTPUT);
  pinMode(dir1PinB,OUTPUT);
  pinMode(dir2PinB,OUTPUT);
  pinMode(speedPinB,OUTPUT);

  // You may have to modify the next 2 lines if using other pins than A2 and A3
  PCICR |= (1 << PCIE1);    // This enables Pin Change Interrupt 1 that covers the Analog input pins or Port C.
  PCMSK1 |= (1 << PCINT10) | (1 << PCINT11) |(1 << PCINT12)|(1 << PCINT13);  // This enables the interrupt for pin 2, 3, 4 and 5.

}

// The Interrupt Service Routine for Pin Change Interrupt 1
// This routine will only be called on any signal change on A2, A3, A4 and A5: exactly where we need to check.

ISR(PCINT1_vect) {
  encoder_L.tick(); // just call tick() to check the state.
  encoder_R.tick();
}


void gotoXY( int X_Desired, int Y_Desired, int Speed  ){

  Flag_Angle=true; //uruchom flagę przy ruchu
  Motors_Rotate_Speed = Speed;
  Motors_Line_Speed = Speed;

  float X_Diff = ( X_Desired -  X_Global );
  float Y_Diff = ( Y_Desired - Y_Global );

//Calculating desired angle based on global angle

  Angle_Desired  = atan2( X_Diff , Y_Diff )*(180/PI);

  if (  Angle_Desired  < 0){
     Angle_Desired  = 360 + Angle_Desired ;
  }

  float add, subtract;

//////Rotating based on desired angle and global angle/////

  if (  Angle_Desired  >  Angle_Global  ) {
    add = Angle_Desired - Angle_Global ;
    subtract = Angle_Global + 360 - Angle_Desired ;
  }
   else{
    add = 360 -  Angle_Global  +  Angle_Desired ;
    subtract =  Angle_Global  -  Angle_Desired ;
  }

Angle_Desired_temp=Angle_Desired;

 if ( subtract > add ){
  if(add>=90){
  Angle_Desired_temp+=6;
  }
 }
 if ( subtract < add ){
  if(subtract>=90){
  Angle_Desired_temp-=4;
  }
 }


    while ( abs(  Angle_Desired_temp  -  Angle_Global  ) > 0.6){

       Serial.print(Angle_Desired_temp);
       Serial.print("  ");
       Serial.print(Angle_Global);
       Serial.print("                        ");

      if ( subtract > add ){

        Serial.println(Y_Global);
        motors_L_R( Motors_Rotate_Speed, -Motors_Rotate_Speed );
      }
      else {
        Serial.println(Y_Global);
        motors_L_R( -Motors_Rotate_Speed , Motors_Rotate_Speed );
      }
   }
        Serial.println(Y_Global);
        motors_L_R(0,0);

 if ( subtract > add ){
  if(add>=90){
  Angle_Global-=6;
  }
 }
 if ( subtract < add ){
  if(subtract>=90){
  Angle_Global+=4;
  }
 }

        Flag_Angle=false; //koniec obrotu uruchom flage


//////////////Movement/////////////


 if (  Angle_Desired  != 0 &&  Angle_Desired  != 90 &&  Angle_Desired  != 180 &&  Angle_Desired  != 270){

  if ( X_Desired >=  X_Global && Y_Desired >= Y_Global ){                    // Angle_Desired ... 0 --> 90
    while (  X_Global <= X_Desired || Y_Global <= Y_Desired ){
        Serial.println(Y_Global);
        motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

  else if ( X_Desired >=  X_Global && Y_Desired < Y_Global ){                // Angle_Desired ... 90 --> 180
    while (  X_Global <= X_Desired || Y_Global >= Y_Desired ){


         Serial.print(X_Global);
         Serial.print("       ");
        Serial.println(Y_Global);

        motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

  else if ( X_Desired <  X_Global && Y_Desired < Y_Global ){                 // Angle_Desired ... 180 --> 270
    while (  X_Global >= X_Desired || Y_Global >= Y_Desired ){

        Serial.println(Y_Global);
        motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

 else if ( X_Desired <  X_Global && Y_Desired >= Y_Global ){                  // Angle_Desired ... 270 --> 360
    while (  X_Global >= X_Desired || Y_Global <= Y_Desired ){

        Serial.println(Y_Global);
        motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }


 }
 else {

  if ( Angle_Desired  == 0){                                                  // Angle_Desired == 0
    while ( Y_Global < Y_Desired ){
      Serial.print(X_Global);
      Serial.print("     ");
       Serial.println(Y_Global);
       motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

  else if ( Angle_Desired  == 90){                                            // Angle_Desired == 90
    while(  X_Global < X_Desired ){
       Serial.println(Y_Global);
       motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

  else if ( Angle_Desired  == 180){                                           // Angle_Desired == 180
     while( Y_Global > Y_Desired ){
       Serial.println(Y_Global);
       motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

  else if ( Angle_Desired  == 270){                                           // Angle_Desired == 270
     while(  X_Global > X_Desired ){
       Serial.println(Y_Global);
       motors_L_R( Motors_Line_Speed , Motors_Line_Speed );
    }
  }

 }
  ///////////END  --> STOP MOTORS////////////////
        Serial.println(Y_Global);
        motors_L_R(0,0);
        X_Global=X_Desired;
        Y_Global=Y_Desired;
        Angle_Global=Angle_Desired;
  }

void loop() {
gotoXY(500,500,20);
gotoXY(1000,500,20);
gotoXY(1000,1000,20);
/*gotoXY(1500,3500,20);
gotoXY(2000,3500,20);
gotoXY(2500,3500,20);
gotoXY(2500,3000,20);
gotoXY(2500,2500,20);
*/
  /*
gotoXY(0,400,20);
gotoXY(800,400,20);
gotoXY(600,1000,20);
gotoXY(200,200,20);
gotoXY(1000,1200,20);
gotoXY(1300,1000,20);
*/
  /*
gotoXY(200,200,20);
gotoXY(200,500,20);
gotoXY(0,700,20);
gotoXY(300,700,20);
gotoXY(600,300,20);
gotoXY(1500,2000,20);
  */

  /*
gotoXY(1500,1000,20);
gotoXY(1000,1000,20);
gotoXY(1000,1500,20);
*/
/*
while (encoder_L.getPosition()<=Encoder_scope_L*15){
    Serial.println(Y_Global);
motors_L_R(20,20);

}
*/
//motors_L_R(0,0);

delay(100000);
}