#include <Wire.h>
#include <Servo.h>
#include "Sensor_9DOF.h"
#include "Button.h"
#include "config.h"

/**************************************************************************/
// Globals
Sensor9DOF Sensor_9DOF = Sensor9DOF();
Servo Fan1;
Servo Fan2;

Button button1;
Button button2;

// Important measured values for control
float R             = 0.0;
float Phi_deg       = 0.0;
float error_int_deg = 0.0;
float p_deg_s       = 0.0;
float p_deg_s_filt  = 0.0;
float time_s        = 0.0;
unsigned long update_time;
unsigned long time_delay;
float cmd_PD = 0.0;

int controlled_fan_PWM = FAN_OFF;
int controlled_fan2_PWM = FAN_OFF;

// Limit switch variables
bool manual_calibration = false;   // <<<<<<<<< Turn to true to set up calibration with fans off (on tutors instruction)
bool limits_calibrated = false;


/**************************************************************************/
/* Some notes:
 * 1) The sensor data cannot be updated within the timer function as
 *    the timer blocks interrupts which are used to handle the IC2
 *    connection with the sensor board. Here the "quick an dirty" solution
 *    to update the sensor data in the main loop has been choosen.
 * 2) The roll angle is integrated from the roll rate with a simple
 *    trapezodial rule
 */
/**************************************************************************/

//==========================================================================================//
//                  >>>>>>>>>>>>> STUDENTS: Start here <<<<<<<<<<<<<

/*

Operation instructions:

Part A:
1)  Plug the arduino in (Do not turn on the power supplies)
2)  Pick gains and control function
3)  Check with tutor that your gains and control function are adequate
4)  Set R = 0 in the setput_output function
5)  Press the arrow in the top left corner to compile and load the program
6)  Manually press the limit switches
7)  Observe how the values change printed to the console as you manually move the mount
8)  Send a character from the console to the arduino to kill the program
9)  Alter gains and start again

Part B:
1)  Make sure the program is in the killed state from part A
2)  Depending on the tutors’ instructions set manual_calibration to true or false
3)  Set R to the step function in the setput_output function
4)  Check with tutor that your gains and control function are adequate
5)  Get tutor to turn the power supplies on
6)  Press the arrow in the top left corner to compile and load the program
7)  If manual_calibration = true manually press the limit switches the get away from the mount
    If manual_calibration = false just wait
8)  DO NOT GO NEAR THE RIG THE SECOND THE FANS START
      If you want to kill the program at any time send a character from the console to the arduino
9)  Observe the behavior of your gains
10) Send a character from the console to the arduino to kill the program
11) Alter gains and start again

*/

// Change (or create) your parameters here
// Please keep Kp < 2.5 , Kd < 0.7*Kp, Ki < Kp
float Kp          = 2.2;
float Kd          = 0.6*Kp;
float Ki          = 0.3*Kp;

float PID_control ()
{
   float control;
   //====================================================//

  /*
   *start to work here and create a PD controller
   *cmd_PD is the angle command of the servo in degrees
   *You can use the following:
   * - Kp
   * - Kd
   * - Ki
   * - R             (reference input)
   * - Phi_deg       (roll angle)
   * - p_deg_s_filt  (filtered roll rate)
   * - error_int_deg (integrated roll angle error)
   */

  control = Kp*(R-Phi_deg)+Kd*(0-p_deg_s_filt)+Ki*(error_int_deg);

  //====================================================//
  return control;
}

void setpoint_output()
{
  // Use R = 0 for part a of the lab when the fans are not turned on
  // Comment out otherwise
  //R = 0;

  // Use R = -15 <--> 15 for the rest of the lab
  // This changes the setpoint every 15 seconds
  int step_time;
  step_time = 15; //<< time in seconds steps occur
  R = 20.0 - 20.0*((float)((update_time-time_delay)/1000%(2*step_time)/step_time));
}

//                  >>>>>>>>>>>>> STUDENTS: end here <<<<<<<<<<<<<
//==========================================================================================//


void setup() {
  // Serial init
  Serial.begin(115200);
  Serial.println("#ENEL321 fan control (╯°□°)╯");
  Serial.println("#");

  // Fans init
  Fan1.attach(FAN_1_PIN);
  Fan2.attach(FAN_2_PIN);

  // Set fans to lower limit (off)
  Fan1.writeMicroseconds(FAN_OFF);
  Fan2.writeMicroseconds(FAN_OFF);
  delay(200);

  // Gyro init
  Sensor_9DOF.init();
  Sensor_9DOF.static_calibration();
  Sensor_9DOF.update_gyro_readings();

  // Limit switch init
  button1.init_Button(LIMIT_1_PIN, true, 5, false);
  button2.init_Button(LIMIT_2_PIN, true, 5, false);
  Serial.println("#");
  Serial.println("#Starting limit switch calibration:...");

  update_time = millis() + DT_MS;
}

