int led_green = 2;
int led_yellow = 3;
int led_red = 4;
int h_enable = 10;
int h_direction = 9;
int pometer = A0;


void setup() {
  Serial.begin(115200);
  //setting the latency for communication 
  pinMode(led_green, OUTPUT);
  pinMode(led_yellow, OUTPUT);
  pinMode(led_red, OUTPUT);
  pinMode(h_enable, OUTPUT);
  pinMode(h_direction, OUTPUT);
  pinMode(pometer, INPUT);
  //defining if each pin is input or output 

  LED_start(); //function to light all LEDS
  Heli_start();
  Shutdown();
}

void loop()
{
}

void LED_start()
{
  digitalWrite(led_green,HIGH);
  digitalWrite(led_yellow,HIGH);
  digitalWrite(led_red,HIGH);
  //turning all the LEDS on 
  delay(500);
  //waiting half a second
  digitalWrite(led_green,LOW);
  digitalWrite(led_yellow,LOW);
  digitalWrite(led_red,LOW);
  //turning all the LEDs off 
}

void Heli_start()
{
  float start_height = Height_Reading();
  unsigned long startTime = millis(); //recording the current time to compare for the 5 second duration
  // creating variables for time, used top compare to 5 second running time
  float timeElapsed = 0;
  do 
  {
    float current_height = Height_Reading(); //The height of the helicopter is determined at the start of every loop
    LED_error(current_height);
    Motor_Control(current_height,start_height);
    timeElapsed = Get_Time(startTime);
    delay(40);
  }while(timeElapsed<5000);
  Heli_Land(start_height);
}

float Height_Reading()
{
  float pot_value = analogRead(pometer);
  //Read the value from the potentiometer
  //Using y=mx+c a formula can be used to convert the potentiometer reading into an angle 
  // Angle = 0.346*Pot_Reading - 141.84
  // 90 is then added so the start position is horizontal
  float angle_degrees = (0.346*pot_value) - 141.84; //angle from, potentiometer is converted into degrees 
  float angle_radians = angle_degrees * (PI/180); //angle converted from degrees to radians so it can be inputted into the sin function
  float height = sin(angle_radians)*0.491;
  //angle is converted into a height by trigonometry
  return height;
}

void LED_error(float current_height)
{
  digitalWrite(led_green,LOW);
  digitalWrite(led_yellow,LOW);
  digitalWrite(led_red,LOW);
  if(current_height>0.07)
  {
    digitalWrite(led_red,HIGH); // if the helicopter is above 70mm then the error is >+20mm so the red LED shows
  }  
  else if(current_height<0.03)
  {
    digitalWrite(led_yellow,HIGH); //if the helicopter is below 30mm then the error is <-20mm so the yellow LED shows
  }
  else
  {
    digitalWrite(led_green,HIGH); //if the helicopter is neither above 70mm or below 30mm it must be within 20mm of the target height so the green LED shows
  }
}

void Motor_Control(float current_height, float start_height)
{
  Serial.println(current_height);
  float target_height = 0.05;
  digitalWrite(h_direction,LOW); //the motor will only go down when the helicopter is landing
  float hover_power = 220;//power required to keep the helicopter hovering
  float add_hover = 0;
  float motor_speed = 0;
  float distance = 0;
  //defining variables to be used in if statements
  if(current_height>target_height)
  {
    distance = current_height - target_height; //works out the distance between the helicopter and target height
    add_hover = map(distance,0,0.05,0,8); //maps the distance to how much engine power is required 
    motor_speed = hover_power - add_hover; //the power is subracted from the hover power so the helicopter descends towards target height
  }
  else if (current_height<target_height && current_height>0)
  {
    distance = target_height - current_height;
    add_hover = map(distance,0,0.05,0,8);
    motor_speed = hover_power + add_hover; //the power mapped is added to the hover power so the helicopter ascends 
  }
  else if(current_height == target_height)
  {
    motor_speed = hover_power; //if the helicopter is exactly level with the target height, the hover height is used 
  }
  else
  {
    motor_speed = 230; //if the final else is reached, the helicopter is below the ground, so max power is used 
  }
  Serial.print("Error = ");
  Serial.print(distance);
  Serial.println("m");
  Serial.print("Height = ");
  Serial.print(current_height);
  Serial.println("m");
  Serial.print("Motor Speed = ");
  Serial.println(motor_speed);
  analogWrite(h_enable,motor_speed); 
}

float Get_Time(unsigned long startTime)
{
  unsigned long currentTime = millis();
  float TimeElapsed = (float) currentTime - startTime;
  return TimeElapsed;
}

void Heli_Land(float start_height)
{
  float current_height = 1000;
  do
  {
    delay(40);
    digitalWrite(h_direction,HIGH);
    analogWrite(h_enable,6);
    current_height = Height_Reading();
  }while(current_height > start_height);
}

void Shutdown()
{
  analogWrite(h_enable,0);
  digitalWrite(led_green,HIGH);
  digitalWrite(led_yellow,HIGH);
  digitalWrite(led_red,HIGH);
  //turning all the LEDS on 
  delay(500);
  //waiting half a second
  digitalWrite(led_green,LOW);
  digitalWrite(led_yellow,LOW);
  digitalWrite(led_red,LOW);
  //turning all the LEDs off 
}