// clang-format off
#include <Arduino.h>
#include <SimpleFOC.h>
#include <STM32_CAN.h>
#include <AS5045.h>
#include <DRV8313.h>
#include <cstring>
#include <iterator>
#include "common/base_classes/FOCMotor.h"
#include "hal_conf_extra.h"
#include "wiring_analog.h"
#include "wiring_constants.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 encoder(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.01, 10.0, CURRENT_SENSOR_1,
                                  CURRENT_SENSOR_2, CURRENT_SENSOR_3);

Commander command(Serial);

struct MotorControlInputs {
  float target_angle;
  float target_velocity;
  bool motor_enabled;
};

MotorControlInputs motor_control_inputs;

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);
  motor->linkSensor(encoder);
  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';
  // motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
  motor->monitor_downsample = 10; // default 10
  commander->verbose = VerboseMode::user_friendly;
  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->current_limit = 0.5;
  motor->sensor_direction = Direction::CCW;
  current_sense->skip_align = true;
  motor->init();
  motor->initFOC();
}

void send_velocity() {
  float current_velocity = motor.shaftVelocity();
  CAN_TX_msg.id = 1;
  CAN_TX_msg.buf[0] = 'V';
  CAN_TX_msg.len = 5;
  memcpy(&CAN_TX_msg.buf[1], &current_velocity, sizeof(current_velocity));
  Can.write(CAN_TX_msg);
}

void send_angle() {
  float current_angle = motor.shaftAngle();
  CAN_TX_msg.id = 1;
  CAN_TX_msg.buf[0] = 'A';
  CAN_TX_msg.len = 5;
  memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
  Can.write(CAN_TX_msg);
}

void send_motor_enabled() {
  CAN_TX_msg.id = 1;
  CAN_TX_msg.buf[0] = 'E';
  memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled,
         sizeof(motor_control_inputs.motor_enabled));
  Can.write(CAN_TX_msg);
}

void send_data() {
  // TODO(vanyabeat) add can functionability for ROS packets
  send_velocity();
  send_angle();
  send_motor_enabled();
  // read_temperature();
  digitalWrite(PC11, !digitalRead(PC11));
}

void read_can_step() {
  char flag = CAN_inMsg.buf[0];
  if (flag == 'V') {
    memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1],
           sizeof(motor_control_inputs.target_velocity));
  } else if (flag == 'A') {
    memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
           sizeof(motor_control_inputs.target_angle));
  } else if (flag == 'E') {
    bool enable_flag = CAN_inMsg.buf[1];
    if (enable_flag == 1) {
      memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1],
             sizeof(motor_control_inputs.motor_enabled));
      motor.enable();
    } else if (enable_flag == 0) {
      memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1],
             sizeof(motor_control_inputs.motor_enabled));
      motor.disable();
    }
  }
  // motor.target = angle;
  digitalWrite(PC10, !digitalRead(PC10));
}

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();
}

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);
  // Setup thermal sensor pin
  // pinMode(TH1, INPUT_ANALOG);
  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(&encoder, &motor, &driver, &current_sense, &command, doMotor);
  _delay(1000);
}

float current_angle = 0.0;
void loop() {
  foc_step(&motor, &command);
  // current_angle = encoder.getAngle();
  // memcpy(&CAN_TX_msg.buf[1], &current_angle, sizeof(current_angle));
  while (Can.read(CAN_inMsg)) {
    read_can_step();
  }
}