// TODO: Calibrate distance sensor
// TODO: Find sensible values for P, I and D

#include <Arduino.h>
#include <Servo.h>
#include <Wire.h>
#include "main.h"

// Define variables for various PID values
int Kp = 0;                   // variable for Proportional action
int Ki = 0;                   // variable for Integral action
int Kd = 0;                   // variable for Differential action
unsigned long time;           // variable for storing current time
float PID_current = 0;        // variable for sensor reading
float PID_current_sum = 0;    // variable for averaging sensor readings
float PID_setpoint = 0;       // variable for desired setpoint
float PID_error = 0;          // variable for calculated error
float PID_error_previous = 0; // variable for storing the previous error
float PID_error_total = 0;    // variable for storing the total error
float PID_control = 0;        // variable for calculated P, I, D action
Servo PID_output;             // object for sending pwm output to the motot

void setup()
{
  // Configure sane values
  PID_setpoint = (GUIDE_LENGTH / 2) - BALL_RADIUS + GUIDE_OFFSET; // This is the middle of the bar

  // Configure IO pins
  pinMode(PIN_AD_P, INPUT);
  pinMode(PIN_AD_I, INPUT);
  pinMode(PIN_AD_D, INPUT);
  pinMode(PIN_AD_SENSOR, INPUT);
  pinMode(PIN_SWITCH_P, INPUT_PULLUP);
  pinMode(PIN_SWITCH_I, INPUT_PULLUP);
  pinMode(PIN_SWITCH_D, INPUT_PULLUP);
  pinMode(PIN_SWITCH_MOTOR, INPUT_PULLUP);
  PID_output.attach(PIN_PWM_MOTOR);    // attaches the servo on pin 9 to the servo object
  PID_output.write(PID_OUTPUT_CENTER); // Put the servco at angle 125, so the balance is in the middle

  Serial.begin(115200);
  time = millis();
}

void loop()
{
  if (millis() > time + PID_PERIOD)
  {
    time = millis();
    PID_error_previous = PID_error;
    Serial.print("Setpoint: ");
    Serial.println(PID_setpoint);

    // read the average over 100 sensor values
    PID_current_sum = 0;
    for (int i = 0; i < AD_SAMPLE_SIZE; i++)
    {
      PID_current_sum += analogRead(PIN_AD_SENSOR);
    }
    Serial.print("raw: ");
    Serial.println(PID_current_sum / AD_SAMPLE_SIZE);
    PID_current = ((13 * pow(PID_current_sum / AD_SAMPLE_SIZE * 5 / 1024, -1)) + 1.8); // TODO
    Serial.print("Current: ");
    Serial.println(PID_current);

    // Calculate the error from the setpoint and the feedback
    PID_error = PID_setpoint - PID_current;
    PID_error_total += PID_error;
    Serial.print("Error: ");
    Serial.println(PID_error);

    // Read the PID parameters and map them to sensible values
    Kp = map(analogRead(PIN_AD_P), 0, 1024, 0, 30);
    Ki = map(analogRead(PIN_AD_I), 0, 1024, 0, 10);
    Kd = map(analogRead(PIN_AD_D), 0, 1024, 0, 100); // TODO

    // Calculate the control, depending on the switch settings
    PID_control = 0;

    if (digitalRead(PIN_SWITCH_P) == 0)
    {
      // Kp * error
      PID_control += Kp * PID_error;
      Serial.print("Kp: ");
      Serial.println(Kp);
    }
    else
    {
      Serial.println("Kp : ---, ");
    }

    if (digitalRead(PIN_SWITCH_I) == 0)
    {
      // Ki * Σ(error(t)) * time
      PID_control += Ki * PID_error_total;
      Serial.print("Ki: ");
      Serial.println(Ki);
    }
    else
    {
      Serial.println("Ki : ---, ");
    }

    if (digitalRead(PIN_SWITCH_D) == 0)
    {
      // Kd * d-error/d-time
      PID_control += Kd * ((PID_error - PID_error_previous) / PID_PERIOD);
      Serial.print("Kd: ");
      Serial.println(Kd);
    }
    else
    {
      Serial.println("Kd : ---, ");
    }

    if (digitalRead(PIN_SWITCH_MOTOR) == 0)
    {
      PID_control = map(PID_control, -255, 255, 0, 180);
      PID_output.write(PID_control);
      Serial.print("PID control: ");
      Serial.print(PID_control);
      Serial.println();
    }
    else
    {
      PID_output.write(PID_OUTPUT_CENTER);
      Serial.println("PID control: ---");
    }
  }
}

/*

### Calibration
-10	145	
-9	145	0
-8	145	0
-7	147	2
-6	148	1
-5	149	1
-4	153	4
-3	157	4
-2	160	3
-1	164	4
0	169	5
1	175	6
2	181	6
3	187	6
4	194	7
5	201	7
6	212	11
7	224	12
8	235	11
9	251	16
10	267	16

 */