#include "main.hpp"

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

#include "main.h"
#include "project.hpp"
//
#include "one_dimensional_code_laser_scanner.hpp"
// #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp"
#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp"
#include "sdk/hal/zhal.hpp"
#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp"
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"

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

using namespace iflytop;

IflytopCanProtocolStackProcesser m_protocolStack;

TMC5130 m_tj_motor;
TMC5130 m_py_motor;

ZGPIO debuglight;

ZGPIO io_fybh_read1;
ZGPIO io_fybh_read2;
ZGPIO io_fybh_read3;
ZGPIO io_fybh_read4;
ZGPIO io_fybh_read5;
ZGPIO io_fybh_read6;

ZGPIO io_fybjl_sensor8;
ZGPIO io_fybjl_sensor7;
ZGPIO io_fybjl_sensor6;
ZGPIO io_fybjl_sensor5;
ZGPIO io_fybjl_sensor4;
ZGPIO io_fybjl_sensor3;
ZGPIO io_fybjl_sensor2;
ZGPIO io_fybjl_sensor1;
ZGPIO io_fybjl_sensor0;

DeviceBaseControlService       m_deviceBaseControlService;
IOControlService               m_ioControlService;
SingleAxisMotorControler       m_pyMotorControlService;  // 平移电机
SingleAxisMotorControler       m_tjMotorControlService;  // 推进电机
OneDimensionalCodeLaserScanner m_oneDimensionalCodeLaserScanner;

