#include <Servo.h>

//Set up pins. (NOTE: const part added to template code to ensure pin values are not changed accidentally in code)
const int motorSpeedPin_L = 3;    // pin for motor L  demand
const int motorDirPin_L = 12;     // pin for motor L direction
const int motorBrakePin_L = 9;    // pin for motor L brake
const int motorSpeedPin_R = 11;   // pin for motor R  demand
const int motorDirPin_R = 13;     // pin for motor R direction
const int motorBrakePin_R = 8;    // pin for motor R brake
const int encPhaseA_L = 18;       // pin for encoder L phase A
const int encPhaseB_L = 19;       // pin for encoder L phase B
const int encPhaseA_R = 20;       // pin for encoder R phase A
const int encPhaseB_R = 21;       // pin for encoder R phase B
const int blackButtonPin = A9;    // pin for black button
const int greenButtonPin = A10;   // pin for green button
const int potPin = A8;            // pin for rotary pot
const int linMotorPin = 6;          // pin for linear motor
Servo linMotor; //Sets up motor (own note)

 
//Set up variables (variables renamed to L and R instead of 1 and 2)
volatile int encCount_L = 0;            // Counts made for encoder Left wheel
volatile int encCount_R = 0;            // Counts made for encoder on Right wheel
float encCountPerSec_L = 0;             // Left Encoder angular velocity in counts/s
float encCountPerSec_R = 0;             // Right Encoder angular velocity in counts/s
float motorDemand_L = 0;                // Left Motor demand, -100% to +100%
float motorDemand_R = 0;                // Right Motor demand, -100% to +100%
int motorCommand_L = 0;                 // Left Motor signal, 0 to 255
int motorCommand_R = 0;                 // Right Motor signal, 0 to 255
volatile int linMotorPos = 0;           // Linear motor pulse length
int potValue;                           // Potentiometer value.
float maxPot = 270;                     // Used to calculate potentiometer value.
float potBias;                          // Potentiometer bias -1 to +1
unsigned long timePassed = 0;           // Time in msec since buttom press
unsigned long timeStarted = 0;          // Time is msec when button pressed. [EDITED TO EQUAL 0 AT INITIALIZATION]
int buttonPressed = 0;                  // Which button Pressed, 0 = neither, 1 = black, 2 = green
float dist = 0;                         // Distance of buggy from start, changed according to encoder data
float avgVel = 0;                       // Average velocity from both encoders
float encRevs_L = 0;                    // Number of rotations in revs of L encoder
float encRevs_R = 0;                    // Number of rotations in revs of R encoder
float dist_L = 0;                       // Distance travelled by L wheel
float dist_R = 0;                       // Distance travelled by R wheel
float pi = 3.1415926535897932384626433832795; //Pi value stored for later use.
bool payloadDropped = false;            // Used to check if payload is dropped with bool logic


void setup() {
  // Setup the pins and variables for the programme and define the interupts for the encoder.
  // Setup Motor L
  pinMode(motorDirPin_L, OUTPUT);      // Initiates Motor Channel A pin
  pinMode(motorBrakePin_L, OUTPUT);    // Initiates Brake Channel A pin
  // Setup Motor R
  pinMode(motorDirPin_R , OUTPUT);     // Initiates Motor Channel A pin
  pinMode(motorBrakePin_R, OUTPUT);    // Initiates Brake Channel A pin
  // Setup Encoder L
  pinMode(encPhaseA_L, INPUT);         // Initiates Encoder 1 Phase A
  pinMode(encPhaseB_L, INPUT);         // Initiates Encoder 1 Phase B
  // Setup Encoder R
  pinMode(encPhaseA_R, INPUT);         // Initiates Encoder 2 Phase A
  pinMode(encPhaseB_R, INPUT);         // Initiates Encoder 2 Phase B
   // Setup Linear Motor Control
  linMotor.attach(6);
  linMotor.writeMicroseconds(2000);    // Change to 2000 to initialise fully extended
  delay(500);                         // Initiates the linear motor control pin [originally 3000]
  // Setup Button box
  pinMode(blackButtonPin, INPUT);      // Initiates Black Button
  pinMode(greenButtonPin, INPUT);      // Initiates Green Button
  pinMode(potPin, INPUT);              // Initiates Potentiometer

  attachInterrupt(digitalPinToInterrupt(encPhaseA_L),encInc_L,CHANGE); // Create interrupt for change in encoder L phase A
  attachInterrupt(digitalPinToInterrupt(encPhaseB_L),encInc_L,CHANGE); // Create interrupt for change in encoder L phase B
  attachInterrupt(digitalPinToInterrupt(encPhaseA_R),encInc_R,CHANGE); // Create interrupt for change in encoder R phase A
  attachInterrupt(digitalPinToInterrupt(encPhaseB_R),encInc_R,CHANGE); // Create interrupt for change in encoder R phase B

  Serial.begin(9600);                 //  setup serial
}

