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

#include "board.h"
#include "sdk\components\subcanmodule\zcancmder_subboard_initer.hpp"
/*******************************************************************************
 *                               PROJECT_INCLUDE                               *
 *******************************************************************************/
#include "sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp"
#include "sdk\components\mini_servo_motor\feite_servo_motor.hpp"
#include "sdk\components\mini_servo_motor\mini_servo_motor_ctrl_module.hpp"
#include "sdk\components\pipette_module\pipette_ctrl_module_v2.hpp"
#include "sdk\components\sensors\m3078\m3078_code_scaner.hpp"
#include "sdk\components\tmc\ic\ztmc4361A.hpp"
#include "sdk\components\tmc\ic\ztmc5130.hpp"

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

static ZCancmderSubboardIniter initer;

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

extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart3_tx;
}
/*******************************************************************************
 *                                GET_DEVICE_ID                                *
 *******************************************************************************/
static ZGPIO   ID0;
static ZGPIO   ID1;
static ZGPIO   ID2;
static ZGPIO   ID3;
static ZGPIO   ID4;
static int32_t getDeviceId() {
  static bool init = false;

  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;
}
/*******************************************************************************
 *                               INIT_SUBMODULE                                *
 *******************************************************************************/
#define GLOBAL_SCALE   32
#define DEFAULT_ENABLE true

void        nvs_init_cb() {}
static void initsubmodule() {
  osDelay(1000);


  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO1_CSN,       //
                    .ennPin = MOTO1_DRV_ENN,   //
    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    // g_motor.rotate(1*1000);

    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(1), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO2_CSN,       //
                    .ennPin = MOTO2_DRV_ENN,   //
    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(2), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO3_CSN,       //
                    .ennPin = MOTO3_DRV_ENN,   //
    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(3), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO4_CSN,       //
                    .ennPin = MOTO4_DRV_ENN,   //
    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(4), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO5_CSN,       //
                    .ennPin = MOTO5_DRV_ENN,   //

    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(5), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO6_CSN,       //
                    .ennPin = MOTO6_DRV_ENN,   //

    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(6), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO7_CSN,       //
                    .ennPin = MOTO7_DRV_ENN,   //

    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(7), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }

  {
    static TMC5130             g_motor;
    static StepMotorCtrlModule g_stepMotorCtrlModule;
    TMC5130::cfg_t             cfg = {
                    .spi    = &TMC_MOTOR_SPI,  //
                    .csgpio = MOTO8_CSN,       //
                    .ennPin = MOTO8_DRV_ENN,   //

    };
    g_motor.initialize(&cfg);
    ZLOGI(TAG, "motora initialize 5160:%x ", g_motor.readICVersion());
    g_motor.enable(true);
    g_motor.setMotorShaft(false);
    g_motor.setGlobalScale(GLOBAL_SCALE);
    static ZGPIO input[10];
    // input[0].initAsInput(MOTOR0_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    // input[1].initAsInput(MOTOR0_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.motor_one_circle_pulse             = 1000;
    smcm_cfg.base_param.motor_one_circle_pulse_denominator = 1;
    smcm_cfg.base_param.stepmotor_ihold                    = 1;
    smcm_cfg.base_param.stepmotor_irun                     = 20;
    smcm_cfg.base_param.motor_shaft                        = 0;
    smcm_cfg.base_param.motor_default_velocity             = 800;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 0;
    smcm_cfg.base_param.max_d                              = 0;
    smcm_cfg.base_param.min_d                              = 0;
    smcm_cfg.base_param.motor_run_to_zero_max_d            = 0;
    smcm_cfg.base_param.motor_run_to_zero_speed            = 1;
    smcm_cfg.base_param.motor_run_to_zero_dec              = 0;
    smcm_cfg.base_param.motor_look_zero_edge_max_d         = 0;
    smcm_cfg.base_param.motor_look_zero_edge_speed         = 1;
    smcm_cfg.base_param.motor_look_zero_edge_dec           = 0;

    g_stepMotorCtrlModule.initialize(initer.get_module_id(8), &g_motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    g_stepMotorCtrlModule.enable(DEFAULT_ENABLE);
    initer.register_module(&g_stepMotorCtrlModule);
  }
}

/*******************************************************************************
 *                                    MAIN                                     *
 *******************************************************************************/
void umain() {
  ZCancmderSubboardIniter::cfg_t cfg =  //
      {
          .deviceId    = getDeviceId(),
          .input_gpio  = {},
          .output_gpio = {},
      };
  initer.init(&cfg);
  initsubmodule();
  initer.loop();
}