#include <Servo.h>
// defining constants:
//Set up pins
const int wheel_diameter = 137;
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 black_button_pin = A9;    // pin for black button
int green_button_pin = A10;   // pin for green button
int potPin = A8;            // pin for rotary pot
int linMotorPin = 6;          // pin for linear motor
Servo linMotor;

// these are just random values:
const int drop_off_distnace = 6000; //(+potentially the length of the buggy)
const int final_distance = 11000;// (+potentially length of buggy/ circumfrance of bumps)

//defining global 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
float time_passed = 0;           // Time in msec since buttom press
unsigned long timeStarted1;
unsigned long timeStarted2;                // Time is msec when button pressed.
int buttonPressed = 0;                  // Which button Pressed, 0 = neither, 1 = black, 2 = green
float distance_travelled_1 = 0;
float distance_travelled_2 = 0;
float angular_velocity_1 = 0;
float angular_velocity_2 = 0;
float translational_velocity_1 = 0;
float translational_velocity_2 = 0;

//These are variables which have been added since editting/updating the code:
bool actuator_activated = false;
unsigned long start_time;
int button_pressed = 0;
unsigned long current_time;
bool second_timer_started = false;
bool destination_reached = false;



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(3000);                         // Initiates the linear motor control pin
  // Setup Button box
  pinMode(black_button_pin, INPUT);      // Initiates Black Button
  pinMode(green_button_pin, 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

  encCount_1 = 0;                             // Initialise encoder 1
  encCount_2 = 0; 
  //ignore the start time bit
  start_time = millis();
  Serial.begin(9600);                 //  setup serial
  Serial.print("Time passed: ");
  Serial.print(",");
  Serial.print(" Encoder 1: ");
  Serial.print(",");
  Serial.print(" Encoder 2: ");
  Serial.print(",");
  Serial.print(" Encoder count per sec 1: ");
  Serial.print(",");
  Serial.print(" Encoder count per sec 2: ");
  Serial.print(",");
  Serial.print(" MotorDemand_1: ");
  Serial.print(",");
  Serial.println(" MotorDemand_2: ");


}

void loop() {
  //there is 512 counts per revolution
  
  
  // This while loop is to determine what set of code we want to run,
  // depending on what button we press (we could just have one set of code so this may not be neccessary)
//   while (button_pressed == 0) {
//     if (analogRead(black_button_pin) < 2){
//       button_pressed = 1;
//       timeStarted1 = millis();     
//     }
//     else if (analogRead(green_button_pin) < 2){
//       button_pressed = 2;
//       timeStarted1 = millis();     
//     }
//     else {
//       button_pressed = 0;
//     }
//     delay(50);
// }


//this section of the code uses the user defined function, which use the
//interrupt pins to constantly measure the specific values. This part of the loop
//calls the functions and constantly measures the needed values for the code to use
//there is 512 counts per revolution                 
distance_travelled_1 = encCount_1 * (1/512) * 3.141592 * wheel_diameter; 
distance_travelled_2 = encCount_2 * (1/512) * 3.141592 * wheel_diameter; 

//angualr velocity of each wheel in degrees per second
angular_velocity_1 = (360/512)*encCountPerSec_1;
angular_velocity_2 = (360/512)*encCountPerSec_2;
translational_velocity_1 = (angular_velocity_1/360) * 3.141592 * wheel_diameter;
translational_velocity_2 = (angular_velocity_2/360) * 3.141592 * wheel_diameter;





//this is to ger the buggy moving from the start until the drop zone
//20 is just an arbritrary  number i cant remeber the exact distance

if (distance_travelled_1 < drop_off_distnace && distance_travelled_2 < drop_off_distnace){
motorDemand_1 = 90;
motorDemand_2 = -90;
}


//this is for our linear actuator so needs to be changed depending on
//our final system, we probably wont have a delay in it
if (distance_travelled_1 > drop_off_distnace && distance_travelled_2 > drop_off_distnace && actuator_activated == false){
  linMotor.writeMicroseconds(1000);  // Change to 1000 to retract
  delay (3000);
  actuator_activated = true;
  start_time = millis();
}

//this if statment is to start the reference time for the ramp up input
//for when after the payload has been dropped, it only runs once, hence the boolean logic
if (distance_travelled_1 > drop_off_distnace && distance_travelled_2 > drop_off_distnace && second_timer_started == false){
  timeStarted2 = millis();
  second_timer_started == true;
}


if (distance_travelled_1 < final_distance && distance_travelled_2 < final_distance && destination_reached == false && actuator_activated == true){
motorDemand_1 = 90; 
motorDemand_2 = -90;
}

//once the buggy has reached the final distance i have set the motor to zero and turned the brakes on
if (distance_travelled_1 > final_distance && distance_travelled_2 > final_distance && destination_reached == false){
destination_reached == true;
motorDemand_1 = 0;
motorDemand_2 = 0;
digitalWrite(motorBrakePin_1, HIGH);
digitalWrite(motorBrakePin_2, HIGH);
}


// the motor code will only run if the buggy hasnt reached the final destination yet
if (destination_reached == false){
// This code below is used to output the demand to the motors, it should not need to be changed.
//this was taken directly from the template
    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
    
    // do we need this?:
    delay(2);

// For debugging, remove once code is complete
    Serial.print(time_passed);
    	Serial.print(", ");
    Serial.print(encCount_1);
    	Serial.print(", ");
    Serial.print(encCount_2);
    	Serial.print(", ");
    Serial.print(encCountPerSec_1);
    	Serial.print(", ");
    Serial.print(encCountPerSec_2);
    	Serial.print(", ");
    Serial.print(motorDemand_1);
    	Serial.print(", ");
    Serial.print(motorDemand_2);
    Serial.println();    


}
}

//void loop ends here






//the following are all user defined functions which i have called upon
//in the main loop
//it uses interrupt pins meaning that it will constantly measure the following values


// 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;
   }
   }
}

// 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;
   }
   }
}

// 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;  
}