// Main loop
void loop() {                                 
 digitalWrite(motorBrakePin_L, HIGH);          //Engage the Brake for Channel A [NOTE: DO WE NEED MOTOR BRAKES ENABLED AT START OF EVERY LOOP???]
 digitalWrite(motorBrakePin_R, HIGH);          //Engage the Brake for Channel B
 delay(1000);
 // Initial buttonPressed variable.
 buttonPressed = 0;                         //Set button pressed to 0 [NOTE: MAYBE MOVE VARIABLE INITIALIZATION TO START BEFORE SETUP???]

// 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.
  }
  
  // buttonPressed = 1; //ADDED JUST FOR RELOAD. REMOVE AFTER
  if (buttonPressed == 1) {                     // [NOTE: ADDED IF STRUCTURE TO ALLOW USE OF BUTTONS TO RUN TWO DIFF LOOP CODES. BUTTON 1 PRESSED MEANS LOOP 1 RUNS]
  
  // 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_L = 0;                             // Initialise Left encoder [PROBABLY DONT SET TO ZERO EVERYTIME]
  encCount_R = 0;                             // Initialise Right encoder 
   timeStarted = millis();                    // Get the time in sec at which the button was pressed.  
  
  // This is the run loop, you set the condition for the while loop to keep running
  while (dist <= 11) {     //Add your condition for the loop to continue running here, that is when should your controller stop? [DECIDED WHILE LOOP RUNS WHILE BUGGY IS BEHIND FINISH LINE AKA 11METRES]
    timePassed = millis() - timeStarted;
    
// Place your run code here:
    encVel_L();
    encVel_R();
    avgVel = (encCountPerSec_L + encCountPerSec_R)*0.5;  // Gets average of velocities using variables calculated in the encVel functions.
    
    encRevs_L = encCount_L/512;                     //512 counts are one revolution of encoder shaft or one full rotation of connected wheel.
    encRevs_R = abs(encCount_R/512);                     //512 counts are one revolution of encoder shaft or one full rotation of connected wheel.
    dist_L = encRevs_L*0.176*pi;                    //Wheel diameter is 176mm, circumference is 0.176pi metres. So circumference x revs gives distance traveled.
    dist_R = encRevs_R*0.176*pi;                    //Same as other variable. Finds distance travelled by each wheel via encoder.
    dist = (dist_L + dist_R)/2;                     //Takes average of distances from both wheels to use as distance travelled by buggy.


      
      
     if (dist <= 5.625) {       //If buggy is behind drop zone, it goes full speed till drop zone.
      motorDemand_L = -100;                          // Sets demands to full power 
      motorDemand_R = 100;
      giveSpeed();                                  // Uses UDF to provide demands to motor (see UDF after loop code ends)
     }

    if ((dist >= 5.625) && (payloadDropped == false)) {    //Once buggy reaches target zone, buggy stop procedure can begin.
      motorDemand_L = 0;                                 // Stop buggy by turning off motor.
      motorDemand_R = 0;
      giveSpeed();                                       // Give motors the speed (zero) using UDF
      delay(3000);
      dropCan();
      payloadDropped = true;
    }

    // if ((dist >= 5.625) && (avgVel = 0)) {
    //   //dropCan();                                        // Runs payload drop code according to UDF below. 
      
    // }

    if ((dist <= 6.375) && (payloadDropped == true)) { //Once payload has been dropped, motors start up again.
      motorDemand_L = -80;                           //Demand set to 80% assuming lower speed needed for bumps
      motorDemand_R = 80;
      giveSpeed();                                  //Gives motors speed again
    } 

    if ((dist >=6.375) && (dist <= 11)) {
      motorDemand_L = -80;
      motorDemand_R = 80;
      giveSpeed();
    }

      delay(1); // CHANGE BACK TO 2 TWO ONCE CODE IS DONE
      
      // For debugging, remove once code is complete
      Serial.print(" Time passed: ");
      Serial.print(timePassed);
      Serial.print(" Distance ");
      Serial.print(dist);
      Serial.print(" Encoder L: ");
      Serial.print(encCount_L);
      Serial.print(" Encoder R: ");
      Serial.print(encCount_R);
      Serial.print(" Encoder count per sec L: ");
      Serial.print(encCountPerSec_L);
      Serial.print(" Encoder count per sec R: ");
      Serial.print(encCountPerSec_R);
      Serial.print(" MotorDemand_L: ");
      Serial.print(motorDemand_L);
      Serial.print(" MotorDemand_R: ");
      Serial.print(motorDemand_R);
      Serial.println();    
    }                                             //End bracket for while loop for dist<11metres
  }                                               //End bracket for if loop on button/code set to 1st option of paths
}                                                 //End bracket for entire void loop. CODE AFTER THIS IS OUTSIDE LOOP STRUCTURE

