#include "apphardware.hpp"

#include "ucomponents/zcan/zcan.hpp"
using namespace iflytop;

#define TAG "AppHardware"

void AppHardware::setTJCScreenInDownloadMode() {
  TJC_UART_CH_SEL.setState(true);
  TJC_UART_CH_EN.setState(false);
}

void AppHardware::initialize() {
  AppHal::rtc_init();

  ZCAN1::ins()->zcaninit(CAN1_TX_PIN, CAN1_RX_PIN);
  AppHal::tmc_spi_init();

  // TJC SCREEN UART INIT
  TJC_UART_CH_SEL.initAsOutput(TJC_UART_CH_SEL_PIN, kxs_gpio_nopull, false, false);
  TJC_UART_CH_EN.initAsOutput(TJC_UART_CH_EN_PIN, kxs_gpio_nopull, false, false);
  static_assert(&TJC_UART_INS == &huart4);
  AppHal::UART4_Init(TJC_UART_TX, TJC_UART_RX, 256000);
  tjcUart = &TJC_UART_INS;

  // BLE REMOTER UART INIT
  static_assert(&BLE_UART_INS == &huart3);
  AppHal::UART3_Init(BLE_UART_TX, BLE_UART_RX, 115200);
  remoteContolerUart = &BLE_UART_INS;

  MOTO_POWER_EN.initAsOutput(MOTO_POWER_EN_IO, kxs_gpio_nopull, false, true);
  MOTO1_CSN.initAsOutput(MOTO1_CSN_IO, kxs_gpio_nopull, false, true);
  MOTO2_CSN.initAsOutput(MOTO2_CSN_IO, kxs_gpio_nopull, false, true);
  MOTO3_CSN.initAsOutput(MOTO3_CSN_IO, kxs_gpio_nopull, false, true);
  MOTO4_CSN.initAsOutput(MOTO4_CSN_IO, kxs_gpio_nopull, false, true);
  MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO2_DRV_ENN.initAsOutput(MOTO2_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO3_DRV_ENN.initAsOutput(MOTO3_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO4_DRV_ENN.initAsOutput(MOTO4_DRV_ENN_IO, kxs_gpio_nopull, false, true);

  MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);
  MOTO1_DRV_ENN.initAsOutput(MOTO1_DRV_ENN_IO, kxs_gpio_nopull, false, true);

  MOTOR1_REF_L.initAsInput(MOTOR1_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR1_REF_R.initAsInput(MOTOR1_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR2_REF_L.initAsInput(MOTOR2_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR2_REF_R.initAsInput(MOTOR2_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR3_REF_L.initAsInput(MOTOR3_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR3_REF_R.initAsInput(MOTOR3_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR4_REF_L.initAsInput(MOTOR4_REF_L_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  MOTOR4_REF_R.initAsInput(MOTOR4_REF_R_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);

  id.ID0.initAsInput(ID0_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  id.ID1.initAsInput(ID1_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  id.ID2.initAsInput(ID2_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  id.ID3.initAsInput(ID3_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  id.ID4.initAsInput(ID4_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);

  IO_OUT1.initAsOutput(IO_OUT1_IO, kxs_gpio_nopull, true, false);
  IO_OUT2.initAsOutput(IO_OUT2_IO, kxs_gpio_nopull, true, false);

  BLE_CONNECTED_STATE.initAsInput(BLE_CONNECTED_STATE_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);

  osDelay(1500);
  MOTO_POWER_EN.setState(false);

  TMC51X0Cfg tmc5130cfg1 = {&MOTOR_SPI_INS, MOTO1_CSN_IO, MOTO1_DRV_ENN_IO};
  TMC51X0Cfg tmc5130cfg2 = {&MOTOR_SPI_INS, MOTO2_CSN_IO, MOTO2_DRV_ENN_IO};
  TMC51X0Cfg tmc5130cfg3 = {&MOTOR_SPI_INS, MOTO3_CSN_IO, MOTO3_DRV_ENN_IO};
  TMC51X0Cfg tmc5130cfg4 = {&MOTOR_SPI_INS, MOTO4_CSN_IO, MOTO4_DRV_ENN_IO};

  MOTO1.initialize(0, tmc5130cfg1);
  MOTO2.initialize(1, tmc5130cfg2);
  MOTO3.initialize(2, tmc5130cfg3);
  MOTO4.initialize(3, tmc5130cfg4);

  ZLOGI(TAG, "motor1 initialize TMC51X0:%x", MOTO1.readICVersion());
  ZLOGI(TAG, "motor2 initialize TMC51X0:%x", MOTO2.readICVersion());
  ZLOGI(TAG, "motor3 initialize TMC51X0:%x", MOTO3.readICVersion());
  ZLOGI(TAG, "motor4 initialize TMC51X0:%x", MOTO4.readICVersion());

  
  MOTO1.setGlobalScale(64);
  MOTO2.setGlobalScale(64);
  MOTO3.setGlobalScale(64);
  MOTO4.setGlobalScale(64);



  MOTO1.setIHOLD_IRUN(10, 31, 100);
  MOTO2.setIHOLD_IRUN(10, 31, 100);
  MOTO3.setIHOLD_IRUN(10, 31, 100);
  MOTO4.setIHOLD_IRUN(10, 31, 100);

  

  MOTO1.setScale(1000);
  MOTO2.setScale(1000);
  MOTO3.setScale(1000);
  MOTO4.setScale(1000);

  // MOTO1.enable(1);
  // MOTO2.enable(1);
  // MOTO3.enable(1);
  // MOTO4.enable(1);

  // MOTO1.rotate(1000);
  // MOTO2.rotate(1000);
  // MOTO3.rotate(1000);
  // MOTO4.rotate(1000);

  IO_OUT1.setState(false);
  IO_OUT2.setState(false);

  AppHal::MX_I2C1_Init();
  eeprom.initialize(&hi2c1);
  if (!eeprom.isOnline()) {
    ZLOGE(TAG, "eeprom is not online");
    return;
  }

  hardwareInitedOK = true;
  ZLOGI(TAG, "hardware initialize ok");
}

bool AppHardware::isHardInitOk() { return hardwareInitedOK; }