///////////////////////////////////////////////////////
//
// Luca Amore
// https://www.lucaamore.com
//
///////////////////////////////////////////////////////

#include <stdio.h>
#include <math.h>
#include <QuickPID.h>
#include <LiquidCrystal_I2C.h>

#include <ESP32Servo.h>

#define PENDOLUM_PIN 18
#define FORCE_PIN 5

#define SETPOINT_PIN 34
#define HORZ_PIN 35
#define VERT_PIN 32
#define SEL_PIN  25

#define ENABLE_CRASH_CONTROL

Servo servo_pendulum;

LiquidCrystal_I2C lcd(0x27, 20, 4);

// Variables and parameters of the PID
float setpoint, input, output, delta;
//QuickPID myPID(&input, &output, &setpoint);
const float Kp = 10, Ki = 4, Kd = 6;

QuickPID myPID(&input, &output, &setpoint, Kp, Ki, Kd, myPID.Action::direct);

// Parameters of the pendulum
const double g = 9.81; // gravitational acceleration
const double l = 0.5;  // length of the pendulum
const double b = 0.1;  // damping coefficient (friction)
const double m = 1.0;  // mass of the pendulum

// Simulation parameters
const double dt = 10E-5; // time step

// Function to update the state of the pendulum
void updatePendulum(double *theta, double *omega, double F) {
    double dtheta = *omega;
    double domega = (g / l) * sin(*theta) - (b / m) * (*omega) + (F / (m * l));
    
    // Update the state using Euler's method
    *theta += dtheta * dt;
    *omega += domega * dt;

#ifdef ENABLE_CRASH_CONTROL
    // Crash control
    if (*theta > M_PI/2){
      *omega = 0;
      *theta = M_PI/2;
    } else if (*theta < -M_PI/2) {
      *omega=0;
      *theta = -M_PI/2;
    }

#endif
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  Wire.begin(21, 22);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("PID Controller");
  lcd.setCursor(0, 1);
  lcd.print("Luca Amore");

  myPID.SetTunings(Kp, Ki, Kd);
  myPID.SetMode(myPID.Control::automatic);

  pinMode(SETPOINT_PIN, INPUT);
  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);

  servo_pendulum.attach(PENDOLUM_PIN);
  servo_pendulum.write(0);

}

void loop() {

    double theta = 0; // initial angle (45 degrees)
    double omega = 0.0; // initial angular velocity
    double F = 0.0; // external force applied to the pendulum

    double t=0;

    while(1) {

        int horz = analogRead(HORZ_PIN);
        int vert = analogRead(VERT_PIN);
        int sel = digitalRead(SEL_PIN) == LOW;

        if (sel){
          theta=0;
          omega=0;
        }

        t += dt;

        F=0;

        //apply PID gains
        setpoint = map(analogRead(SETPOINT_PIN),0,4095,0,100);
        input = map(theta * (180.0 / M_PI), -90, 90, 0, 100);
        myPID.SetOutputLimits(-8000, 8000);

        myPID.Compute();

        F=(-vert+2048)*1000;
        F+=output;

        updatePendulum(&theta, &omega, F);

        servo_pendulum.write(theta * (180.0 / M_PI) + 90);
        
        //Serial.print("Time: ");
        //Serial.print(t, 2);
        //Serial.print(", A: ");
        //Serial.print(theta * (180.0 / M_PI), 2);
        //Serial.print(", V: ");
        //Serial.print(omega * (180.0 / M_PI), 2);
        //Serial.print(", Force: ");
        //Serial.print(F, 2);
        //Serial.print(", PID: ");
        //Serial.print(output, 2);
        //Serial.print(", P: ");
        //Serial.print(myPID.GetPterm(), 2);
        //Serial.print(", I: ");
        //Serial.print(myPID.GetIterm(), 2);
        //Serial.print(", D: ");
        //Serial.print(myPID.GetDterm(), 2);
        Serial.print("SP: ");
        Serial.print(setpoint, 2);
        Serial.print("IN: ");
        Serial.print(input, 2);
        Serial.println();

        //lcd.init();
        lcd.setCursor(0, 0);
        lcd.print("SP : "+String(setpoint)+"     ");
        lcd.setCursor(0, 1);
        lcd.print("IN : "+String(input)+"     ");
        lcd.setCursor(0, 2);
        delta = setpoint-input;
        lcd.print("DLT: "+String(delta))+"     ";
        lcd.setCursor(0, 3);
        lcd.print("PID: "+String(output)+"     ");

    }

    Serial.print("Someting is under control!");
}