/*
  Arduino Switch Statement Template
  This template demonstrates how to use a switch statement in Arduino.
*/
#include <Servo.h>


// set pin numbers:
///STEERING PINS
#define SIGNAL          2   // wheel encoder?
#define POWER_5V        3
#define STEPPER_PULSE   13    // 8 - Prev 
#define STEPPER_DIR     8     // 12 - Prev
#define STEPPER_ENA     12    // 11 - Prev
#define LED             13
#define STEERING_POT    A0

///BRAKE PINS
/*Brakes released ony when both solenoids powered
 * This is to minimise risk, when arduino shuts off, brakes are applied
 * Also if one solenoid fails brakes are still applied
 * Soleniod1 Solenoid2
 *    1         1       - Brakes Released
 *    0         1       - Brakes Applied
 *    1         0       - Brakes Applied
 *    0         0       - Brakes Applied 
 */
#define BRAKE_TERM1     4
#define BRAKE_TERM2     5
#define BRAKE_POT       A2    // Previously A1

///ACCELERATOR PINS
/* 3Pin S8218 ServoMotor
 * 1. Negative (B)
 * 2. positive (R)
 * 3. Signal (O/W) PIN 6
 *  45 deg => 500 usec
 *  Anticlockwise  1000 - 2000 usec
 */
#define ACCELERATOR_PIN 9

///GEAR SHIFTING PINS
/*Terminal1 Terminal2
 *    1         1       - Foward Shift 
 *    0         1       - No Shift
 *    1         0       - No Shift
 *    0         0       - Backward Shift * 
 */
#define GEAR_POT        A2  // Motor Position Feedback
#define GEAR_TERM1      7   // Terminal1
#define GEAR_TERM2      6   // Terminal2

//****************CONSTANTS*************************
#define PARK_GEAR       0
#define REVERSE_GEAR    117
#define NEUTRAL_GEAR    168
#define DRIVE_GEAR      219

#define ACC_MAX         800     // Tperiod(us) relating to angle when accelerator pedal floored
//#define ACC_STEPS       383      // Tperiod(us) relating to 5mm incremental angles of accelerator pedal
#define ACC_STEPS       1000      // Tperiod(us) relating to 5mm incremental angles of accelerator pedal
#define ACC_MIN         5400    // Tperiod(us)relating to angle when accelerator pedal released
#define ACC_DEG_TIME    11      // Servor Motor Angle position to signal clock period 500us/45deg


//#define BRAKE_MAX       740     // when the actuator is extended by 119 mm
#define BRAKE_MAX       580     // 11 Nov 2023 changed with PCB from 740 to 463 ( 450)
//#define BRAKE_MIN       630     // when the actuator is extended by 90 mm   //
#define BRAKE_MIN       370       //// 11 Nov 2023 changed with PCB from 630 to 360 ( 370)
#define BRAKE_STEPS     10      //  relates to increments of 2mm

#define STEER_MAX       710   // 3.9kOhm - Right Max - 790 , Old 999
#define STEER_ZERO      639   // 2kOhm  - Mid - 410 ADC bits, old 639
#define STEER_MIN       80   // 0 - 330 Ohm- Left Max - 62 bits, old 279

volatile uint16_t pulseCounter = 0;
uint32_t lastLoop = 0;
uint16_t instPulse;
uint16_t speedArray[8] = {0,0,0,0,0,0,0,0};
uint8_t inPtr = 0;
uint16_t avgSpeed = 0;
uint16_t stepperEnabled = 0;
uint16_t brakingEnabled = 0;
uint16_t acceleratorAngle = ACC_MIN;
uint16_t gearPot = 0;
uint16_t requestedBrakeVal = BRAKE_MAX-80;
uint16_t brakePot = 0;
uint16_t requestedGear = PARK_GEAR;
uint16_t selectedGear = PARK_GEAR;
uint8_t brakeFlag = 0;

uint16_t steerMeas=0;
uint16_t steerRef = STEER_ZERO;
uint32_t loopTime = 0;
uint16_t varDelay = 5000;

float startTime = 0;

//Object Declarations
Servo Accelerator;

