// clang-format off
#include <Arduino.h>
#include <SimpleFOC.h>
#include <STM32_CAN.h>
#include <AS5045.h>
#include <DRV8313.h>
#include "hal_conf_extra.h"
// clang-format on

STM32_CAN Can(CAN2, DEF);

static CAN_message_t CAN_TX_msg;
static CAN_message_t CAN_inMsg;

SPIClass spi;
MagneticSensorAS5045 sensor(AS5045_CS, AS5045_MOSI, AS5045_MISO, AS5045_SCLK);

BLDCMotor motor(POLE_PAIRS);

DRV8313Driver driver(TIM1_CH1, TIM1_CH2, TIM1_CH3, EN_W_GATE_DRIVER,
                     EN_U_GATE_DRIVER, EN_V_GATE_DRIVER, SLEEP_DRIVER,
                     RESET_DRIVER, FAULT_DRIVER);
LowsideCurrentSense current_sense(0.0125, 20, CURRENT_SENSOR_1, CURRENT_SENSOR_2,
                                  CURRENT_SENSOR_3);

Commander command(Serial);

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

void setup_foc(MagneticSensorAS5045 *sensor, BLDCMotor *motor, DRV8313Driver *driver,
               LowsideCurrentSense *current_sense, Commander *commander,
               CommandCallback callback) {
    sensor->init(&spi);
    motor->linkSensor(sensor);
    driver->pwm_frequency = 20000;
    driver->voltage_power_supply = 20;
    driver->voltage_limit = 40;
    driver->init();
    motor->linkDriver(driver);
    current_sense->linkDriver(driver);
    current_sense->init();
    motor->linkCurrentSense(current_sense);
    SimpleFOCDebug::enable(nullptr);
    commander->add('M', callback, "motor");
    motor->useMonitoring(Serial);
    motor->monitor_start_char = 'M';
    motor->monitor_end_char = 'M';
    commander->verbose = VerboseMode::machine_readable;
    motor->controller = MotionControlType::angle;
    motor->torque_controller = TorqueControlType::voltage;
    motor->foc_modulation = FOCModulationType::SinePWM;
    motor->PID_velocity.P = 0.3;
    motor->PID_velocity.I = 15;
    motor->PID_velocity.D = 0.001;
    motor->PID_velocity.output_ramp = 1000;
    motor->LPF_velocity.Tf = 0.005;
    motor->LPF_angle.Tf = 0.005;
    motor->P_angle.P = 15;
    motor->velocity_limit = 40;
    motor->voltage_limit = 40;
    motor->sensor_direction = Direction::CCW;
    current_sense->skip_align = true;
    motor->init();
    motor->initFOC();
}


void send_data() {
    // TODO(vanyabeat) add can functionability for ROS packets
    CAN_TX_msg.id = 1;
    CAN_TX_msg.buf[0] = 'A';
    CAN_TX_msg.len = 5;
    Can.write(CAN_TX_msg);
    digitalWrite(PC11, !digitalRead(PC11));
}

void read_can_message() {
    float angle;
    memcpy(&angle, &CAN_inMsg.buf[1], sizeof(angle));
    motor.target = angle;
    digitalWrite(PC10, !digitalRead(PC10));
}

void run_foc(BLDCMotor *motor, Commander *commander) {
    motor->loopFOC();
    motor->move();
    motor->monitor();
    commander->run();
}

void setup() {
    Serial.setRx(HARDWARE_SERIAL_RX_PIN);
    Serial.setTx(HARDWARE_SERIAL_TX_PIN);
    Serial.begin(115200);
    // setup led pin
    pinMode(PC11, OUTPUT);
    pinMode(PC10, OUTPUT);
    Can.begin();
    Can.setBaudRate(1000000);
    TIM_TypeDef *Instance = TIM2;
    HardwareTimer *SendTimer = new HardwareTimer(Instance);
    SendTimer->setOverflow(100, HERTZ_FORMAT);  // 50 Hz
    SendTimer->attachInterrupt(send_data);
    SendTimer->resume();
    setup_foc(&sensor, &motor, &driver, &current_sense, &command, doMotor);
    _delay(1000);
}

float current_angle = 0;
void loop() {
    run_foc(&motor, &command);
    current_angle = sensor.getAngle();
    memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
    while (Can.read(CAN_inMsg)) {
        read_can_message();
    }
}