void input_sensors_init() {
  io_fybh_read1.initAsInput(FYBH_READ1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  io_fybh_read2.initAsInput(FYBH_READ2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  io_fybh_read3.initAsInput(FYBH_READ3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  io_fybh_read4.initAsInput(FYBH_READ4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  io_fybh_read5.initAsInput(FYBH_READ5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  io_fybh_read6.initAsInput(FYBH_READ6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);

  io_fybjl_sensor8.initAsInput(FYBJL_SENSOR8, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  io_fybjl_sensor7.initAsInput(FYBJL_SENSOR7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  io_fybjl_sensor6.initAsInput(FYBJL_SENSOR6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  io_fybjl_sensor5.initAsInput(FYBJL_SENSOR5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);
  io_fybjl_sensor4.initAsInput(FYBJL_SENSOR4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);
  io_fybjl_sensor3.initAsInput(FYBJL_SENSOR3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  io_fybjl_sensor2.initAsInput(FYBJL_SENSOR2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);
  io_fybjl_sensor1.initAsInput(FYBJL_SENSOR1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);
  io_fybjl_sensor0.initAsInput(FYBJL_SENSOR0, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/);
}


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

void Main::run() {
  ZHALCORE::cfg_t oscfg = {
      .delayhtim = &DELAY_US_TIMER,
      .debuguart = &DEBUG_UART,
  };
  ZHALCORE::getInstance()->initialize(oscfg);

  ZLOGI(TAG, "zapp:%s", VERSION);
  printf("int32_t %d int %d longint %d\n", sizeof(int32_t), sizeof(int), sizeof(long int));

  debuglight.initAsOutput(DEBUG_LIGHT_GPIO, ZGPIO::kMode_nopull, false, false);
  ZHAL_CORE_REG(200, { debuglight.toggleState(); });

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

  /*******************************************************************************
   *                                 电机初始化                                  *
   *******************************************************************************/
  {
    TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR0_ENN, .csn_pin = MOTOR0_CSN};

    m_py_motor.initialize(&cfg);
    int32_t chipv = m_py_motor.readChipVERSION();
    ZLOGI(TAG, "m_py_motor:%lx", chipv);
    m_py_motor.setIHOLD_IRUN(1, 31, 0);
    // m_py_motor.rotate(500000);
  }

  {
    TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN};

    m_tj_motor.initialize(&cfg);
    int32_t chipv = m_tj_motor.readChipVERSION();
    ZLOGI(TAG, "m_tj_motor:%lx", chipv);
    m_tj_motor.setIHOLD_IRUN(1, 31, 0);
    // m_tj_motor.rotate(500000);
  }

  /*******************************************************************************
   *                                扫码枪初始化                                 *
   *******************************************************************************/
  {
      // 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_deviceBaseControlService.initialize(&m_protocolStack, DEVICE_ID);
    m_deviceBaseControlService.setListener([](int32_t engineer_mode) {
      // m_pyMotorControlService.setEngineerMode(engineer_mode);
      // m_tjMotorControlService.setEngineerMode(engineer_mode);
    });
  }

  m_ioControlService.initialize(&m_protocolStack, GPIO_INPUT_ADD_BASE, nullptr, [this](int io_off) {
    if (io_off == 0) return io_fybjl_sensor0.getState();
    if (io_off == 1) return io_fybjl_sensor1.getState();
    if (io_off == 2) return io_fybjl_sensor2.getState();
    if (io_off == 3) return io_fybjl_sensor3.getState();
    if (io_off == 4) return io_fybjl_sensor4.getState();
    if (io_off == 5) return io_fybjl_sensor5.getState();
    if (io_off == 6) return io_fybjl_sensor6.getState();

    if (io_off == 7) return io_fybjl_sensor7.getState();
    if (io_off == 8) return io_fybjl_sensor8.getState();
    if (io_off == 9) return io_fybh_read1.getState();
    if (io_off == 10) return io_fybh_read2.getState();
    if (io_off == 11) return io_fybh_read3.getState();
    if (io_off == 12) return io_fybh_read4.getState();
    if (io_off == 13) return io_fybh_read5.getState();
    if (io_off == 14) return io_fybh_read6.getState();

    return false;
  });

  /*******************************************************************************
   *                                  横移电机                                   *
   *******************************************************************************/
  m_py_motor.setIHOLD_IRUN(2, 12, 0);
  m_py_motor.setMotorShaft(false);

  m_pyMotorControlService.initialize(  //
      "infeedMotor", &m_protocolStack, REG_PY_MOTOR_CTRL_ADD_BASE,
      &io_fybjl_sensor1,  // ZERO_GPIO
      NULL,               // LGPIO
      NULL,               // RGPIO
      &m_py_motor         // motor
  );

  m_pyMotorControlService.cfg_acc->setVal(30000);
  m_pyMotorControlService.cfg_dec->setVal(30000);
  m_pyMotorControlService.cfg_velocity->setVal(1000000);
  m_pyMotorControlService.cfg_zero_shift->setVal(0);
  m_pyMotorControlService.cfg_runhome_velocity->setVal(50000);
  m_pyMotorControlService.cfg_runtohome_dec->setVal(100000);
  m_pyMotorControlService.cfg_min_pos->setVal(-1000);
  m_pyMotorControlService.cfg_max_pos->setVal(1144028);
  m_pyMotorControlService.cfg_runtohome_max_distance->setVal(INT32_MAX);
  m_pyMotorControlService.cfg_runtohome_leave_zero_point_distance->setVal(256 * 200 * 1);

  /*******************************************************************************
   *                                  推杆电机                                   *
   *******************************************************************************/

  m_tj_motor.setIHOLD_IRUN(2, 12, 0);
  m_tj_motor.setMotorShaft(true);

  m_tjMotorControlService.initialize(  //
      "outfeedMotor", &m_protocolStack, REG_TJ_MOTOR_CTRL_ADD_BASE,
      &io_fybjl_sensor4,  // ZERO_GPIO
      NULL,               // LGPIO
      NULL,               // RGPIO
      &m_tj_motor         // motor
  );
  m_tjMotorControlService.cfg_acc->setVal(30000);
  m_tjMotorControlService.cfg_dec->setVal(30000);
  m_tjMotorControlService.cfg_velocity->setVal(1000000);
  m_tjMotorControlService.cfg_zero_shift->setVal(0);
  m_tjMotorControlService.cfg_runhome_velocity->setVal(50000);
  m_tjMotorControlService.cfg_runtohome_dec->setVal(100000);
  m_tjMotorControlService.cfg_min_pos->setVal(-1000);
  m_tjMotorControlService.cfg_max_pos->setVal(1130108);
  m_tjMotorControlService.cfg_runtohome_max_distance->setVal(INT32_MAX);
  m_tjMotorControlService.cfg_runtohome_leave_zero_point_distance->setVal(256 * 200 * 1);

  {
#if 0

扫描开始位置:518400
扫描结束位置:659200

#endif

    OneDimensionalCodeLaserScanner::cfg_t cfg;
    cfg.triggerPin   = FYBJL_SENSOR0;
    cfg.codestartpos = 518500;
    cfg.codeendpos   = 659200;
    cfg.readder      = [this]() { return m_tj_motor.getXACTUAL(); };
    m_oneDimensionalCodeLaserScanner.initialize(&m_protocolStack, REG_LARSER_SCANNER_CTRL_ADD_BASE, &cfg);
  }

  ZLOGI(TAG, "init done");
  while (1) {
    ZHALCORE::getInstance()->loop();
  }
}