/** 
 * B-G431B-ESC1 position motion control example with encoder
 *
 */
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h>

// Motor instance
//BLDCMotor motor = BLDCMotor(7, 0.14, 2450, 0.00003);
//BLDCMotor motor = BLDCMotor(7, 0.15, 940, 0.000125);
BLDCMotor motor = BLDCMotor(7, 0.14, 850, 0.0000425);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);

// encoder instance
MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor);

HardwareSerial Serial1(PB6, PB7);

// velocity set point variable
float target = 0;
float targetP = 0;
float targetI = 0;
float targetV = 0;
// instantiate the commander
Commander commandComp = Commander(Serial);
Commander commandESP = Commander(Serial1);
void doTargetP(char* cmd) { commandComp.scalar(&targetP, cmd); }
void doTargetI(char* cmd) { commandComp.scalar(&targetI, cmd); }
void doTargetV(char* cmd) { commandESP.scalar(&targetV, cmd); }

void setup() {

  //pinMode(LED_RED, OUTPUT); // Set board LED for debugging
  //digitalWrite(LED_RED, HIGH);
  
  // use monitoring with serial 
  Serial.begin(115200);
  Serial1.begin(115200);

  // enable more verbose output for debugging
  // comment out if not needed
  //SimpleFOCDebug::enable(&Serial);

  // initialize encoder sensor hardware
  sensor.init();

  // link the motor to the sensor
  motor.linkSensor(&sensor);
  
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  // link current sense and the driver
  currentSense.linkDriver(&driver);

  // initialise the current sensing
  if(!currentSense.init()){
    Serial.println("Current sense init failed.");
    return;
  }
  // no need for aligning
  currentSense.skip_align = true;
  motor.linkCurrentSense(&currentSense);

  motor.sensor_direction= Direction::CW;
  motor.zero_electric_angle = 0;

  // set motion control loop to be used
  motor.controller = MotionControlType::velocity;
  motor.torque_controller = TorqueControlType::foc_current;
  //motor.torque_controller = TorqueControlType::dc_current;
  //motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

  motor.current_sp = motor.current_limit;

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.01;
  motor.PID_velocity.I = 0.004;

  driver.voltage_limit = 10;
  motor.voltage_limit = 5;
  //motor.current_limit = 0.2;

  motor.PID_velocity.limit = motor.voltage_limit;

  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 10;
 
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;

  // comment out if not needed
  //motor.useMonitoring(Serial);

  //motor.monitor_downsample = 100000;
  motor.motion_downsample = 1;
  
  // initialize motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // add target command T
  commandComp.add('P', doTargetP, "target P");
  commandComp.add('I', doTargetI, "target I");
  commandESP.add('V', doTargetV, "target V");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity using serial terminal:"));
  _delay(1000);
}

void loop() {

  // main FOC algorithm function
  motor.loopFOC();

  //motor.PID_velocity.P = targetP;
  //motor.PID_velocity.I = targetI;

  //Serial.println(target);

  // Motion control function
  motor.move(targetV);

  
  if (abs(motor.target) < 20.0f && motor.enabled==1)
    motor.disable();
  if (abs(motor.target) >= 20.0f && motor.enabled==0)
    motor.enable();

  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  //motor.monitor();
  
  // user communication
  /*
  static uint32_t lastSerialRead = 0;
  if (lastSerialRead == 32) {
    lastSerialRead = 0;
    commandESP.run();
    commandComp.run();
  }
  lastSerialRead += 1;
  */
  commandESP.run();
  //commandComp.run();
}
