#include "config.h"

void doMotor(char *cmd) {
  command.motor(&motor, cmd);
  digitalWrite(PC10, !digitalRead(PC10));
  delayMicroseconds(2);
}



void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
               Commander *commander, CommandCallback callback) {
  encoder->init(&spi);

  // Driver configuration
  driver->pwm_frequency = 20000;
  driver->voltage_power_supply = 24;
  driver->voltage_limit = 24;
  driver->init();

  // Current sense initialization
  current_sense->linkDriver(driver);
  current_sense->init();

  // Motor configuration
  motor->linkSensor(encoder);
  motor->linkDriver(driver);
  motor->linkCurrentSense(current_sense);
  motor->useMonitoring(Serial);
  motor->monitor_downsample = 5000;  // Default monitoring interval
  motor->controller = MotionControlType::angle;
  motor->torque_controller = TorqueControlType::voltage;
  motor->foc_modulation = FOCModulationType::SpaceVectorPWM;

  // PID Configuration
  motor->PID_velocity.P = 0.75;
  motor->PID_velocity.I = 20;
  motor->LPF_velocity.Tf = 0.005;
  motor->P_angle.P = 0.5;
  motor->LPF_angle.Tf = 0.001;

  // Motor limits
  motor->velocity_limit = 40;  // Speed limit in rad/s (382 rpm)
  motor->voltage_limit = 24;
  motor->current_limit = 0.5;

  motor->sensor_direction = Direction::CCW;
  motor->init();
  motor->initFOC();
}


void foc_step(BLDCMotor *motor, Commander *commander) {
  if (motor_control_inputs.target_velocity != 0 ||
      motor->controller == MotionControlType::velocity) {
    if (motor->controller != MotionControlType::velocity) {
      motor->controller = MotionControlType::velocity;
    }
    motor->target = motor_control_inputs.target_velocity;
  } else {
    if (motor->controller != MotionControlType::angle) {
      motor->controller = MotionControlType::angle;
    }
    motor->target = motor_control_inputs.target_angle;
  }

  motor->loopFOC();
  motor->move();
  motor->monitor();
  commander->run();
}