void int_handler()
{
  pulseCounter++;
}

void setup() {
  pinMode(POWER_5V, OUTPUT);
  pinMode(SIGNAL, INPUT);
  pinMode(STEPPER_PULSE, OUTPUT);
  pinMode(STEPPER_DIR, OUTPUT);
  pinMode(STEPPER_ENA, OUTPUT);
  pinMode(LED, OUTPUT);
  pinMode(BRAKE_TERM1,OUTPUT);
  pinMode(BRAKE_TERM2,OUTPUT);
  
//  pinMode(ACCELERATOR_PIN,OUTPUT);
  pinMode(GEAR_TERM1,OUTPUT);
  pinMode(GEAR_TERM2,OUTPUT);

 // digitalWrite(BRAKE_TERM1, LOW);  
//  digitalWrite(BRAKE_TERM2, HIGH);
  
  digitalWrite(GEAR_TERM1, LOW);
  digitalWrite(GEAR_TERM2, HIGH);
  
  digitalWrite(STEPPER_PULSE, LOW);
  digitalWrite(STEPPER_DIR, HIGH); //high is left
  digitalWrite(STEPPER_ENA, LOW);  //HIGH to enable  

  pulseCounter = 0;
  digitalWrite(POWER_5V, HIGH);
  attachInterrupt(digitalPinToInterrupt(SIGNAL), int_handler, CHANGE);

  //Initialisations
  stepperEnabled = 1;   // stepper always enabled for this case
  digitalWrite(STEPPER_ENA, HIGH);
  
  // attach the servo to the used pin number 
  Accelerator.attach(ACCELERATOR_PIN); 
  Accelerator.writeMicroseconds(ACC_MIN);     // Release Accelerator Pedal

  //initialise Brakes, by breaking safety
  //requestedBrakeVal = BRAKE_MAX-10;
  brakePot= analogRead(BRAKE_POT);
  //applyBrakes();
  steerMeas = analogRead(STEERING_POT);
    
  Serial.begin(57600);
}

void loop() {

  if(Serial.available() > 0)
  {
      uint8_t ch = Serial.read();   // read next coming character without removing it from serial buffer
      switch(ch)
      {
        case 'a' :
          ch = Serial.read(); // view remove the character from serial buffer
          Serial.println("accelarating");
          accelerate();
          break;

        case 'b':
          ch = Serial.read();
          Serial.println("applying brake");
          if(requestedBrakeVal < BRAKE_MAX)
          {
            requestedBrakeVal += BRAKE_STEPS; 
            applyBrakes();
          }
          break;

        case 'c':
          // Action for range 2
          Serial.println("you selected C");
          break;

        case 'd':
          ch = Serial.read();
          decelerate();
          break;

        case 'r':
          ch = Serial.read();
          Serial.println("release brake");
          
          if(requestedBrakeVal > BRAKE_MIN)
          {
            requestedBrakeVal -= BRAKE_STEPS; 
            releaseBrakes();
          }
          
          break;
       case 'g':
          if(Serial.available() >= 4)
          {
            ch = Serial.read();
            requestedGear = 0;
            for(ch = 0; ch < 3; ch++)
            {
              requestedGear *= 10;
              requestedGear +=(Serial.read() - '0') % 10; 
            }
            shiftGear();
          }
          break;
        case 'e':
            ch = Serial.read();
            //floorBrakes();
            break;
        case 's':
          if(Serial.available() >= 4)
          {
            ch = Serial.read();
            //          steerRef = Serial.parseInt();
            //        Serial.parseInt() is too slow (around 800ms) so we have to make our own
            //          To save on speed, we don't check that valid digits have been sent
            //          Using this function with non-digits will have undetermined behaviour
            steerRef = 0;
            for(ch = 0; ch < 3; ch++)
            {
              steerRef *= 10;
              steerRef += (Serial.read() - '0') % 10;
            }
            
            if(steerRef > STEER_MAX)
            {
              steerRef = STEER_MAX;
            }
            if(steerRef < STEER_MIN)
            {
              steerRef = STEER_MIN;
            }
          }
          break;
        
        default:
          // Default action (optional)
          Serial.println("Default: Action for any other value");
          break;
      }

    // Small delay to avoid flooding the serial monitor
    delay(500);
  }
}