//Function to provide motors their speeds and channel directions
void giveSpeed() {
  
//This code below is used to output the demand to the motors, it should not need to be changed.
    motorCommand_L = round(motorDemand_L*2.55);   //Converts -100 to 100 to -255 to 255
    motorCommand_R = round(motorDemand_R*2.55);   //Converts -100 to 100 to -255 to 255
    motorCommand_L = abs(motorCommand_L);         //Makes motorCommand_L a positive value. [CHANGED TO -VE TO ALLOW MOTORS SPIN IN OPPOSITE DIRECIONS]
    motorCommand_R = abs(motorCommand_R);         //Makes motorCommand_R a positive value.

  if (motorDemand_L > 0) {                      //Sets motor 1 direction.
    digitalWrite(motorDirPin_L, HIGH);          //Establishes forward direction of Channel A
  }
  else {
    digitalWrite(motorDirPin_L, LOW);           //Establishes reverse direction of Channel A
  }
  digitalWrite(motorBrakePin_L, LOW);           //Disengage the Brake for Channel A
  analogWrite(motorSpeedPin_L, motorCommand_L); //Spins the motor on Channel A at motor demand
  
  if (motorDemand_R > 0) {                      //Sets motor 2 direction.
    digitalWrite(motorDirPin_R, HIGH);            //Establishes forward direction of Channel B
  }
  else {
    digitalWrite(motorDirPin_R, LOW);           //Establishes reverse direction of Channel B
  }
  digitalWrite(motorBrakePin_R, LOW);           //Disengage the Brake for Channel B
  analogWrite(motorSpeedPin_R, motorCommand_R); //Spins the motor on Channel B at motor demand 
}


// Function for calculating encoder velocity L
void encVel_L() {
   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;     // Calculate the time that has elapsed since last velocity measure
   encChange = encCount_L-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_L = 0;
    prevEncCount = encCount_L;           // 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_L = 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_L;
   }
   }
}

// Function for calculating encoder velocity R
void encVel_R() {
   int measurementWidth = 4;             // Number of counts that need to pass before determining velocity
   static int timeElapsed = 0;           //  Time elapsed 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_R-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_R = 0;
    prevEncCount = encCount_R;             // 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_R = 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_R;
   }
   }
}

// ISR for counting the encoder L value
void encInc_L() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA;
   bool currentPhaseB;
   
   currentPhaseA = digitalRead(encPhaseA_L);                                       // Get current value of phase A and phase B
   currentPhaseB = digitalRead(encPhaseB_L); 
   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_L) == HIGH && digitalRead(encPhaseB_L) == HIGH) {
    encCount_L ++;
   }
   else if (digitalRead(encPhaseA_L) == HIGH && digitalRead(encPhaseB_L) == LOW) {
    encCount_L --;
   }
   else if (digitalRead(encPhaseA_L) == LOW && digitalRead(encPhaseB_L) == HIGH) {
    encCount_L --;
   }
   else {
    encCount_L ++;
   }
   }
   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_L) == HIGH && digitalRead(encPhaseB_L) == HIGH) {
    encCount_L --;
   }
   else if (digitalRead(encPhaseA_L) == HIGH && digitalRead(encPhaseB_L) == LOW) {
    encCount_L ++;
   }
   else if (digitalRead(encPhaseA_L) == LOW && digitalRead(encPhaseB_L) == HIGH) {
    encCount_L ++;
   }
   else {
    encCount_L --;
   }
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

// ISR for counting the encoder R value
void encInc_R() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA;
   bool currentPhaseB;
   
   currentPhaseA = digitalRead(encPhaseA_R);                                       // Get current value of phase A and phase B
   currentPhaseB = digitalRead(encPhaseB_R); 
   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_R) == HIGH && digitalRead(encPhaseB_R) == HIGH) {
    encCount_R ++;
   }
   else if (digitalRead(encPhaseA_R) == HIGH && digitalRead(encPhaseB_R) == LOW) {
    encCount_R --;
   }
   else if (digitalRead(encPhaseA_R) == LOW && digitalRead(encPhaseB_R) == HIGH) {
    encCount_R --;
   }
   else {
    encCount_R ++;
   }
   }
   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_R) == HIGH && digitalRead(encPhaseB_R) == HIGH) {
    encCount_R --;
   }
   else if (digitalRead(encPhaseA_R) == HIGH && digitalRead(encPhaseB_R) == LOW) {
    encCount_R ++;
   }
   else if (digitalRead(encPhaseA_R) == LOW && digitalRead(encPhaseB_R) == HIGH) {
    encCount_R ++;
   }
   else {
    encCount_R --;
   }
   }
   
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

void dropCan() {
   // If buggy reaches boundary of drop zone and buggy has stopped (average velocity is zero), begin dropping payload. 
    linMotor.writeMicroseconds(1000);                    // [NOTE: 1000 MEANS IT WILL RETRACT AS NEEDED TO DROP CAN] Change to 2000 to extend linear actuator
    delay (3000);         //[CONSIDER IF YOU NEED THE DELAY AND EXTENDING AGAIN] Adjust as needed
    // linMotor.writeMicroseconds(2000);                   // Change to 1000 to retract
    // delay (3000);                                                   // Adjust as needed
   // payloadDropped = true;                              //Variable edited to show payload drop complete, allowing rest of loop to continue.
  
}