// 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 <iostream>
#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"
#include "config.h"
#include "process_can.h"
void SysTick_Handler(void) {
  HAL_IncTick();
}

STM32_CAN Can(CAN2, DEF);



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


volatile float kt = 0.1;  // Torque calculation constant

FLASH_RECORD* flash_rec;


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

MotorControlInputs motor_control_inputs;

 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 uint32_t ipsr_value = 0;



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