#include <RobotParams.h>
#include <OdometricLocalizer.h>
#include "Arduino.h"
#include <digitalWriteFast.h>  
#define PI 3.14159265 
#define TwoPI 6.28318531

RobotParams _RobotParams = RobotParams();       // Class to Initialize robot parameters :  wheelDiameter , countsPerRevolution , trackWidth 
OdometricLocalizer _OdometricLocalizer(&_RobotParams);   // Class to compute X ,Y , Angle of robot 

int ENA=8;    // SpeedPinA  
int ENB=9;    // SpeedPinB 

int IN1=48;    // RightMotor1
int IN2=49;    // RightMotor2
 
int IN3=50;    // LeftMotor1
int IN4=51;    // LeftMotor2

float P= 0.2;

long difference ;

// Quadrature encoders
// Left encoder
#define encoder0PinA 18  // interrupt 0

#define encoder0PinB 2  // interrupt 5

volatile signed int encoder0Pos = 0;



#define encoder1PinA 3  // interrupt 1

#define encoder1PinB 19  // interrupt 4

volatile signed int encoder1Pos = 0;



int RightWheel,LeftWheel;
int  Difference ;  
        
bool _IsInitialized = false;


void setup() {


Serial.begin(115200);

//Motors Pins
 pinMode(ENA,OUTPUT);
 pinMode(ENB,OUTPUT);
 pinMode(IN1,OUTPUT);
 pinMode(IN2,OUTPUT);
 pinMode(IN3,OUTPUT);
 pinMode(IN4,OUTPUT);
 digitalWrite(ENA,HIGH);    //enable motorA
 digitalWrite(ENB,HIGH);    //enable motorB
 

 //Encoders Pins
 pinMode(encoder0PinA, INPUT); 
 pinMode(encoder0PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(5, doEncoderA, CHANGE);
// encoder pin on interrupt 5 (pin 3)

  attachInterrupt(0, doEncoderB, CHANGE);  
  
pinMode(encoder1PinA, INPUT); 
pinMode(encoder1PinB, INPUT); 
// encoder pin on interrupt 0 (pin 2)

 attachInterrupt(1, doEncoderA1, CHANGE);
// encoder pin on interrupt 5 (pin 3)

attachInterrupt(4, doEncoderB1, CHANGE); 
  
  
 InitializeDriveGeometry();
 RequestInitialization();

}



void loop() 
{


doEncoderA();
doEncoderB();
doEncoderA1();
doEncoderB1();


 _OdometricLocalizer.Update(encoder0Pos, encoder1Pos);     // Passing Encoders Counts to get X , Y , Heading
 
   float P= 2;
long difference =long (encoder1Pos - encoder0Pos);
if (difference > 0)
{LeftWheel -= int(P * difference);
RightWheel += int(P * difference);}
else
{LeftWheel += int(P * difference);
RightWheel -= int(P * difference);}

 moveForward();



}


void moveForward() 
{

    LeftWheel=20;
    RightWheel=20;

 //  Converting To PWM
   toPWM (RightWheel,LeftWheel);
   
   
}


void doEncoderA(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder0PinB) == HIGH) {   
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
 // Serial.println (encoder0Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder0PinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder0PinA) == HIGH) {  
      encoder0Pos = encoder0Pos + 1;         // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder0PinA) == LOW) {   
      encoder0Pos = encoder0Pos + 1;          // CW
    } 
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
}


void doEncoderA1(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder1PinA) == HIGH) { 
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder1PinB) == HIGH) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
//  Serial.println (encoder1Pos, DEC);          
  // use for debugging - remember to comment out
}

void doEncoderB1(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) {  
      encoder1Pos = encoder1Pos + 1;         // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else { 
    // check channel B to see which way encoder is turning  
    if (digitalRead(encoder1PinA) == LOW) {   
      encoder1Pos = encoder1Pos + 1;          // CW
    } 
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
}





void RequestInitialization()
{
    _IsInitialized = true;

    if (!_RobotParams.IsInitialized)
    {
      _IsInitialized = false;


}}

void InitializeDriveGeometry()
{
  float wheelDiameter =6.5 ; // in CM
  float trackWidth =18.9; // in CM
  int countsPerRevolution =333.3; //
  
  _RobotParams.Initialize(wheelDiameter, trackWidth, countsPerRevolution);
}







// Coverting to PWM

void toPWM (float Wr,float Wl)
{
     RightWheel=Wr;
     LeftWheel=Wl;
       //Serial.println( "PMW");
    int rightPWM, leftPWM;  
  if (RightWheel > 0) {
    //forward
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
    
  }  else if (RightWheel < 0){
    //reverse
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  }
  
  if (RightWheel == 0) {
   rightPWM = 0;
   analogWrite(ENA, rightPWM);
  } else {
    rightPWM = map(abs(RightWheel), 1, 100, 1, 255);

    analogWrite(ENA, rightPWM);
  }
  
  
  
     if (LeftWheel > 0) {
     //forward
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  }  else if (LeftWheel < 0) {
     //reverse
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);}
  
  if (LeftWheel == 0) {
    leftPWM = 0;
    analogWrite(ENB, leftPWM);
  } else {
    leftPWM = map(abs(LeftWheel), 1, 100, 1, 255);

    analogWrite(ENB, leftPWM);
  }
  
  
}