#include <Servo.h>

  //Set up pins
  int motorSpeedPin_1 = 3;    // pin for motor 1  demand
  int motorDirPin_1 = 12;     // pin for motor 1 direction
  int motorBrakePin_1 = 9;    // pin for motor 1 brake
  int motorSpeedPin_2 = 11;   // pin for motor 2  demand
  int motorDirPin_2 = 13;     // pin for motor 2 direction
  int motorBrakePin_2 = 8;    // pin for motor 2 brake
  int encPhaseA_1 = 18;       // pin for encoder 1 phase A
  int encPhaseB_1 = 19;       // pin for encoder 1 phase B
  int encPhaseA_2 = 20;       // pin for encoder 2 phase A
  int encPhaseB_2 = 21;       // pin for encoder 2 phase B
  int blackButtonPin = A9;    // pin for black button
  int greenButtonPin = A10;   // pin for green button
  int potPin = A8;            // pin for rotary pot
  int linMotorPin = 6;          // pin for linear motor
  Servo linMotor;

 
  //Set up variables
  volatile int encCount_1 = 0;            // Counts made for encoder 1
  volatile int encCount_2 = 0;            // Counts made for encoder 2
  float encCountPerSec_1 = 0;             // Encoder 1 angular velocity in counts/s
  float encCountPerSec_2 = 0;             // Encoder 2 angular velocity in counts/s
  float motorDemand_1 = 0;                // Motor 1 demand, -100% to +100%
  float motorDemand_2 = 0;                // Motor 2 demand, -100% to +100%
  int motorCommand_1 = 0;                 // Motor 1 signal, 0 to 255
  int motorCommand_2 = 0;                 // Motor 2 signal, 0 to 255
  volatile int linMotorPos = 0;           // Linear motor pulse length
  int potValue;                           // Potentiometer value.
  float maxPot = 270;                     // Used to calculate potentimeter value.
  float potBias;                          // Potentiometer bias -1 to +1
  unsigned long timePassed = 0;           // Time in msec since buttom press
  unsigned long timeStarted;              // Time is msec when button pressed.
  unsigned long prevTime_1 = 0;           // Time in msec of the last loop
  unsigned long prevTime_2 = 0;           // Time in msec of the last loop   
  int buttonPressed = 0;                  // Which button Pressed, 0 = neither, 1 = black, 2 = green
  int encCountPerRev = 512;               // Number of counts per revolution
  int velocity_1 = 0;                   // Encoder 1 Wheel Speed
  int velocity_2 = 0;                   // Encoder 2 Wheel Speed
  int avg_distanceTravelled = 0;        // Average distance travelled by the buggy
  int distanceTravelled_1 = 0;          // Calculated distance by encoder 1
  int distanceTravelled_2 = 0;          // Calculated distance by encoder 2
  int wheelDiameter = 176;            // Used to calculate wheel velocity and distance travelled
  bool dropZone = false;                  // Intialise payload logic            
  static int dt = 0;                      // Controller frequency in msecs
  float prevError_1 = 0;                  // Initalise previous  error values
  float prevError_2 = 0;                  // Initalise previous  error values
  int targetVelocity = 70;             // Initalise target velocity of the wheels
  float controlSignal_1 = 0;              // Intialise the Control signal value
  float controlSignal_2 = 0;              // Intialise the Control signal value
  int outMax = 100;                       // Intialise integral output values
  int outMin = -100;                      // Intialise integral output values
 



  void setup() {
    // Setup the pins and variables for the programme and define the interupts for the encoder.
    // Setup Motor 1
    pinMode(motorDirPin_1, OUTPUT);      // Initiates Motor Channel A pin
    pinMode(motorBrakePin_1, OUTPUT);    // Initiates Brake Channel A pin
    // Setup Motor 2
    pinMode(motorDirPin_2 , OUTPUT);     // Initiates Motor Channel A pin
    pinMode(motorBrakePin_2, OUTPUT);    // Initiates Brake Channel A pin
    // Setup Encoder 1
    pinMode(encPhaseA_1, INPUT);         // Initiates Encoder 1 Phase A
    pinMode(encPhaseB_1, INPUT);         // Initiates Encoder 1 Phase B
    // Setup Encoder 2
    pinMode(encPhaseA_2, INPUT);         // Initiates Encoder 2 Phase A
    pinMode(encPhaseB_2, INPUT);         // Initiates Encoder 2 Phase B
    // Setup Linear Motor Control
    linMotor.attach(6);
    linMotor.writeMicroseconds(2000);    // Change to 2000 to initialise fully extended
    delay(1000);                         // Initiates the linear motor control pin
    // Setup Button box
    pinMode(blackButtonPin, INPUT);      // Initiates Black Button
    pinMode(greenButtonPin, INPUT);      // Initiates Green Button
    pinMode(potPin, INPUT);              // Initiates Potentiometer

    attachInterrupt(digitalPinToInterrupt(encPhaseA_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase A
    attachInterrupt(digitalPinToInterrupt(encPhaseB_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase B
    attachInterrupt(digitalPinToInterrupt(encPhaseA_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase A
    attachInterrupt(digitalPinToInterrupt(encPhaseB_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase B

    Serial.begin(9600);               //  setup serial

    Serial.print(" 1. System Start ");
  }

  // Main loop
  void loop(){                                
  digitalWrite(motorBrakePin_1, HIGH);          //Engage the Brake for Channel A
  digitalWrite(motorBrakePin_2, HIGH);          //Engage the Brake for Channel B
  delay(3000);
  // Initial buttonPressed variable.
  buttonPressed = 0;                         //Set button pressed to 0

  // Waits in this section until button pressed.
    while (buttonPressed == 0) {                 //Stay in while loop until a button is pressed.
      if (analogRead(blackButtonPin) < 2) {      //Check for the black button to be pressed.
        buttonPressed = 1;                       //Set button pressed to 1
      }
      else if (analogRead(greenButtonPin) < 2) { //Check for the green button to be pressed.
        buttonPressed = 2;                       //Set button pressed to 2
      }
      else {                                     //Else no button pressed.
        buttonPressed = 0;                       //Set button pressed to 0
      }
        delay(50);                               //Wait 50ms before checking button status again.
    }
   
    Serial.print(" 2. Button Pressed" );

    // Code about to start running, intialise some variables ready for the running loop.
    potValue = analogRead(potPin);              // Read potentiometer value
    potBias = (potValue-260)/maxPot;            // Calculate potBias
    encCount_1 = 0;                             // Initialise encoder 1
    encCount_2 = 0;                             // Initialise encoder 2
    timeStarted = millis();                     // Get the time in sec at which the button was pressed.
    timePassed = millis() - timeStarted;        // Initialise timePassed to 0

    if (buttonPressed == 2) {             //Black button pushed for simple controller
     while (avg_distanceTravelled < 11500) {
     
      distanceTravelled_1 = -1*(encCount_1/encCountPerRev)*wheelDiameter*PI;           // Calculation for the distance travelled by each wheel
    distanceTravelled_2 = (encCount_2/encCountPerRev)*wheelDiameter*PI;          // Calculation for the distance travelled by each wheel
    avg_distanceTravelled = (distanceTravelled_1 + distanceTravelled_2)/2;          // Average distance travelled between both wheels

      if (avg_distanceTravelled < 5500) {                  // Motor control for the intial phase
        motorDemand_1 = -80;
        motorDemand_2 = 80;
      }
      else if (avg_distanceTravelled >= 5500 && avg_distanceTravelled <= 6900) {      //  Motor control during Payload drop phase
         motorDemand_1 = -20;
        motorDemand_2 = 20;
      }
      else if (avg_distanceTravelled > 6900 && avg_distanceTravelled <= 11500) {      // Motor control for final phase
        motorDemand_1 = -80;
        motorDemand_2 = 80;
      }
      else { 
        motorDemand_1 = 0;
        motorDemand_2 = 0;
      }
     encVel_1();                              // call encoder velcoity functions
      encVel_2();
      
      motorControl();                         // call motor control function
      
      payloadDrop();

      Serial.print(" motor demand 1 ");
      Serial.print(motorDemand_1);
      Serial.print(" motor demand 2 ");
      Serial.print(motorDemand_2);
      Serial.print(" distance travelled ");
      Serial.print(avg_distanceTravelled);
      Serial.print(" Wheel speed 1 ");
      Serial.print(velocity_1);
      Serial.print(" Wheel speed 2 ");
      Serial.print(velocity_2);
      Serial.println();
     }     
      motorCommand_1 = 0;  // Slow down buggy once it reaches the finish line
      motorCommand_2 = 0;
    }

   if (buttonPressed == 1) {                //Green putton pushed run PID Controller
 
  // This is the run loop, you set the condition for the while loop to keep running
  while (avg_distanceTravelled < 11500) {     //Add your condition for the loop to continue running here, that is when should your controller stop?
      timePassed = millis() - timeStarted;

    distanceTravelled_1 = -1*(encCount_1/encCountPerRev)*wheelDiameter*PI;
    distanceTravelled_2 = (encCount_2/encCountPerRev)*wheelDiameter*PI;
    avg_distanceTravelled = (distanceTravelled_1 + distanceTravelled_2)/2;

  // Place your run code here:
      encVel_1();
      encVel_2();
    
    //Check what the target velcoity should be depending where the buggy is
     targetVelocitycheck();
   
   //Check if drop zone  
    payloadDrop();

    //Calculate control signal
      calculatePID_1();
      calculatePID_2();

      // This code below is used to output the demand to the motors, it should not need to be changed.
      motorCommand_1 = round(motorDemand_1*2.55);  //Coverts -100 to 100 to -255 to 255
      motorCommand_2 = round(motorDemand_2*2.55);  //Coverts -100 to 100 to -255 to 255
      motorCommand_1 = abs(motorCommand_1);         //Makes motorCommand_1 a positive value.
      motorCommand_2 = abs(motorCommand_2);         //Makes motorCommand_2 a positive value.
      if (motorDemand_1 > 0) {                      //Sets motor 1 direction.
      digitalWrite(motorDirPin_1, HIGH);            //Establishes forward direction of Channel A
      }
      else {
        digitalWrite(motorDirPin_1, LOW);           //Establishes reverse direction of Channel A
      }
      digitalWrite(motorBrakePin_1, LOW);           //Disengage the Brake for Channel A
      analogWrite(motorSpeedPin_1, motorCommand_1); //Spins the motor on Channel A at motor demand
     
      if (motorDemand_2 > 0) {                      //Sets motor 2 direction.
      digitalWrite(motorDirPin_2, HIGH);            //Establishes forward direction of Channel B
      }
      else {
        digitalWrite(motorDirPin_2, LOW);           //Establishes reverse direction of Channel B
      }
      digitalWrite(motorBrakePin_2, LOW);           //Disengage the Brake for Channel B
      analogWrite(motorSpeedPin_2, motorCommand_2); //Spins the motor on Channel B at motor demand
     
      delay(2);
     
      // For debugging, remove once code is complete
      Serial.print("Time passed: ");
      Serial.print(timePassed);
      /*Serial.print(" Encoder 1: ");
      Serial.print(encCount_1);
      Serial.print(" Encoder 2: ");
      Serial.print(encCount_2);
      Serial.print(" Encoder count per sec 1: ");
      Serial.print(encCountPerSec_1);
      Serial.print(" Encoder count per sec 2: ");
      Serial.print(encCountPerSec_2);*/
      Serial.print(" MotorDemand_1: ");
      Serial.print(motorDemand_1);
      Serial.print(" MotorDemand_2: ");
      Serial.print(motorDemand_2);
      Serial.print(" Average Distance Travelled ");
      Serial.print(avg_distanceTravelled);
      Serial.print(" Wheel 1 Distance ");
      Serial.print(distanceTravelled_1);
      Serial.print(" Wheel 2 Distance ");
      Serial.print(distanceTravelled_1);
      Serial.print(" Wheel speed 1 ");
      Serial.print(velocity_1);
      Serial.print(" Wheel speed 2 ");
      Serial.print(velocity_2);
      Serial.println();

    }
  }   motorCommand_1 = 0;  // Slow down buggy once it reaches the finish line
      motorCommand_2 = 0; 
  }

  // Function for calculating encoder velocity 1
  void encVel_1() {
    int measurementWidth = 4;             // Number of counts that need to pass before determining velocity
    static int timeElapsed = 0;           //  Time elasped since last velocity  given
    static int refTime;                   // Current reference time in ms
    static float encChange =0;            // Change in encoder count since last velocity measure
    static int prevEncCount = 0;            // Encoder value at last velocity measure
    static int encChangeAbs = 0;          // Absolute change in encoder count
    static int storedTime = millis();     // Time at previous velocity measure in ms
    refTime = millis();                   // Update reference time in ms
    timeElapsed = refTime-storedTime;     // Calcualte the time that has elapsed since last velocity measure
    encChange = encCount_1-prevEncCount;    // Calculate the change in counts since previous velocity measure
    if (timeElapsed >= 400) {             // If time is over 400ms since last velocity measure, assume zero velocity
      encCountPerSec_1 = 0;
      prevEncCount = encCount_1;             // set previous encoder Count to the current encoder count and storedTime to current time in ms
      storedTime = refTime;
    }
      else {                                               // If the time elapsed is short...
        encChangeAbs = abs(encChange);                     // Get absolute encoder change.
      if (encChangeAbs >= measurementWidth) {              // If absolute encoder change is greater or equal to measurement width...
        encCountPerSec_1 = 1000*(encChange/timeElapsed);   // Calculate counts per second to give velocity measure
        storedTime = refTime;                              // set previous encoder Count to the current encoder count and storedTime to current time in ms
        prevEncCount = encCount_1;
    }
    }
    velocity_1 = (encCountPerSec_1 / encCountPerRev)*60;  //Calculate the rpm of the wheel 1
  }

  // Function for calculating encoder velocity 2
  void encVel_2() {
    int measurementWidth = 4;             // Number of counts that need to pass before determining velocity
    static int timeElapsed = 0;           //  Time elasped since last velocity  given
    static int refTime;                   // Current reference time in ms
    static float encChange =0;            // Change in encoder count since last velocity measure
    static int prevEncCount = 0;            // Encoder value at last velocity measure
    static int encChangeAbs = 0;          // Absolute change in encoder count
    static int storedTime = millis();     // Time at previous velocity measure in ms
    refTime = millis();                   // Update reference time in ms
    timeElapsed = refTime-storedTime;     // Calcualte the time that has elapsed since last velocity measure
    encChange = encCount_2-prevEncCount;    // Calculate the change in counts since previous velocity measure
    if (timeElapsed >= 400) {             // If time is over 400ms since last velocity measure, assume zero velocity
      encCountPerSec_2 = 0;
      prevEncCount = encCount_2;             // set previous encoder Count to the current encoder count and storedTime to current time in ms
      storedTime = refTime;
    }
      else {                                               // If the time elapsed is short...
        encChangeAbs = abs(encChange);                     // Get absolute encoder change.
      if (encChangeAbs >= measurementWidth) {              // If absolute encoder change is greater or equal to measurement width...
        encCountPerSec_2 = 1000*(encChange/timeElapsed);   // Calculate counts per second to give velocity measure
        storedTime = refTime;                              // set previous encoder Count to the current encoder count and storedTime to current time in ms
        prevEncCount = encCount_2;
    }
    }
    velocity_2 = (encCountPerSec_2 / encCountPerRev)*60;   //Calculate the rpm of the wheel 2
  }


  // ISR for counting the encoder 1 value
  void encInc_1() {
    static bool prevPhaseA;
    static bool prevPhaseB;
    bool currentPhaseA;
    bool currentPhaseB;
   
    currentPhaseA = digitalRead(encPhaseA_1);                                       // Get current value of phase A and phase B
    currentPhaseB = digitalRead(encPhaseB_1);
    if (currentPhaseA != prevPhaseA) {                                              // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B
    if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
      encCount_1 ++;
    }
    else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
      encCount_1 --;
    }
    else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
      encCount_1 --;
    }
    else {
      encCount_1 ++;
    }
    }
    else if (currentPhaseB != prevPhaseB) {                                         // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B
      if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) {
      encCount_1 --;
    }
    else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) {
      encCount_1 ++;
    }
    else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) {
      encCount_1 ++;
    }
    else {
      encCount_1 --;
    }
    }
    prevPhaseA = currentPhaseA;
    prevPhaseB = currentPhaseB;  
  }

  // ISR for counting the encoder 2 value
  void encInc_2() {
    static bool prevPhaseA;
    static bool prevPhaseB;
    bool currentPhaseA;
    bool currentPhaseB;
   
    currentPhaseA = digitalRead(encPhaseA_2);                                       // Get current value of phase A and phase B
    currentPhaseB = digitalRead(encPhaseB_2);
    if (currentPhaseA != prevPhaseA) {                                              // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B
    if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) {
      encCount_2 ++;
    }
    else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) {
      encCount_2 --;
    }
    else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) {
      encCount_2 --;
    }
    else {
      encCount_2 ++;
    }
    }
    else if (currentPhaseB != prevPhaseB) {                                      // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B
      if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) {
      encCount_2 --;
    }
    else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) {
      encCount_2 ++;
    }
    else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) {
      encCount_2 ++;
    }
    else {
      encCount_2 --;
    }
    }
    prevPhaseA = currentPhaseA;
    prevPhaseB = currentPhaseB;  
  }

  //Function for calculating motor demand signal
  void calculatePID_1() {
    int kp = 1.2;
    int ki = 2.5;
    int kd = 0; 
    int velocityError_1 = 0;
    static float integral_1;
    float derivative_1 = 0;            
    static int dt = 0;           //  Time elasped since last velocity  given
    static int refTime;                   // Current reference time in ms
    static int storedTime = millis();     // Time at previous velocity measure in ms
    refTime = millis();                   // Update reference time in ms
    dt = refTime-storedTime;
    storedTime = millis();
    
    velocityError_1 =  -targetVelocity - velocity_1;    //calculate velocity error for wheel 1

    integral_1 += velocityError_1*dt/1000;
    if (integral_1 < outMin) {                //restricts intergral term to reduce windup
      integral_1 = outMin;
    }
    else if (integral_1 > outMax) {
      integral_1 = outMax;
    }
    derivative_1 = (velocityError_1 - prevError_1)/dt;
    prevError_1 = velocityError_1;

    controlSignal_1 = kp*velocityError_1 + ki*integral_1 + kd*derivative_1;

    motorDemand_1 = constrain(controlSignal_1,-100,100);
  }

  void calculatePID_2() {
    int kp = 1.2;
    int ki = 2.5;
    int kd = 0; 
    int velocityError_2 = 0; 
    static float integral_2;
    float derivative_2 = 0;
    static int dt = 0;           //  Time elasped since last velocity  given
    static int refTime;                   // Current reference time in ms
    static int storedTime = millis();     // Time at previous velocity measure in ms
    refTime = millis();                   // Update reference time in ms
    dt = refTime-storedTime;
    storedTime = millis();

    velocityError_2 =  targetVelocity - velocity_2;         // calculate velocity error for wheel 2

    integral_2 += velocityError_2*dt/1000;
    if (integral_2 > outMax) {                  //restricts intergral term to reduce windu
      integral_2 = outMax;
    }
    else if (integral_2 < outMin) {
      integral_2 = outMin;
    }

    derivative_2 = (velocityError_2 - prevError_2)/dt;
    prevError_2 = velocityError_2;

    controlSignal_2 = (kp*velocityError_2 + ki*integral_2);
    motorDemand_2 = constrain(controlSignal_2,-100,100);
  }

