#include <Encoder.h>
#include <PID_v1.h>
int RB = 12;
int RF = 11;
int pwmR = 10;
int LB = 8;
int LF = 7;
int pwmL = 9;

double target=0;
double input1;
double output;
double Kp=0, Ki=0, Kd=0;
double temppwmL,temppwmR =0;

Encoder myEnc1(2, 4);
Encoder myEnc2(3, 5);
long newPosL = 0;
long newPosR = 0;

char dir;
int d,m;
int f;
char ins;
int a;
int r;
int k=0;

PID myPid(&input1, &output, &target, Kp, Ki, Kd, DIRECT);

char di[100];
int mag[100];


void setup() {
  Serial.begin(9600);
  pinMode(LF, OUTPUT);
  pinMode(LB, OUTPUT);
  pinMode(pwmL, OUTPUT);
  pinMode(RF, OUTPUT);
  pinMode(RB, OUTPUT);
  pinMode(pwmR, OUTPUT);
  input();
  printarray();
  myPid.SetMode(AUTOMATIC);
  myPid.SetTunings(Kp,Ki,Kd);
  logic();
}

int n1=0;

void input()
{
  Serial.println("Enter no. of instructions");
  while (Serial.available() == 0) {}
  n1 = Serial.parseInt();
  for(int i=0;i<n1;i++)
  { 
    Serial.println("Enter Dir : ");
    while (Serial.available() == 0) {}
    char n = (char)Serial.read();
   if (n == 'F' || n == 'f' || n == 'B' || n == 'b' || n == 'L' || n == 'l' || n == 'R' || n == 'r') 
   {
    di[i] = n;
    Serial.println("Enter mag : ");
    while (Serial.available() == 0) {}
    m = Serial.parseInt();
    mag[i] = m;
   }
   if(n=='.')
   {
    break;
   }
 }
}

void printarray()
{ 
 for(int i=0;i<n1;i++)
 {
   Serial.print(di[i]);
   Serial.println(mag[i]);
 }
}

void loop() {

}

void logic()
{
    for (int q = 0; q < n1; q++) 
    {
      ins = di[q];
      if (ins == 'F' || ins == 'f' || ins == 'B' || ins == 'b') 
      {
        if (ins == 'F' || ins == 'f')
         { 
          f = 1;
         }
        else if (ins == 'B' || ins == 'b')
         { 
          f = 0;
         }
        d = mag[q];
        MOVE(f, d);
      }
      else if (ins == 'R' || ins == 'r' || ins == 'L' || ins == 'l') 
      {
        if (ins == 'R' || ins == 'r')
          {
            r = 1;
          }
        else if (ins == 'L' || ins == 'l')
          {
            r = 0;
          }
        a = mag[q];
        TURN(r, a);
      }
    }
}
void MOVE(int a, int b) {
  target = b * 1455;
  if (a == 1) {
    while ((newPosL <= target) || (newPosR <= target)) {
      PaRenc();
      myPid.Compute();
      temppwmL,temppwmR = map(output, 0,target, 40,255);
      SetMotor(RF, RB, LF, LB, 1, 1, pwmR, temppwmR, pwmL, temppwmL);
      if((newPosL>target)||(newPosR>target))
      {
        break;
      }
    }
    if(((newPosL>target)||(newPosR>target)))
    {
     SetMotor(RF, RB, LF, LB, 0, 0, pwmR, 0, pwmL, 0);
     //delay(100);
     //Serial.println("hello");
     SetEncVar();
    }
  }
  if (a == 0) {
    long tarl=-target;
    long tarr=-target;
    while ((newPosL >= tarl) || (newPosR >= tarr)) {
      PaRenc();
      myPid.Compute();
      temppwmL,temppwmR = map(output, 0,target, 40,255);
      SetMotor(RF, RB, LF, LB, -1, -1, pwmR, temppwmR, pwmL, temppwmL);
      if((newPosL<tarl)||(newPosR<tarr))
      {
        break;
      }
    }
    if((newPosL<tarl)||(newPosR<tarr))
    {
      SetMotor(RF, RB, LF, LB, 0, 0, pwmR, 0, pwmL, 0);
      //Serial.println("hello again");
      SetEncVar();
    }
  }
}
void TURN(int a, int b) {
  int target = b * 1600;
  if (a == 1) {
    Serial.println("im here");
    long tarL = target;
    long tarR = -target;
    while ((newPosR >= tarR)||(newPosL<= tarL)) {
      //Serial.println("yo");
      PaRenc();
      myPid.Compute();
      temppwmL,temppwmR = map(output, 0,target, 40,255);
      SetMotor(RF, RB, LF, LB, -1, -1, pwmR, temppwmR, pwmL, temppwmL);
      if((newPosR < tarR)||(newPosL>tarL))
      {
        //Serial.println("hello");
        break;
      }
    }
    if ((newPosR < tarR)||(newPosL>tarL)) {
      //Serial.println("hello again");
      SetMotor(RF, RB, LF, LB, 0, 0, pwmR, 0, pwmL, 0);
      SetEncVar();
    }
  }
  if (a == 0) {
    long tarL = -target;
    long tarR = target;
    while ((newPosR <= tarR)||(newPosL>=tarL)) {
      PaRenc();
      myPid.Compute();
      temppwmL,temppwmR = map(output, 0,target, 40,255);
      SetMotor(RF, RB, LF, LB, 1, -1, pwmR, temppwmR, pwmL, temppwmL);
      if((newPosR > tarR)||(newPosL<tarL))
      {
        break;
      }
    }
    if ((newPosR > tarR)||(newPosL<tarL)) {
      SetMotor(RF, RB, LF, LB, 0, 0, pwmR, 0, pwmL, 0);
      SetEncVar();
    }
  }
}
void SetEncVar() {
  myEnc1.write(0);
  myEnc2.write(0);
}
void SetMotor(int inPinR1, int inPinR2, int inPinL1, int inPinL2, int w1, int w2, int pR, int vR, int pL, int vL) {
  if (w1 == 1) {
    digitalWrite(inPinR1, HIGH);
    digitalWrite(inPinR2, LOW);
    analogWrite(pR, vR);
  } else if (w1 == -1) {
    digitalWrite(inPinR1, LOW);
    digitalWrite(inPinR2, HIGH);
    analogWrite(pR, vR);
  } else if (w1 == 0) {
    digitalWrite(inPinR1, LOW);
    digitalWrite(inPinR2, LOW);
    analogWrite(pR, 0);
  }
  if (w2 == 1) {
    digitalWrite(inPinL1, HIGH);
    digitalWrite(inPinL2, LOW);
    analogWrite(pL, vL);
  } else if (w2 == -1) {
    digitalWrite(inPinL1, LOW);
    digitalWrite(inPinL2, HIGH);
    analogWrite(pL, vL);
  } else if (w2 == 0) {
    digitalWrite(inPinL1, LOW);
    digitalWrite(inPinL2, LOW);
    analogWrite(pL, 0);
  }
}
void PaRenc() {
  newPosL = myEnc1.read();
  newPosR = myEnc2.read();
  input1 = newPosL;
  Serial.print(newPosL);
  Serial.print(" - ");
  Serial.println(newPosR);
}