// clang-format off
#include "Arduino.h"
#include "stm32f446xx.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
#include "reg_cah.h"
#include "flash.h"


void SysTick_Handler(void) {
  HAL_IncTick();
}


STM32_CAN Can(CAN2, DEF);
/*      for FLASH         */
uint32_t flash_flag;
uint8_t flag_can = 0;
uint32_t flash_error;
FLASH_EraseInitTypeDef pEraseInit;
uint32_t SectorError;

volatile uint16_t msg_id;
volatile uint16_t id_x;
volatile uint8_t msg_ch;
volatile uint8_t crc_h;
volatile uint8_t crc_l;

volatile float kt = 0.1;  //for torgue calculation

static FLASH_RECORD* flash_rec;
static FLASH_RECORD flash_buf[PARAM_COUNT];
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 = 0.0;
  float target_velocity = 0.0;
  bool motor_enabled = false;
  bool foc_state = false;
};

MotorControlInputs motor_control_inputs;

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

void CAN2_RX0_IRQHandler() {
  // Пустая функция, но прерывание не приведет к Default Handler
}

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

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

  current_sense->linkDriver(driver);
  current_sense->init();

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

  //  PID start
  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;
  //  PID end

  motor->velocity_limit = 40;  // Ограничение по скорости вращения rad/s (382 rpm)
  motor->voltage_limit = 24;
  motor->current_limit = 0.5;

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

void send_velocity() {
  float current_velocity = motor.shaftVelocity();
  uint8_t id = *(volatile uint8_t*)ADDR_VAR;
  CAN_TX_msg.id = id;
  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();
  uint8_t id = *(volatile uint8_t*)ADDR_VAR;
  CAN_TX_msg.id = id;
  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() {
  uint8_t id = *(volatile uint8_t*)ADDR_VAR;
  CAN_TX_msg.id = id;
  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_foc_state() {
  uint8_t id = *(volatile uint8_t*)ADDR_VAR;
  CAN_TX_msg.id = id;
  CAN_TX_msg.buf[0] = 'F';
  memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state,
    sizeof(motor_control_inputs.foc_state));
Can.write(CAN_TX_msg);
}

void send_id() {
  /*          data for reading of firmware      */
  flash_rec = load_params();
  if (flash_rec == nullptr) { // Проверка на NULL
      // Обработка ошибки: запись в лог, сигнализация и т.д.
      return;
  }
  CAN_TX_msg.id = flash_rec->value;
  CAN_TX_msg.len = 8;
  CAN_TX_msg.buf[0] = 'I';
  memcpy(&CAN_TX_msg.buf[1], &(flash_rec->value), sizeof(uint8_t));

  uint8_t crc_data[sizeof(CAN_TX_msg.id) + 2] = {0}; // Размер: размер ID + 2 байта данных
  memcpy(crc_data, &CAN_TX_msg.id, sizeof(CAN_TX_msg.id)); // Копируем ID (11/29 бит)
  memcpy(crc_data + sizeof(CAN_TX_msg.id), &CAN_TX_msg.buf[0], 2); // Копируем 'I' и value
  // Расчет CRC 
  uint16_t crc_value = validate_crc16(crc_data, sizeof(crc_data));

  // Добавление CRC к сообщению
  CAN_TX_msg.buf[6] = crc_value & 0xFF; // Добавляем старший байт CRC
  CAN_TX_msg.buf[7] = (crc_value >> 8) & 0xFF; // Добавляем младший байт CRC

  Can.write(CAN_TX_msg);
  __NOP();
}

void send_motor_torque() {
  float i_q = motor.current.q; // Ток по оси q (А)
  float torque = kt * i_q; // Расчет момента
  torque *= 100;
  flash_rec = load_params();
  CAN_TX_msg.id = flash_rec->value;
  CAN_TX_msg.buf[0] = 'T';
  CAN_TX_msg.len = 5;
  memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque));
  Can.write(CAN_TX_msg);
}

void setup_id(uint8_t my_id) {
  write_param(addr_id,my_id);
  send_id();
}



void send_data() {
  // send_velocity();
  // send_angle();
  // send_motor_enabled();
  // read_temperature();
  // GPIOC->ODR ^= GPIO_ODR_OD11;
}

void listen_can(const CAN_message_t &msg) {
  msg_id = msg.id;

  msg_ch = msg_id & 0xF;         // получения id, чтобы выбрать, что делать
  id_x = (msg_id >> 4) & 0x7FF;  //получение адреса устройства страшие 2 бита  msg_ch  = msg_id & 0xF;         // получения id, чтобы выбрать, что делать
  
  
  /*          Вычисление CRC          */
  // Объединение старшего и младшего байтов для получения полученного CRC
  uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
  uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки

  // Копируем ID сообщения в буфер данных для расчета CRC 2 байта
  memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
  
  // Копируем данные сообщения в буфер (без байтов CRC)
  memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
  
  // Рассчитываем CRC для полученных данных
  uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
  
  // Проверяем совпадение CRC
  if (calculated_crc != received_crc) {
    // Несовпадение CRC, игнорируем сообщение
    return;
  }


  /*          0x691  
  69  -   адрес устройства    
  1   -   что делать дальше с данными     */
  
  if(id_x == flash_rec->value){
    if(msg_ch == REG_WRITE){
    switch(msg.buf[0]) {
      case REG_ID:
  /*      setup new id            */
        setup_id(msg.buf[1]); 
        break;

      case REG_LED_BLINK:
        for (int i = 0; i < 10; i++) {
          GPIOC->ODR ^= GPIO_ODR_OD10;
         delay(100);
        }
      break;

      case MOTOR_ENABLED:
        if (msg.buf[1] == 1) {
          motor.enable();
          motor_control_inputs.motor_enabled = 1;
    }   else {
          motor.disable();
          motor_control_inputs.motor_enabled = 0;
      }

    default:
      break;
    }
  }
 else if (msg_ch == REG_READ) {
    switch (msg.buf[0]) {
    case REG_ID:
      send_id();
      break;
    case MOTOR_VELOCITY:
      send_velocity(); 
      break;

    case MOTOR_ANGLE:
      send_angle();
      break;

    case MOTOR_ENABLED:
      send_motor_enabled();
      break;

    case MOTOR_TORQUE:
      send_motor_torque();
      break;

    case FOC_STATE:
      send_foc_state();
      break;
    
    default:
      break;
    }
  }
}
}




volatile uint32_t ipsr_value = 0;


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(){
  /*   bias for vector int    */
  // __set_MSP(*(volatile uint32_t*)0x08008000);
  // SCB->VTOR = (volatile uint32_t)0x08008000;
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
Serial.begin(115200);

pinMode(PC11, OUTPUT);
pinMode(PC10,OUTPUT);
GPIOC->ODR &= ~GPIO_ODR_OD10;
// 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();
  flash_rec = load_params();
  for(int i = 0;i < PARAM_COUNT;i++)
    flash_buf[i] = flash_rec[i];
  setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
  GPIOC->ODR |= GPIO_ODR_OD11;
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::torque;
  __enable_irq();
  }

void loop() {
 foc_step(&motor, &command);
  CAN_message_t msg;
  GPIOC->ODR ^= GPIO_ODR_OD11;
  delay(500);
  while (Can.read(msg)) {
    listen_can(msg);
  }
}