#include "apphardware.hpp"
using namespace iflytop;

#define TAG "AppHardware"

void AppHardware::initialize() {
  AppHal::UART3_Init();
  AppHal::UART4_Init();
  AppHal::MX_I2C1_Init();
  AppHal::MX_HSPI1_Init();

  tjcUart            = &huart4;
  remoteContolerUart = &huart3;

  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);
  ID1.initAsInput(ID1_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  ID2.initAsInput(ID2_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  ID3.initAsInput(ID3_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  ID4.initAsInput(ID4_IO, kxs_gpio_nopull, kxs_gpio_no_irq, false);
  ID5.initAsInput(ID5_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_IO_PE6.initAsInput(PE6, kxs_gpio_nopull, kxs_gpio_no_irq, false);



  osDelay(10);
  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(tmc5130cfg1);
  MOTO2.initialize(tmc5130cfg2);
  MOTO3.initialize(tmc5130cfg3);
  MOTO4.initialize(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.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);

//   ZCAN1::ins()->init();
 
}