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

#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
#include "sdk/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp"
#include "sdk/os/zos.hpp"
#include "sdk\components\flash\zsimple_flash.hpp"
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"
#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\zcancmder_module\zcan_basic_order_module.hpp"
#include "sdk\components\zcancmder_module\zcan_xy_robot_module.hpp"
//
#include "sdk\components\flash\znvs.hpp"
//

#define TAG "main"
using namespace iflytop;
using namespace std;

extern void umain();
extern "C" {
void StartDefaultTask(void const* argument) { umain(); }
}

static TMC5130                 g_motor;
static ZCanCmder               g_zcanCmder;
static StepMotorCtrlModule     g_stepMotorCtrlModule;
static ZCanStepMotorCtrlModule g_zcanStepMotorCtrlModule;

uint8_t getId() {
  static bool  init = false;
  static ZGPIO ID0;
  static ZGPIO ID1;
  static ZGPIO ID2;
  static ZGPIO ID3;
  static ZGPIO ID4;
  if (!init) {
    ID0.initAsInput(ID0_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    ID1.initAsInput(ID1_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    ID2.initAsInput(ID2_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    ID3.initAsInput(ID3_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    ID4.initAsInput(ID4_IO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    init = true;
  }
  uint8_t id = ID0.getState() * 1 + ID1.getState() * 2 + ID2.getState() * 4 + ID3.getState() * 8 + ID4.getState() * 16;
  return id;
}

void umain() {
  chip_cfg_t chipcfg;
  chipcfg.us_dleay_tim          = &DELAY_US_TIMER;
  chipcfg.tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER;
  chipcfg.huart                 = &DEBUG_UART;
  chipcfg.debuglight            = DEBUG_LIGHT_GPIO;

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  uint8_t deviceId = getId();
  ZLOGI(TAG, "deviceId:%d", deviceId);

  /*******************************************************************************
   *                                   NVSINIT                                   *
   *******************************************************************************/
  ZNVS::ins().initialize(IFLYTOP_NVS_CONFIG_FLASH_SECTOR);
  {
    static I_XYRobotCtrlModule::flash_config_t cfg;
    XYRobotCtrlModule::create_default_cfg(cfg);
    ZNVS::ins().alloc_config(MOTOR_CFG_FLASH_MARK, (uint8_t*)&cfg, sizeof(cfg));
  }
  ZNVS::ins().init_config();
  // Pin_t              enn_pin;
  // Pin_t              csn_pin;
  osDelay(1000);
  {
    TMC5130::cfg_t cfg = {
        .spi             = &TMC_MOTOR_SPI,          //
        .csgpio          = MOTOR0_CSN,              //
        .ennPin          = MOTOR0_ENN,              //
        .spi_mode_select = MOTOR1_SPI_MODE_SELECT,  //
    };
    g_motor.initialize(&cfg);
    g_motor.setMotorShaft(false);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
  }

  g_motor.setAcceleration(300000);
  g_motor.setDeceleration(300000);
  g_motor.setIHOLD_IRUN(0, 8, 10);

  g_motor.rotate(0);
  // g_motor.enable(false);

  auto zcanCmder_cfg = g_zcanCmder.createCFG(deviceId);
  g_zcanCmder.init(zcanCmder_cfg);

  /*******************************************************************************
   *                            zcanBasicOrderModule                             *
   *******************************************************************************/
#if 0
  zcanBasicOrderModule.initialize(&zcanCmder);
  zcanBasicOrderModule.reg_set_io(1, [](bool val) { ZLOGI(TAG, "write io 1:%d", val); });
  zcanBasicOrderModule.reg_read_io(1, []() {
    ZLOGI(TAG, "read io 1");
    return 1;
  });
  zcanBasicOrderModule.reg_read_adc(1, []() {
    ZLOGI(TAG, "read adc 1");
    return 123;
  });
#endif

  /*******************************************************************************
   *                            zcanXYRobotCtrlModule                            *
   *******************************************************************************/
  static ZGPIO input[10];
  input[0].initAsInput(SENSOR_INT0, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[1].initAsInput(SENSOR_INT1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[2].initAsInput(SENSOR_INT2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[3].initAsInput(SENSOR_INT3, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[4].initAsInput(SENSOR_INT4, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[5].initAsInput(SENSOR_INT5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[6].initAsInput(SENSOR_INT6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[7].initAsInput(SENSOR_INT7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[8].initAsInput(SENSOR_INT8, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[9].initAsInput(SENSOR_INT9, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

  OSDefaultSchduler::getInstance()->regPeriodJob(
      [](OSDefaultSchduler::Context& context) {
        ZLOGI(TAG, "[0]:%d [1]:%d [2]:%d [3]:%d [4]:%d [5]:%d [6]:%d [7]:%d [8]:%d [9]:%d",  //
              input[0].getState(),                                                           //
              input[1].getState(),                                                           //
              input[2].getState(),                                                           //
              input[3].getState(),                                                           //
              input[4].getState(),                                                           //
              input[5].getState(),                                                           //
              input[6].getState(),                                                           //
              input[7].getState(),                                                           //
              input[8].getState(),                                                           //
              input[9].getState());
      },
      1000);

  //g_stepMotorCtrlModule.initialize(deviceId, &g_motor, input, ZARRAY_SIZE(input));
  g_zcanStepMotorCtrlModule.initialize(&g_zcanCmder, 1, &g_stepMotorCtrlModule);

  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    g_zcanCmder.loop();
  }
}