void loop() {
  bool limit_reached = false;

  //update roll rate from sensor
  Sensor_9DOF.update_gyro_readings();
  p_deg_s = Sensor_9DOF.getP()*180.0/PI;
  button1.get_Button_pressed();
  button2.get_Button_pressed();

  if (update_time <= millis())
  {
      //update time
      update_time += DT_MS;
      time_s += DT;

    // After gyro calibration contiually update the roll
    update_roll();

    if(limits_calibrated)
    {
      // Update control loop once the system has calibrated
      update_control(PID_control);

      // Print data to the computer
      print_updates();

      // Test to see if limit switches have been pressed
      // Will cause program to stop if they have for safety
      if (update_buttons())
      {
        limit_reached = true;
      }
    }
    else
    {
      // At the start of the program go through a calibration procedue
      // to find the angles the switches are pressed
      update_calibration();
    }

    // Kill Switch via console commands (and character sent)
    if (Serial.available())
    {
      char c;
      c = Serial.read();
      if (c>0)
      {
        limit_reached = true;
      }
    }

  }

  delay(1);

  // Exit program if limit switches pressed
  if (limit_reached)
  {
    Serial.print("#Kill switch acitvated pressed cutting engines please reset...");
    while(1)
    {
      Fan1.writeMicroseconds(FAN_OFF);
      Fan2.writeMicroseconds(FAN_OFF);
    }
  }
}


/**************************************************************************/
// Updates the roll angle and other variables for use in control
void update_roll()
{
  static float Phi_deg_pre   = 0.0;
  static float p_deg_s_pre   = 0.0;
  static float moving_average_filter_array[FILTER_SIZE] = {0.0};
  float sum_filter = 0.0;


  //Integrate roll rate with trapezodial rule
  Phi_deg     = Phi_deg + (p_deg_s + p_deg_s_pre)/2.0*DT;  // << Control variable
  p_deg_s_pre = p_deg_s;

  //Integrate roll error with trapezodial rule
  error_int_deg = error_int_deg + ((R - Phi_deg) + (R-Phi_deg_pre))/2.0*DT;  // << Control variable
  Phi_deg_pre = Phi_deg;

  if (error_int_deg > ERROR_INT_SATURATION)
  {
    error_int_deg = ERROR_INT_SATURATION;
  }
  else if (error_int_deg < -ERROR_INT_SATURATION)
  {
    error_int_deg = -ERROR_INT_SATURATION;
  }

  //update moving average filter for PD control
  //the first entry is the oldest one
  for (int ii=0; ii <FILTER_SIZE-1 ; ii++)
  {
    moving_average_filter_array[ii] = moving_average_filter_array[ii+1];
    sum_filter += moving_average_filter_array[ii];
  }

  moving_average_filter_array[FILTER_SIZE-1] = p_deg_s;
  sum_filter += moving_average_filter_array[FILTER_SIZE-1];

  p_deg_s_filt = sum_filter / FILTER_SIZE;  // << Control variable
}

/**************************************************************************/
// Update the output to the controlled fan based on the input readings
void update_control(float (*control_func)())
{
  static int  controlled_fan_PWM_pre = FAN_OFF;
  static bool first_run = true;

  if (first_run)
  {
    time_delay = update_time;
    first_run = false;
  }

  // Update the setpoint position
  setpoint_output();

  //switch sign to handle integration direction of servos
  cmd_PD = -(*control_func)();

  //limit the control output
  if (cmd_PD > CONTROL_LIMIT)
  {
    cmd_PD = CONTROL_LIMIT;
  }
  if (cmd_PD < -CONTROL_LIMIT)
  {
    cmd_PD = -CONTROL_LIMIT;
  }

  //convert control output (angle in deg) into PWM signal by using a linear interpolation
  controlled_fan_PWM = (int)(mapping_float(cmd_PD,
                                      -CONTROL_LIMIT,
                                      CONTROL_LIMIT,
                                      FAN_LOW,
                                      FAN_HIGH));
  controlled_fan2_PWM = (int)(mapping_float(-cmd_PD,
                                      -CONTROL_LIMIT,
                                      CONTROL_LIMIT,
                                      FAN_LOW,
                                      FAN_HIGH));

  //convert servo angle into PWM signal
  //only update PWM if command has changed
  if (controlled_fan_PWM != controlled_fan_PWM_pre)
  {
    Fan1.writeMicroseconds(controlled_fan_PWM);
    Fan2.writeMicroseconds(controlled_fan2_PWM);
  }

  controlled_fan_PWM_pre = controlled_fan_PWM;
}