void applyBrakes(){
  //switch 
  brakePot = analogRead(BRAKE_POT);
  Serial.print("requestedBrakeVal = ");
  Serial.println(requestedBrakeVal);

  startTime = millis();
  if(requestedBrakeVal < BRAKE_MAX)
  {
    if(requestedBrakeVal > analogRead(BRAKE_POT)){
        //APPLY BRAKES
      Serial.println("Applying Brake:");
      digitalWrite(BRAKE_TERM1, LOW);
      digitalWrite(BRAKE_TERM2,LOW);
      brakeFlag=0;

      //Implement Timeout on while loop
      while(!(brakeFlag) && ((millis()-startTime)<1000)){
        brakePot = analogRead(BRAKE_POT);
        if(brakePot >= requestedBrakeVal){
          brakeFlag = 0;
        }
        Serial.print("BrakePot:" );   //debug
        Serial.println(brakePot);     //debug
        delay(100);
     }
    }
    else{
        
      //Serial.println("DONE BRAKING");
      //STOP BRAKES
      digitalWrite(BRAKE_TERM1, HIGH);
      digitalWrite(BRAKE_TERM2, LOW);
      brakePot = analogRead(BRAKE_POT);
      if(brakePot >= BRAKE_MIN)
      {
        brakingEnabled = 1;
        //Serial.println("BRAKES APPLIEDs");
      }
    }
  }
  else{
    requestedBrakeVal = BRAKE_MAX;
    brakingEnabled = 1;
  }
}

void accelerate(){
//    Make servo go to set degrees 

     acceleratorAngle -= ACC_STEPS; 
//   Accelerator.write(0);
   Accelerator.writeMicroseconds(acceleratorAngle);
}

void decelerate(){
//   Make servo go to set degrees 
     acceleratorAngle += ACC_STEPS; 
//   Accelerator.write(0);    // input angle in degrees
      Accelerator.writeMicroseconds(acceleratorAngle);
      
}

void releaseBrakes(){
  brakePot = analogRead(BRAKE_POT);
 Serial.print("requestedBrakeVal = ");
 Serial.println(requestedBrakeVal);


 startTime = millis();
  if(requestedBrakeVal > BRAKE_MIN){
    Serial.print("Brakepot = ");
    Serial.println(brakePot);
  
    if(requestedBrakeVal < brakePot){
      //RELEASE BRAKES
      Serial.println("Releasing Brake");
      digitalWrite(BRAKE_TERM1, HIGH);
      digitalWrite(BRAKE_TERM2, HIGH);
      brakePot = analogRead(BRAKE_POT);
      while((brakePot > requestedBrakeVal)&&((millis()-startTime)<1000)){
        brakePot = analogRead(BRAKE_POT);
        Serial.print("brakePot:");    //debug
        Serial.println(brakePot);     //debug
        delay(100);
        //delay(10);
      }
    }
      else{
      //STOP BRAKES
      digitalWrite(BRAKE_TERM1, LOW);
      digitalWrite(BRAKE_TERM2,HIGH);
      //Serial.println("DONE RELEASING");
      
      if(brakePot <= (BRAKE_MIN+5))
      {
        brakingEnabled = 0;
        //Serial.println("BRAKES RELEASED");
      }
    }
  }
    else{
    requestedBrakeVal = BRAKE_MIN;
    brakingEnabled = 0;
  }
}

