// 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);
/*      FLASH Related Definitions      */
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;

/* bool for test CAN    */
volatile bool CAN_GET = false;


volatile float kt = 0.1;  // Torque calculation constant

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 setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
               DRV8313Driver *driver, LowsideCurrentSense *current_sense,
               Commander *commander, CommandCallback callback) {
  encoder->init(&spi);

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

  // Current sense initialization
  current_sense->linkDriver(driver);
  current_sense->init();

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

  // PID Configuration
  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;

  // Motor limits
  motor->velocity_limit = 40;  // Speed limit in rad/s (382 rpm)
  motor->voltage_limit = 24;
  motor->current_limit = 0.5;

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

void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length = 7) {
  // Create CAN message
  CAN_message_t msg_l;
  msg_l.id = id;
  // msg_l.len = 8;  // Protocol-defined message length
  memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
  memcpy(&msg_l.buf[1], data, ID_SIZE);

  // Prepare CRC calculation buffer (ID + data)
  uint8_t crc_data[CAN_MSG_MAX_LEN];

  // Copy message ID 
  memcpy(crc_data, (uint8_t*)&msg_l.id, ID_SIZE);
  // Copy all data bytes
  for(int i = 1;i < CAN_MSG_MAX_LEN;i++)
    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1], sizeof(uint8_t));  //As byte variable
  // Calculate CRC
  uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN);
 
  // Insert CRC into buffer
  msg_l.buf[6] = crc_value & 0xFF;
  msg_l.buf[7] = (crc_value >> 8) & 0xFF;

  // Send message
  Can.write(msg_l);
}
  
void send_velocity() {  
  float current_velocity = motor.shaftVelocity();  
  if (flash_rec == nullptr) {  // Null check  
      // Error handling: logging, alerts, etc.  
      return;
  }
  uint8_t value = flash_rec[vel].value;
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'V',&value,sizeof(value));
}

void send_angle() {
  float current_angle = motor.shaftAngle();
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'A',&current_angle,sizeof(current_angle));
}

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() {
  /* Firmware data reading */
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }
  uint8_t value = flash_rec[foc_id].value;
  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'F',&value,sizeof(value));
}

void send_id() {
  /* Firmware data reading */
  if (flash_rec == nullptr) {  // Null check
      // Error handling: logging, alerts, etc.
      return;
  }

  uint8_t id = flash_rec[addr_id].value;
  send_can_with_id_crc(id,'I',&id,sizeof(id));
  __NOP();
}

void send_motor_torque() {
  float i_q = motor.current.q;  // Q-axis current (A)
  float torque = kt * i_q;      // Torque calculation
  torque *= 100;
  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 send_pid(uint8_t param_pid){
  if (flash_rec == nullptr) {  // Null check
    return;
  }
  uint8_t id = flash_rec[addr_id].value;
  uint8_t d = flash_rec[param_pid].value;
  uint8_t data_send = 0;
  int l = 0;
  while(d /= 10)
    l++;
  if(l >= 2)
    data_send = (float)d;
  else if(l == 1)
    data_send = (float)(d * 10);
  else
    data_send = (float)(d * 100);
  if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp;
  else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki;
  else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd;
  send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send));
}

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

void setup_angle(float target_angle) {
  motor.enable();  // Enable motor if disabled
  motor.controller = MotionControlType::angle;
  motor.move(target_angle);
}

void setup_pid_angle(uint8_t param_pid, float data){
  switch (param_pid) {
    case pid_p:
      motor.P_angle.P = data;
      break;
    case pid_i:
      motor.P_angle.I = data;
      break;
    case pid_d:
      motor.P_angle.D = data;
      break;
    default:
      break;
  }
  uint8_t check = uint8_t(data);
  uint8_t data_save = 0;
  if(check != 0)
    if(check /= 10)
      data_save = check;
    else 
      data_save = (uint8_t)(data * 10);
  else
    data_save = (uint8_t)(data * 100);
  write_param(param_pid,data_save);
}

void listen_can(const CAN_message_t &msg) {
  msg_id = msg.id;
  msg_ch = msg_id & 0xF;        // Extract message channel
  id_x = (msg_id >> 4) & 0x7FF; // Extract device address

  /* CRC Calculation */
  uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
  uint8_t data[10] = {0}; // Message buffer for CRC verification

  // Copy message ID (2 bytes)
  memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id));
  // Copy message data (excluding CRC bytes)
  memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2);
  
  // Calculate CRC
  uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2);
  
  // Verify CRC match
  if (calculated_crc != received_crc) {
    return; // Ignore message on CRC mismatch
  }
  flash_rec = load_params();
  /* Message Structure: 0x691
     69 - Device address
     1 - Action code */
  if(id_x == flash_rec[addr_id].value){
    if(msg_ch == REG_WRITE){
      switch(msg.buf[0]) {
        case REG_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_ANGLE:
          memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
                sizeof(motor_control_inputs.target_angle));
          setup_angle(motor_control_inputs.target_angle);
          break;
        case REG_MOTOR_POSPID_Kp:
          setup_pid_angle(pid_p,msg.buf[1]);
          break;
        case REG_MOTOR_POSPID_Ki:
          setup_pid_angle(pid_i,msg.buf[1]);
          break;
        case REG_MOTOR_POSPID_Kd:
          setup_pid_angle(pid_d,msg.buf[1]);
          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;
        case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break;
        case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break;
        case REG_MOTOR_POSPID_Kd: send_pid(pid_d); 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(){
  // Vector table initialization (commented out)
  // __set_MSP(*(volatile uintф32_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;
  // Can.enableMBInterrupts();
  Can.begin();
  Can.setBaudRate(1000000);
  // Настройка прерываний CAN
  CAN2->IER |= CAN_IER_FMPIE0;

  flash_rec = load_params();
  /*  For test          */
  // int value = 3;  //addr
  // write_param(addr_id,value); //for update address in firmware
  // // Load parameters from flash
  // flash_rec = load_params();

  for(int i = 0;i < PARAM_COUNT;i++)
    flash_buf[i] = flash_rec[i];
  
  // Initialize FOC system
  setup_foc(&encoder, &motor, &driver, &current_sense, &command, doMotor);
  
  CAN2->IER |= CAN_IER_FMPIE0 |  // Сообщение в FIFO0
  CAN_IER_FFIE0 |   // FIFO0 full
  CAN_IER_FOVIE0;    // FIFO0 overflow
  HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 1, 0);
  // HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);

  // Default motor configuration
  GPIOC->ODR |= GPIO_ODR_OD11;
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::torque;
  __enable_irq();
  
}



void loop() {
  __enable_irq();
  foc_step(&motor, &command);
  CAN_message_t msg;
  GPIOC->ODR ^= GPIO_ODR_OD11;  // Toggle status LED
  delay(500);
  
  // // Process incoming CAN messages
  while (Can.read(msg)) {
    CAN_GET = true;
  }
  /*  If receive data from  CAN */
  if(CAN_GET) {
    listen_can(msg);
  }
}