/*
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name:  Amrita Paul
-------- Username:  mn23ap
--------------------------------------------
*/

// Define Arduino Pins 
const int POTENTIOMETER_PIN = A0;
const int H_BRIDGE_ENABLE_PIN = 10; 
const int H_BRIDGE_DIRECTION_PIN = 9; 

// Define Outputs 
const int GREEN_LED_PIN = 2;
const int YELLOW_LED_PIN = 3;
const int RED_LED_PIN = 4;

// Calibration constants for potentiometer
const float POTENTIOMETER_MIN_VALUE = 212; 
const float POTENTIOMETER_MAX_VALUE = 426.91; 
const float ANGLE_MIN = -68.5; 
const float ANGLE_MAX = 5.85; 
const float BEAM_LENGTH = 0.491; 

unsigned long START_TIME = 0;
unsigned long TARGET_REACHED_TIME = 0;  // Time when the target height is reached
float MAX_ERROR = 0;
float TOTAL_ERROR = 0;
int ERROR_COUNT = 0;

// Define the proportional, integral, and derivative gains
const float k_p = 0.3; // Proportional gain
const float k_d = 0.05; // Derivative gain

// Define the scaling factor to reduce motor speed
const float SPEED_SCALE = 0.5;

float last_error = 0.0; // Variable to store the previous error
float integral = 0.0;   // Variable to store the integral of the error

void setup() {
  //Initialise serial communication
  Serial.begin(9600);
  
  // Configure hardware connections to the sensors and motors
  pinMode(POTENTIOMETER_PIN, INPUT); 
  pinMode(H_BRIDGE_ENABLE_PIN, OUTPUT);
  pinMode(H_BRIDGE_DIRECTION_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT); 
  pinMode(RED_LED_PIN, OUTPUT); 
  
  // Initialise H-Bridge Controller and LEDS
  digitalWrite(H_BRIDGE_ENABLE_PIN, LOW); 
  digitalWrite(H_BRIDGE_DIRECTION_PIN, LOW); 
  
  // Indicate initialisation is complete
  for (int i = 0; i < 3; i++) {
    digitalWrite(GREEN_LED_PIN, HIGH);
    digitalWrite(YELLOW_LED_PIN, HIGH);
    digitalWrite(RED_LED_PIN, HIGH); 
    delay(500);
    digitalWrite(GREEN_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, LOW);
    digitalWrite(RED_LED_PIN, LOW); 
  }
  
  Serial.println("Initialisation Complete!"); 
}

void loop() {
  // Read the potentiometer value
  int POTENTIOMETER_VALUE = analogRead(POTENTIOMETER_PIN); 
  
  // Convert potentiometer value to an angle 
  float ANGLE = map(POTENTIOMETER_VALUE, POTENTIOMETER_MIN_VALUE, POTENTIOMETER_MAX_VALUE, ANGLE_MIN, ANGLE_MAX);
  
  // Calculate current height of the drone
  float HEIGHT = BEAM_LENGTH * sin(radians(ANGLE)); 
  
  // Calculate the target height from the target angle 
  float TARGET_HEIGHT = 0.05;  // Target height is set to 50mm
 
  // Calculate the error
  float ERROR = TARGET_HEIGHT - HEIGHT; 
  
  // Calculate the derivative term
  float derivative = ERROR - last_error;
  
  // Calculate the integral term
  integral += ERROR;
  
  // Update the last_error variable
  last_error = ERROR;
  
  // Performance Statistics 
  if (ERROR < 0.02 && START_TIME == 0) {
    START_TIME = millis();
  }
  if (ERROR > MAX_ERROR) {
    MAX_ERROR = ERROR;
  }
  TOTAL_ERROR += ERROR;
  ERROR_COUNT++;
  
  // Implement control logic here
  int PWM_VALUE = k_p * ERROR + k_d * derivative; 
  int constrained_PWM_VALUE = constrain(PWM_VALUE, 0, 255);  // Scale down the PWM value
  
  if (abs(ERROR) > 0.02) {
    if (ERROR > 0) {
      digitalWrite(H_BRIDGE_ENABLE_PIN, HIGH); 
      analogWrite(H_BRIDGE_DIRECTION_PIN, constrained_PWM_VALUE); 
    } else {
      digitalWrite(H_BRIDGE_DIRECTION_PIN, LOW); 
      analogWrite(H_BRIDGE_ENABLE_PIN, constrained_PWM_VALUE); 
    }
   } else {
     if (millis() - START_TIME >= 5000) { 
      shutdown();  
    }
    digitalWrite(H_BRIDGE_ENABLE_PIN, LOW); 
    digitalWrite(H_BRIDGE_DIRECTION_PIN, LOW);
  }
  
  // Light the output LEDs according to the current control error
  if (ERROR >= -0.02 && ERROR <= 0.02) { 
    digitalWrite(GREEN_LED_PIN, HIGH); 
    digitalWrite(YELLOW_LED_PIN, LOW); 
    digitalWrite(RED_LED_PIN, LOW); 
  } else if (ERROR < -0.02) { 
    digitalWrite(GREEN_LED_PIN, LOW); 
    digitalWrite(YELLOW_LED_PIN, HIGH); 
    digitalWrite(RED_LED_PIN, LOW); 
  } else if (ERROR > 0.02) { 
    digitalWrite(GREEN_LED_PIN, LOW); 
    digitalWrite(YELLOW_LED_PIN, LOW); 
    digitalWrite(RED_LED_PIN, HIGH); 
  } else {
    digitalWrite(GREEN_LED_PIN, LOW); 
    digitalWrite(YELLOW_LED_PIN, LOW); 
    digitalWrite(RED_LED_PIN, LOW); 
  }
  
  // Send telemetry information via serial communication
  Serial.print("Time: ");
  Serial.print((millis() - START_TIME) / 1000.0, 2);
  Serial.println("s");
  Serial.print("Current Angle: ");
  Serial.print(ANGLE, 2);
  Serial.println("°");
  Serial.print("Height: ");
  Serial.print(HEIGHT, 2);
  Serial.println("m");
  Serial.print("Error: ");
  Serial.print(ERROR, 2);
  Serial.println("m");
  Serial.println("------------------------");
  
  // Calculate and report performance statistics
  unsigned long ELAPSED_TIME = millis() - START_TIME;
  float AVERAGE_ERROR = TOTAL_ERROR / ERROR_COUNT;

  Serial.print("Time taken to reach the TARGET HEIGHT: ");
  Serial.print(ELAPSED_TIME/1000.0,2);
  Serial.println("s");
  Serial.print("Maximum error encountered after reaching the TARGET HEIGHT: ");
  Serial.print(MAX_ERROR);
  Serial.println("m");
  Serial.print("Average error encountered after reaching the TARGET HEIGHT: ");
  Serial.print(AVERAGE_ERROR);
  Serial.println("m");
}

// Shutdown the system 
void shutdown() {
  // Light all LEDs for 1 second
  digitalWrite(GREEN_LED_PIN, HIGH);
  digitalWrite(YELLOW_LED_PIN, HIGH);
  digitalWrite(RED_LED_PIN, HIGH); 
  delay(1000);
  // Turn off the motor
  digitalWrite(H_BRIDGE_ENABLE_PIN, LOW);
  digitalWrite(H_BRIDGE_DIRECTION_PIN, LOW);
}