#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\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(); }
}
/*******************************************************************************
 *                                GET_DEVICE_ID                                *
 *******************************************************************************/
static int32_t getDeviceId() { return BOARD_ID; }
/*******************************************************************************
 *                               INIT_SUBMODULE                                *
 *******************************************************************************/
void        nvs_init_cb() {}
static void initsubmodule() {
  /**
   * @brief 入料
   */
  {
    static TMC5130             motor;
    static StepMotorCtrlModule stepMotorCtrlModule;

    TMC5130::cfg_t cfg = {
        .spi             = &TMC_MOTOR_SPI,          //
        .csgpio          = MOTOR1_CSN,              //
        .ennPin          = MOTOR1_ENN,              //
        .spi_mode_select = MOTOR1_SPI_MODE_SELECT,  //
    };
    motor.initialize(&cfg);
    motor.setMotorShaft(false);
    ZLOGI(TAG, "motor1 initialize 5160:%x ", motor.readICVersion());

    static ZGPIO input[2];
    input[0].initAsInput(MOTOR1_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    input[1].initAsInput(MOTOR1_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    I_StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.distance_scale             = 200;
    smcm_cfg.base_param.distance_scale_denominator = 1;
    smcm_cfg.base_param.irun                       = 24;
    smcm_cfg.base_param.ihold                      = 1;
    smcm_cfg.base_param.x_shaft                    = false;
    smcm_cfg.base_param.maxspeed                   = 150;

    stepMotorCtrlModule.initialize(initer.get_module_id(1), &motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    initer.register_module(&stepMotorCtrlModule);
  }
  /**
   * @brief 平移
   */
  {
    static TMC5130             motor;
    static StepMotorCtrlModule stepMotorCtrlModule;

    TMC5130::cfg_t cfg = {
        .spi             = &TMC_MOTOR_SPI,          //
        .csgpio          = MOTOR2_CSN,              //
        .ennPin          = MOTOR2_ENN,              //
        .spi_mode_select = MOTOR2_SPI_MODE_SELECT,  //
    };
    motor.initialize(&cfg);
    motor.setMotorShaft(false);
    ZLOGI(TAG, "motor2 initialize 5160:%x ", motor.readICVersion());

    static ZGPIO input[2];
    input[0].initAsInput(MOTOR2_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    input[1].initAsInput(MOTOR2_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    I_StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.distance_scale             = 100;
    smcm_cfg.base_param.distance_scale_denominator = 1;
    smcm_cfg.base_param.irun                       = 24;
    smcm_cfg.base_param.ihold                      = 1;
    smcm_cfg.base_param.x_shaft                    = true;
    smcm_cfg.base_param.maxspeed                   = 50;
    smcm_cfg.base_param.run_to_zero_speed          = 50;

    stepMotorCtrlModule.initialize(initer.get_module_id(2), &motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    initer.register_module(&stepMotorCtrlModule);
  }

  /**
   * @brief 出料
   */
  {
    static TMC5130             motor;
    static StepMotorCtrlModule stepMotorCtrlModule;

    TMC5130::cfg_t cfg = {
        .spi             = &TMC_MOTOR_SPI,          //
        .csgpio          = MOTOR3_CSN,              //
        .ennPin          = MOTOR3_ENN,              //
        .spi_mode_select = MOTOR3_SPI_MODE_SELECT,  //
    };
    motor.initialize(&cfg);
    motor.setMotorShaft(false);
    ZLOGI(TAG, "motor3 initialize 5160:%x ", motor.readICVersion());

    static ZGPIO input[2];
    input[0].initAsInput(MOTOR3_REFL /*REFL*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
    input[1].initAsInput(MOTOR3_REFR /*REFR*/, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

    I_StepMotorCtrlModule::flash_config_t smcm_cfg = {0};
    StepMotorCtrlModule::create_default_cfg(smcm_cfg);
    smcm_cfg.base_param.distance_scale             = 100;
    smcm_cfg.base_param.distance_scale_denominator = 1;
    smcm_cfg.base_param.irun                       = 24;
    smcm_cfg.base_param.ihold                      = 1;
    smcm_cfg.base_param.x_shaft                    = true;
    smcm_cfg.base_param.maxspeed                   = 50;
    smcm_cfg.base_param.run_to_zero_speed          = 50;

    stepMotorCtrlModule.initialize(initer.get_module_id(3), &motor, input, ZARRAY_SIZE(input), nullptr, &smcm_cfg);
    initer.register_module(&stepMotorCtrlModule);
  }
#if 0
  {
    static M3078CodeScanner                    codescanner;
    static M3078CodeScanner::hardware_config_t cfg = {
        .uart    = &huart3,
        .hdma_rx = nullptr,
        .hdma_tx = nullptr,

        .codeReadOkPin = PinNull,
        .rstPin        = PinNull,
        .triggerPin    = PE15,
    };
    codescanner.initialize(BOARD_ID * 10 + 4, &cfg);
    g_ziProtocolParser.registerModule(&codescanner);
  }
#endif
}

/*******************************************************************************
 *                                    MAIN                                     *
 *******************************************************************************/
void umain() {
  ZCancmderSubboardIniter::cfg_t cfg =  //
      {
          .deviceId = getDeviceId(),
          .input_gpio =
              {
                  {.pin = PD0, .mode = ZGPIO::kMode_nopull, .irqtype = ZGPIO::kIRQ_noIrq, .mirror = true},
                  {.pin = PD1, .mode = ZGPIO::kMode_nopull, .irqtype = ZGPIO::kIRQ_noIrq, .mirror = true},
                  {.pin = PD2, .mode = ZGPIO::kMode_nopull, .irqtype = ZGPIO::kIRQ_noIrq, .mirror = true},
                  {.pin = PD3, .mode = ZGPIO::kMode_nopull, .irqtype = ZGPIO::kIRQ_noIrq, .mirror = true},
                  {.pin = PD4, .mode = ZGPIO::kMode_nopull, .irqtype = ZGPIO::kIRQ_noIrq, .mirror = true},
              },
          .output_gpio = {},
      };
  initer.init(&cfg);
  initsubmodule();
  initer.loop();
}