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


#include <ESP32Servo.h>

#define PENDOLUM_PIN 18
#define FORCE_PIN 5

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

#define ENABLE_CRASH_CONTROL

Servo servo_pendulum;

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

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 = 1.0;  // 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 = 0.0001; // 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!");

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

  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 = 50;
        input = map(theta * (180.0 / M_PI), -90, 90, 0, 100);
        myPID.SetOutputLimits(-5000, 5000);

        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.println();
    }

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