void payloadDrop () {

 
    if (avg_distanceTravelled >= 6075 && avg_distanceTravelled <= 6825) {
     dropZone = true;
    }
    else {
      dropZone = false;
    }
    if (dropZone == true) {
      linMotor.writeMicroseconds(1000);
    }
    else {
      linMotor.writeMicroseconds(2000);
    }
  }

  void targetVelocitycheck () {

    if (avg_distanceTravelled >= 5500 && avg_distanceTravelled <= 6850) {
      targetVelocity = 10;
    }  
    else if (avg_distanceTravelled >= 6850 && avg_distanceTravelled <= 11500) {
      targetVelocity = 80;
    }
    else {
      targetVelocity = 80;
    }
  }

  void motorControl() {
      
      // This code below is used to output the demand to the motors, it should not need to be changed.
      motorCommand_1 = round(motorDemand_1*2.55);  //Coverts -100 to 100 to -255 to 255
      motorCommand_2 = round(motorDemand_2*2.55);  //Coverts -100 to 100 to -255 to 255
      motorCommand_1 = abs(motorCommand_1);         //Makes motorCommand_1 a positive value.
      motorCommand_2 = abs(motorCommand_2);         //Makes motorCommand_2 a positive value.
      if (motorDemand_1 > 0) {                      //Sets motor 1 direction.
      digitalWrite(motorDirPin_1, HIGH);            //Establishes forward direction of Channel A
      }
      else {
        digitalWrite(motorDirPin_1, LOW);           //Establishes reverse direction of Channel A
      }
      digitalWrite(motorBrakePin_1, LOW);           //Disengage the Brake for Channel A
      analogWrite(motorSpeedPin_1, motorCommand_1); //Spins the motor on Channel A at motor demand
     
      if (motorDemand_2 > 0) {                      //Sets motor 2 direction.
      digitalWrite(motorDirPin_2, HIGH);            //Establishes forward direction of Channel B
      }
      else {
        digitalWrite(motorDirPin_2, LOW);           //Establishes reverse direction of Channel B
      }
      digitalWrite(motorBrakePin_2, LOW);           //Disengage the Brake for Channel B
      analogWrite(motorSpeedPin_2, motorCommand_2); //Spins the motor on Channel B at motor demand
     
      delay(2);

  }

  /*void AllignmentCheck() {
   potValue = analogRead(potPin);
   potBias = (potValue-260)/maxPot; 
   float diffVel = 0;
   float magVel = 0;
   float motorBias_1 = 0;
    float motorBias_2 = 0;

  if (velocity_1 > velocity_2) {
   diffVel = velocity_1 - velocity_2;
   magVel = velocity_1 + velocity_2;

   motorBias_1 = 1 - diffVel/magVel;
   motorBias_2 = 1;
  }

  else if (velocity_2 > velocity_1) {
   diffVel = velocity_2 - velocity_1;
   magVel = velocity_1 + velocity_2;

   motorBias_2 = 1 - diffVel/magVel;
   motorBias_1 = 1;
 
  }
  else {
    motorBias = 1;
  }
  }*/