void shiftGear(){
  if(requestedGear > selectedGear)
  {
    gearPot = analogRead(GEAR_POT);

    //switch on gear shift motor
    // Extend linear actuator lever supply positive voltage
    digitalWrite(GEAR_TERM1, LOW);
    digitalWrite(GEAR_TERM2, LOW);
    
    while(gearPot < requestedGear){
      
      gearPot = analogRead(GEAR_POT);
    }
    
    //switch off gear shift motor
    digitalWrite(GEAR_TERM1, LOW);
    digitalWrite(GEAR_TERM2, HIGH);

    //Update Gear Position
    selectedGear = requestedGear;
  }

  
  if(requestedGear < selectedGear)
  {
    gearPot = analogRead(GEAR_POT);

    //switch on gear shift motor
    // Retract linear actuator lever supply negative voltage
    digitalWrite(GEAR_TERM1, HIGH);
    digitalWrite(GEAR_TERM2, HIGH);
    
    while(gearPot > requestedGear){
      gearPot = analogRead(GEAR_POT);
    }
    
    //switch off gear shift motor
    digitalWrite(GEAR_TERM1, LOW);
    digitalWrite(GEAR_TERM2, HIGH);

    //Update Gear Position
    selectedGear = requestedGear;
  }
}

void brakeMotorActuateTestLimits()
{
  while(brakePot > BRAKE_MIN)
  {
    brakePot = analogRead(BRAKE_POT);
    Serial.print("BrakePot:");

    Serial.println(brakePot);
    Serial.print("Releasing Brake:");
    digitalWrite(BRAKE_TERM1, HIGH);
    digitalWrite(BRAKE_TERM2, HIGH);
  }
  
  Serial.print("Stopping Brake:");

  digitalWrite(BRAKE_TERM1, LOW);
  digitalWrite(BRAKE_TERM2,HIGH);

  while(brakePot < BRAKE_MAX)
  {
    brakePot = analogRead(BRAKE_POT);
    Serial.print("BrakePot:");
    Serial.print("Applying Brake:");
    Serial.println(brakePot);
    digitalWrite(BRAKE_TERM1, LOW);
    digitalWrite(BRAKE_TERM2, LOW);
  }
 
  //Stop Brakes
    Serial.print("Stopping Brake:");

  digitalWrite(BRAKE_TERM1, LOW);
  digitalWrite(BRAKE_TERM2,HIGH);
}
void accServoTest()
{
  Serial.println("Decelerate");  
  decelerate();

  delay(100);

  Serial.println("Accelerate");
  accelerate();
  
  delay(100);  
}


///Test Code: Steering Stepper Motor Test, Going Far left ( to STEER MIN) and Right  to STEER MAX) 
///
void stepperMotorTest()
{
  steerMeas = analogRead(STEERING_POT);
  steerMeas = 0.85*steerMeas + 0.15*analogRead(STEERING_POT);
  Serial.print("SteerMeas:");
  Serial.println(steerMeas);
  
  Serial.print("Going Left");
  while(steerMeas> STEER_MIN )
  {
    digitalWrite(STEPPER_DIR, LOW);   //CCW - LEFT
    steerMeas = 0.85*steerMeas + 0.15*analogRead(STEERING_POT);
    Serial.print("SteerMeas:");
    Serial.println(steerMeas);
    Serial.print("Going Left");
    delay(5);

    // Step the motor
    digitalWrite(STEPPER_PULSE, LOW);
    delayMicroseconds(10);
    digitalWrite(STEPPER_PULSE, HIGH);
    delayMicroseconds(10);
  }
  delayMicroseconds(10);  

  Serial.print("Going Right");
  while(steerMeas< STEER_MAX)
  {
      
    digitalWrite(STEPPER_DIR, HIGH);  //CW - RIGHT
    steerMeas = 0.85*steerMeas + 0.15*analogRead(STEERING_POT);
    Serial.print("SteerMeas:");
    Serial.println(steerMeas);
    Serial.print("Going Right");
    delay(5);
    // Step the motor
    digitalWrite(STEPPER_PULSE, HIGH);
    delayMicroseconds(10);
    digitalWrite(STEPPER_PULSE, LOW);
    delayMicroseconds(10);
  }
}

void gear_Implementation()
{
  //GEAR Implementation
 Serial.begin(57600);
 while(1){
   Accelerator.write(200);
   delay(1000);
   Accelerator.write(180);
   delay(1000);
   Accelerator.write(160);
   delay(1000);
   Accelerator.write(140);
   delay(1000);
   Accelerator.write(120);
   delay(1000);
   Accelerator.write(100);
   delay(1000);
   Accelerator.write(80);
   delay(1000);
   Accelerator.write(60);
   delay(1000);
 }
}