#include "main.hpp"

#include <stddef.h>
#include <stdio.h>

#include "main.h"
#include "project.hpp"
//
#include "sdk/hal/zhal.hpp"
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"

#define TAG "main"
namespace iflytop {
Main gmain;
};

using namespace iflytop;

IflytopCanProtocolStackProcesser m_protocolStack;
TMC4361A                         m_motora;
TMC4361A                         m_motorb;

void input_sensors_init() {
  ARM_SENSOR1_GPIO.initAsInput(STM32_GPIO::kInput_risingAndFallingIrq, true);
  ARM_SENSOR2_GPIO.initAsInput(STM32_GPIO::kInput_risingAndFallingIrq, true);
  ARM_SENSOR3_GPIO.initAsInput();
  ARM_SENSOR4_GPIO.initAsInput();
  ARM_SENSOR5_GPIO.initAsInput();
  ARM_SENSOR6_GPIO.initAsInput();
  ARM_SENSOR7_GPIO.initAsInput(STM32_GPIO::kInput_risingAndFallingIrq, true);
  ARM_SENSOR8_GPIO.initAsInput(STM32_GPIO::kInput_risingAndFallingIrq, true);
}
uint32_t intput_sensors_get_table0() {
  uint32_t val = 0;
  val |= ARM_SENSOR1_GPIO.getStateUint32() << 1;
  val |= ARM_SENSOR2_GPIO.getStateUint32() << 2;
  val |= ARM_SENSOR3_GPIO.getStateUint32() << 3;
  val |= ARM_SENSOR4_GPIO.getStateUint32() << 4;
  val |= ARM_SENSOR5_GPIO.getStateUint32() << 5;
  val |= ARM_SENSOR6_GPIO.getStateUint32() << 6;
  val |= ARM_SENSOR7_GPIO.getStateUint32() << 7;
  val |= ARM_SENSOR8_GPIO.getStateUint32() << 8;
  return val;
}

STM32_GPIO *input_sensors_get_arm1_homegpio() { return &ARM_SENSOR8_GPIO; }
STM32_GPIO *input_sensors_get_arm2_homegpio() { return &ARM_SENSOR7_GPIO; }

icps::error_t Main::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser *processer, icps::WriteEvent *event) {}
icps::error_t Main::onHostRegisterReadEvent(IflytopCanProtocolStackProcesser *processer, icps::ReadEvent *event) {}
void          Main::onHostRegisterReportEvent(IflytopCanProtocolStackProcesser *processer, icps::ReportEvent *event) {}

void Main::run() {
  iflytop_no_os_cfg_t oscfg = {
      .delayhtim = &DELAY_US_TIMER,
      .debuguart = &DEBUG_UART,
  };
  iflytop_no_os_init(&oscfg);

  ZLOGI(TAG, "robotic_core_xy:%s", VERSION);

  DEBUG_LIGHT_GPIO.initAsOutput(true);
  ZHAL_CORE_REG(200, { DEBUG_LIGHT_GPIO.toggleState(); });

  /*******************************************************************************
   *                               GPIO输入初始化                                *
   *******************************************************************************/
  input_sensors_init();
  intput_sensors_get_table0();

  /*******************************************************************************
   *                                 电机初始化                                  *
   *******************************************************************************/
  TMC_MOTOR1_SPI_SELECT1_IO.initAsOutput(true);
  TMC_MOTOR1_nFREEZE_IO.initAsOutput(true);
  TMC_MOTOR1_nRESET_IO.initAsOutput(true);
  TMC_MOTOR1_SUB_IC_ENN_IO.initAsOutput(true);
  TMC_MOTOR2_SPI_SELECT1_IO.initAsOutput(true);
  TMC_MOTOR2_nFREEZE_IO.initAsOutput(true);
  TMC_MOTOR2_nRESET_IO.initAsOutput(true);
  TMC_MOTOR2_SUB_IC_ENN_IO.initAsOutput(true);
  {
    TMC4361A::cfg_t motora_cfg = {.spi               = &TMC_MOTOR_SPI,
                                  .csgpio            = &TMC_MOTOR1_SPI_SELECT1_IO,
                                  .resetPin          = &TMC_MOTOR1_nRESET_IO,
                                  .fREEZEPin         = &TMC_MOTOR1_nFREEZE_IO,
                                  .ennPin            = NULL,
                                  .driverIC_ennPin   = &TMC_MOTOR1_SUB_IC_ENN_IO,
                                  .driverIC_resetPin = NULL};

    m_motora.initialize(&motora_cfg);
    int32_t ic4361Version = m_motora.readICVersion();
    int32_t ic2160Version = m_motora.readSubICVersion();
    ZLOGI(TAG, "m_motora:TMC4361Version:%lx  TMC2160VERSION:%lx", ic4361Version, ic2160Version);
  }

  {
    TMC4361A::cfg_t motorb_cfg = {.spi               = &TMC_MOTOR_SPI,
                                  .csgpio            = &TMC_MOTOR2_SPI_SELECT1_IO,
                                  .resetPin          = &TMC_MOTOR2_nRESET_IO,
                                  .fREEZEPin         = &TMC_MOTOR2_nFREEZE_IO,
                                  .ennPin            = NULL,
                                  .driverIC_ennPin   = &TMC_MOTOR2_SUB_IC_ENN_IO,
                                  .driverIC_resetPin = NULL};

    m_motorb.initialize(&motorb_cfg);
    int32_t ic4361Version = m_motorb.readICVersion();
    int32_t ic2160Version = m_motorb.readSubICVersion();
    ZLOGI(TAG, "m_motorb:TMC4361Version:%lx  TMC2160VERSION:%lx", ic4361Version, ic2160Version);
  }

  /*******************************************************************************
   *                                扫码枪初始化                                 *
   *******************************************************************************/
  {
    // static ZUART uart;
    // PB6.initAsOutput(STM32_GPIO::kOutput_nopull, true, false);
    // ZUART::cfg_t cfg = {
    //     .name          = "CODE_SCANER_UART",
    //     .huart         = &CODE_SCANER_UART,
    //     .rxbuffersize  = 300,
    //     .rxovertime_ms = 10,
    // };
    // uart.initialize(&cfg,[](){})
    //     uart.initialize_basic("CODE_SCANER_UART", &m_hardware, &CODE_SCANER_UART);
    // uart.initialize_setRxBuffer(300);
    // uart.initialize_setRxOvertime(33);
    // uart.initialize_finished();
  }

  /*******************************************************************************
   *                                协议栈初始化                                 *
   *******************************************************************************/
  {
    auto *cfg = IflytopCanProtocolStackProcesser::createDefaultConfig(DEVICE_ID, 128);
    m_protocolStack.initialize(cfg);
    m_protocolStack.setDumpPacketFlag(false);
    m_protocolStack.registerListener(this);
    m_protocolStack.activeReg(REG_GPIO_INPUT0, icps::kwr, 0);
  }

  m_motora.setIHOLD_IRUN(1, 12, 0);  // 小臂
  m_motora.setMotorShaft(0);

  m_motorb.setIHOLD_IRUN(1, 12, 0);  // 小臂
  m_motorb.setMotorShaft(0);

  m_motora.rotate(1200000);
  m_motorb.rotate(1200000);

  while (1) {
    ZHALCORE::getInstance()->loop();
  }
}