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

#include "sdk/components/step_motor_ctrl_module/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\ztmc4361A.hpp"
#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp"
#include "sdk\components\zcancmder\zcan_board_module.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\zcancmder_module\zcan_basic_order_module.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\protocol_parser.hpp"

//
#include "board.h"
#include "sdk\components\flash\znvs.hpp"
#include "sdk\components\subcanmodule\zcancmder_subboard_initer.hpp"
//

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

static ZCancmderSubboardIniter initer;

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

#define FLASH_MASK_XYRobotCtrlModule1 "XYRobotCtrlModule_1"

void           initmodule() {}
static int32_t getDeviceId() { return BOARD_ID; }
void           nvs_init_cb() {
  /**
   * @brief 初始化配置，系统中需要配置都需要在这里进行初始化
   */
  static I_XYRobotCtrlModule::flash_config_t cfg;
  XYRobotCtrlModule::create_default_cfg(cfg);
  ZNVS::ins().alloc_config(FLASH_MASK_XYRobotCtrlModule1, (uint8_t*)&cfg, sizeof(cfg));
}
static void initsubmodule() {
  static XYRobotCtrlModule xyRobotCtrlModule;
  static TMC4361A          motora;  // 1
  static TMC4361A          motorb;  // 2
  {
    TMC4361A::cfg_t cfg = {
        .spi               = &TMC_MOTOR_SPI,             //
        .csgpio            = TMC_MOTOR1_SPI_SELECT1_IO,  //
        .resetPin          = TMC_MOTOR1_nRESET_IO,       //
        .fREEZEPin         = TMC_MOTOR1_nFREEZE_IO,      //
        .ennPin            = PinNull,                    //
        .driverIC_ennPin   = TMC_MOTOR1_SUB_IC_ENN_IO,   //
        .driverIC_resetPin = PinNull,                    //
    };
    motora.initialize(&cfg);
    motora.setMotorShaft(false);
    motora.setGLOBAL_SCALER(32);
    ZLOGI(TAG, "motora initialize TMC4361A:%x DriverIC:%x", motora.readICVersion(), motora.readSubICVersion());
  }

  {
    TMC4361A::cfg_t cfg = {
        .spi               = &TMC_MOTOR_SPI,             //
        .csgpio            = TMC_MOTOR2_SPI_SELECT1_IO,  //
        .resetPin          = TMC_MOTOR2_nRESET_IO,       //
        .fREEZEPin         = TMC_MOTOR2_nFREEZE_IO,      //
        .ennPin            = PinNull,                    //
        .driverIC_ennPin   = TMC_MOTOR2_SUB_IC_ENN_IO,   //
        .driverIC_resetPin = PinNull,                    //
    };
    motorb.initialize(&cfg);
    motorb.setMotorShaft(false);
    motorb.setGLOBAL_SCALER(32);
    ZLOGI(TAG, "motorb initialize TMC4361A:%x DriverIC:%x", motorb.readICVersion(), motorb.readSubICVersion());
  }

  motora.setAcceleration(300);
  motora.setDeceleration(300);
  motora.setIHOLD_IRUN(0, 4, 10);

  motorb.setAcceleration(300);
  motorb.setDeceleration(300);
  motorb.setIHOLD_IRUN(0, 4, 10);

  motora.rotate(0);
  motorb.rotate(0);

  /*******************************************************************************
   *                            zcanXYRobotCtrlModule                            *
   *******************************************************************************/
  static ZGPIO input[4];
  input[0].initAsInput(ARM_X_ZERO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[1].initAsInput(ARM_Y_ZERO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[2].initAsInput(ARM_X_LIMIT, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);
  input[3].initAsInput(ARM_Y_LIMIT, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true);

  static XYRobotCtrlModule::flash_config_t xy_defaultcfg;
  XYRobotCtrlModule::create_default_cfg(xy_defaultcfg);
  xy_defaultcfg.configInited       = true;
  xy_defaultcfg.basecfg.robot_type = 1;

  xy_defaultcfg.basecfg.robot_type     = I_XYRobotCtrlModule::hbot;
  xy_defaultcfg.basecfg.x_shaft        = 0;
  xy_defaultcfg.basecfg.y_shaft        = 1;
  xy_defaultcfg.basecfg.ihold          = 1;
  xy_defaultcfg.basecfg.irun           = 16;
  xy_defaultcfg.basecfg.iholddelay     = 100;
  xy_defaultcfg.basecfg.distance_scale = 7344;
  xy_defaultcfg.basecfg.shift_x        = 0;
  xy_defaultcfg.basecfg.shift_y        = 0;

  // limit
  xy_defaultcfg.basecfg.acc      = 800;
  xy_defaultcfg.basecfg.dec      = 800;
  xy_defaultcfg.basecfg.breakdec = 1600;
  xy_defaultcfg.basecfg.maxspeed = 600;
  xy_defaultcfg.basecfg.min_x    = 0;
  xy_defaultcfg.basecfg.max_x    = 0;
  xy_defaultcfg.basecfg.min_y    = 0;
  xy_defaultcfg.basecfg.max_y    = 0;

  xy_defaultcfg.basecfg.run_to_zero_max_d    = 10000 * 100;
  xy_defaultcfg.basecfg.run_to_zero_speed    = 80;
  xy_defaultcfg.basecfg.run_to_zero_dec      = 1600;
  xy_defaultcfg.basecfg.look_zero_edge_max_d = 10000 * 3;
  xy_defaultcfg.basecfg.look_zero_edge_speed = 10;
  xy_defaultcfg.basecfg.look_zero_edge_dec   = 1600;

  xyRobotCtrlModule.initialize(initer.get_module_id(1), &motora, &motorb, &input[0], 4, xy_defaultcfg, nullptr);
  xyRobotCtrlModule.dumpcfg();

  initer.register_module(&xyRobotCtrlModule);
}

void umain() {
  ZCancmderSubboardIniter::cfg_t cfg =  //
      {
          .deviceId = getDeviceId(),
          .input_gpio =
              {
                  {ARM_SENSOR5_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true},
                  {ARM_SENSOR6_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true},
                  {ARM_SENSOR7_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true},
                  {ARM_SENSOR8_GPIO, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true},
              },
          .output_gpio =
              {
                  //  {PD0, ZGPIO::kMode_nopull, false, false},
              },
      };
  initer.init(&cfg);
  initsubmodule();
  initer.loop();
}