#include <AS5040.h>
#include <Arduino.h>
#include <SPI.h>
#include <SimpleFOC.h>

#pragma region "Motor and sensor setup"
#define LED1 PC10
#define LED2 PC11
#define HARDWARE_SERIAL_RX_PIN PB7
#define HARDWARE_SERIAL_TX_PIN PB6
#define AS5040_CS PB15
#define AS5040_MISO PB14
#define AS5040_MOSI PC1
#define AS5040_SCLK PB10
#define CURRENT_SENSOR_1 PB1 // INA180A
#define CURRENT_SENSOR_2 PB0
#define CURRENT_SENSOR_3 PC5
#pragma endregion

AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
// Init iPower 6208
BLDCMotor motor = BLDCMotor(12);
BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
InlineCurrentSense current_sense = InlineCurrentSense(0.51, 50, PB1, PB0, PC5);
Commander command = Commander(Serial);
void onMotor(char *cmd) { command.motor(&motor, cmd); }

void setup() {
  Serial.setRx(HARDWARE_SERIAL_RX_PIN);
  Serial.setTx(HARDWARE_SERIAL_TX_PIN);
  Serial.begin(115200);

  pinMode(PC8, OUTPUT);
  pinMode(PC9, OUTPUT);
  pinMode(PA11, OUTPUT);
  pinMode(PA12, OUTPUT);
  pinMode(PC6, OUTPUT);
  pinMode(PA8, OUTPUT);
  pinMode(PA9, OUTPUT);
  pinMode(PA10, OUTPUT);
  digitalWrite(PC8, HIGH);
  digitalWrite(PC9, HIGH);
  delay(1);
  digitalWrite(PA11, HIGH);
  digitalWrite(PA12, HIGH);
  digitalWrite(PC6, HIGH);

  // arduino pin initialization INA180A current sensor
  pinMode(CURRENT_SENSOR_1, OUTPUT);
  pinMode(CURRENT_SENSOR_2, OUTPUT);
  pinMode(CURRENT_SENSOR_3, OUTPUT);
  analogReadResolution(12);
  sensor.init();
  motor.linkSensor(&sensor);

  driver.pwm_frequency = 20000;
  driver.voltage_power_supply = 12;
  driver.voltage_limit = 12;
  driver.init();
  motor.linkDriver(&driver);

  current_sense.linkDriver(&driver);
  // init current sense
  current_sense.init();
  // link the motor to current sense
  motor.linkCurrentSense(&current_sense);
  // control loop type and torque mode
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::torque;
  motor.motion_downsample = 0;

  // velocity loop PID
  motor.PID_velocity.P = 0;
  motor.PID_velocity.I = 0;
  motor.PID_velocity.D = 0;
  motor.PID_velocity.output_ramp = 0;
  motor.PID_velocity.limit = 0;
  // Low pass filtering time constant
  motor.LPF_velocity.Tf = 0;
  // angle loop PID
  motor.P_angle.P = 0;
  motor.P_angle.I = 0;
  motor.P_angle.D = 0;
  motor.P_angle.output_ramp = 0;
  motor.P_angle.limit = 0;
  // Low pass filtering time constant
  motor.LPF_angle.Tf = 0;
  // current q loop PID
  motor.PID_current_q.P = 0;
  motor.PID_current_q.I = 0;
  motor.PID_current_q.D = 0;
  motor.PID_current_q.output_ramp = 0;
  motor.PID_current_q.limit = 0;
  // Low pass filtering time constant
  motor.LPF_current_q.Tf = 0;
  // current d loop PID
  motor.PID_current_d.P = 0;
  motor.PID_current_d.I = 0;
  motor.PID_current_d.D = 0;
  motor.PID_current_d.output_ramp = 0;
  motor.PID_current_d.limit = 0;
  // Low pass filtering time constant
  motor.LPF_current_d.Tf = 0;
  // Limits
  motor.velocity_limit = 0;
  motor.voltage_limit = 0;
  motor.current_limit = 0;
  // sensor zero offset - home position
  motor.sensor_offset = 0;
  // general settings
  // motor phase resistance
  motor.phase_resistance = 0;
  // pwm modulation settings
  motor.foc_modulation = FOCModulationType::SinePWM;
  motor.modulation_centered = 1;
  command.add('A', onMotor, "motor");
  // tell the motor to use the monitoring
  motor.useMonitoring(Serial);
  motor.monitor_downsample = 0; // disable monitor at first - optional

  motor.init();
  motor.initFOC();
}

void loop() {
  motor.loopFOC();
  motor.monitor();
  motor.move();
  command.run();
}