/**************************************************************************/
// Limit switch calibration procedure for the system
void update_calibration()
{
  static float limit1_ang = 0.0;
  static float limit2_ang = 0.0;
  static bool limit1_pressed = false;
  static bool limit2_pressed = false;
  static bool enter_delay = false;
  static unsigned long delay_cal = 0;
  bool limit1_press = false;
  bool limit2_press = false;
  static bool first_run = true;

  if (first_run)
  {
    time_delay = update_time;
    first_run = false;
  }

  // Read buttons
  limit1_press = button1.get_Button_pressed();
  limit2_press = button2.get_Button_pressed();

  // Turn on fan 1 slowly to find the first limit switch's position
  if (!limit1_pressed && !limit2_pressed && !manual_calibration)
  {
    int fan_up;

    fan_up = FAN_SLOW_TURN + (update_time-time_delay)/30;
    if (fan_up > FAN_HIGH)
    {
      fan_up = FAN_HIGH;
    }
    Fan1.writeMicroseconds(fan_up);
  }

    // Turn on fan 2 slowly to find the first limit switch's position
  if (limit1_pressed && !limit2_pressed && !manual_calibration)
  {
    int fan_up;

    fan_up = FAN_SLOW_TURN + (update_time-time_delay)/30;
    if (fan_up > FAN_HIGH)
    {
      fan_up = FAN_HIGH;
    }
    Fan2.writeMicroseconds(fan_up);
  }

  // If first limit switch has been pressed record the angle
  // and turn the system in the other direction
  if (limit1_press && !limit1_pressed)
  {
    limit1_ang = Phi_deg;
    limit1_pressed = true;
    first_run = true;
    Serial.println("#Limit switch 1 pressed");
    Fan1.writeMicroseconds(FAN_OFF);

  }

  // If the second limit switch has been pressed record the angle
  // and turn the fans off
  if (limit2_press && !limit2_pressed)
  {
    limit2_ang = Phi_deg;
    limit2_pressed = true;
    Serial.println("#Limit switch 2 pressed");
    Fan2.writeMicroseconds(FAN_OFF);
  }

  // If both switches have been pressed set the mid point between the
  // two limit switch positions as the zero point so the setpoint is
  // the maximum position away from the limits
  if(limit1_pressed && limit2_pressed && !delay_cal)
  {
    float diff;
    float temp;

    diff = limit1_ang - limit2_ang;
    temp = diff/2;
    Phi_deg = Phi_deg - limit2_ang - temp;
    limit2_ang = -temp;
    limit1_ang = limit2_ang + diff;

    enter_delay = true;
    delay_cal = update_time;
    Serial.print("#Calibration complete control starting in ");
    Serial.print(CONTROL_START_DELAY/1000);
    Serial.println(" seconds");
  }

  // Delay the system before entering the control loop
  if (enter_delay && ((delay_cal+CONTROL_START_DELAY) < update_time))
  {
    limits_calibrated = true;
    Serial.println("Gains:");
    Serial.println("Kp,Ki,Kd");
    Serial.print(Kp);
    Serial.print(",");
    Serial.print(Ki);
    Serial.print(",");
    Serial.println(Kd);
    Serial.println("Time (s),Setpoint (deg),Phi (deg),Roll rate (deg/s),Error int (deg), PPM_diff");
  }
}


/**************************************************************************/
// In the control loop test to see if the limit switches have been pressed
bool update_buttons()
{
  bool limit1_press = false;
  bool limit2_press = false;

  // Read buttons
  limit1_press = button1.get_Button_pressed();
  limit2_press = button2.get_Button_pressed();

  if (limit1_press || limit2_press)
  {
    Fan1.writeMicroseconds(FAN_OFF);
    Fan2.writeMicroseconds(FAN_OFF);
    return true;
  }
  return false;
}

/**************************************************************************/
// Print updates for post analysis
void print_updates()
{
//  Serial.print("$>");
  Serial.print(time_s);
  Serial.print(",");
  Serial.print(R);
  Serial.print(",");
  Serial.print(Phi_deg);
  Serial.print(",");
  Serial.print(p_deg_s_filt);
  Serial.print(",");
  Serial.print(error_int_deg);
  Serial.print(",");
  Serial.print(controlled_fan_PWM-controlled_fan2_PWM);
  Serial.println();
}

// PS : Please do not code like this... (╯°□°)╯︵ ┻━┻