#include <QuickPID.h>

#define MOT_DX_STEP 19
#define MOT_DX_DIR 18
#define MOT_SX_STEP 17
#define MOT_SX_DIR 16

// Nema 17 make 1.8° per step. Using A4988 drivers, and 1/16th microstepping, it results in 0.1125° per step
constexpr double ANGLE_PER_STEP = 0.1125;
// Just used a kitchen scale, good enough
constexpr double WEIGHT = 0.961;
constexpr double WHEEL_RADIUS = 0.0475;
// Experimentally, the lowest pulse my steppers can handle without stalling + some leeway
constexpr double MAX_HALF_PERIOD = 75; // in microseconds
// Which means there is a maximum velocity achievable
constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD * ANGLE_PER_STEP) * PI / 180;

// Derived and analytical model, linearized it and simulated in MATLAB.
// PID values are then calculated and verified by simulation in Simulink. I ain't calibrating a PID by hand on this robot
// I modified the ArduPID library to make it accept negative values for the parameters
constexpr double KP = 42;
constexpr double KI = KP * 10.8852;
constexpr double KD = 0.3; 

// IMU little bit tilted
// TODO: Implement an outer control loop for angular velocity. But that requires encoders on the motors
// TODO: try to achieve it crudely by just using a PI controller on the velocity given by the PID balance controller
float setpoint = 0.0;
float output = 0;
float input = 0;

float yaw{ 0 }, pitch{ 0 }, roll{ 0 };



QuickPID pitchCtrl(&input, &output, &setpoint, KP, KI, KD, pitchCtrl.pMode::pOnError, pitchCtrl.dMode::dOnMeas, pitchCtrl.iAwMode::iAwCondition, pitchCtrl.Action::reverse);

void setup() {
  Serial.begin(9600);

  delay(1000);

  setup_imu();
  
  // Just to signal it is working
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  // Let the initial error from madgwick filter discharge without affecting the integral term of the PID
  unsigned long t = millis();
  while (millis() - t < 5000) {
    update_imu();
  }

  // Backward because all coefficients need to be negative
  pitchCtrl.SetOutputLimits(-MAX_VELOCITY, MAX_VELOCITY);
  pitchCtrl.SetMode(pitchCtrl.Control::automatic);
  pitchCtrl.SetSampleTimeUs(1000);

  digitalWrite(LED_BUILTIN, LOW);
}

void setup1(){
  pinMode(MOT_DX_DIR, OUTPUT);
  pinMode(MOT_SX_DIR, OUTPUT); 

  pinMode(MOT_DX_STEP, OUTPUT);
  pinMode(MOT_SX_STEP, OUTPUT);
}

unsigned long last_time_motors = micros(), current_time_motors = micros();
bool b = true;
// Such a long halfperiod means that the motors are not moving
uint32_t t = INT_MAX;
int32_t period = INT_MAX, halfperiod1 = INT_MAX;

void loop1(){
  // TODO: handle the steppers using interrupt timers. The second core could be used for IMU processing, while the first one handles the different control loops

  // Retrieve the half period value from core0. Non blocking call
  if(rp2040.fifo.available()) {
    rp2040.fifo.pop_nb(&t);
    
    int32_t period = (int32_t)t;

    // Direction need to be changed during motor pulse
    // Doing it here unsure it happens at the correct time
    if(period > 0){
      // Positivie direction
      digitalWriteFast(MOT_DX_DIR, HIGH);
      digitalWriteFast(MOT_SX_DIR, HIGH);
      halfperiod1 = (uint32_t)(period);
    }else{
      // Negative direction
      digitalWriteFast(MOT_DX_DIR, LOW);
      digitalWriteFast(MOT_SX_DIR, LOW);
      halfperiod1 = (uint32_t)(-period);
    }
  }

  current_time_motors = micros();
  if(current_time_motors - last_time_motors > halfperiod1){
    // Half a pulse. Next cycle will be the rest
    digitalWriteFast(MOT_DX_STEP, b);
    digitalWriteFast(MOT_SX_STEP, b);
    b = !b;
    last_time_motors = current_time_motors;
  }
}

double frequency = 0;
int32_t half_period0 = 0;
double velocity = 0;

uint32_t current_time = millis(), last_time = millis();

void loop() {
  update_imu();

  input = pitch;
  //if(abs(input -setpoint) <= 0.01) input = setpoint;
  
  // I also modified the ArduPID library to use compute as a boolean. If calculations were done, it returns true. If not enough time has elapsed, it returns false
  if(pitchCtrl.Compute()){

    double tvelocity = output;
    tvelocity = tvelocity / WHEEL_RADIUS * 180 / PI;
    frequency = tvelocity *  0.1125;
    half_period0 = 1000000 / (2*frequency);

    // Send the half period to core 1. Non blocking
    rp2040.fifo.push_nb(half_period0);
  }

}