#include "main.hpp"

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

//
#include "feite_servo_motor.hpp"
#include "sdk/os/zos.hpp"
//
#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
#include "sdk\components\eq_20_asb_motor\eq20_servomotor.hpp"
#include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp"
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp"
#include "sdk\components\step_motor_45\step_motor_45.hpp"
#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp"
#include "sdk\components\zcancmder\zcanreceiver_master.hpp"
//
#include "sdk\components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp"
#include "sdk\components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.hpp"
#include "sdk\components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp"
#include "sdk\components/xy_robot_ctrl_module/zcan_xy_robot_master_module.hpp"
#include "sdk\components\eq_20_asb_motor\script_cmder_eq20_servomotor.hpp"
#include "sdk\components\mini_servo_motor\mini_servo_motor_ctrl_module.hpp"
#include "sdk\components\mini_servo_motor\scirpt_cmder_mini_servo_motor_ctrl_module.hpp"
#include "sdk\components\step_motor_45\script_cmder_step_motor_45.hpp"
// #include "sdk\components\scriptcmder_module\xy_robot_script_cmder_module.hpp"
// #include "sdk\components\zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp"
// #include "sdk\components\zcancmder_master_module\zcan_xy_robot_master_module.hpp"

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

using namespace iflytop;
using namespace std;
#define CHECK_ARGC(n)            \
  if (argc != (n + 1)) {         \
    ZLOGE(TAG, "argc != %d", n); \
    context->breakflag = true;   \
    return;                      \
  }
extern "C" {
void StartDefaultTask(void const* argument) { iflytop::gmain.run(); }
}
/*******************************************************************************
 *                                    配置                                     *
 *******************************************************************************/
static chip_cfg_t chipcfg = {
    .us_dleay_tim          = &DELAY_US_TIMER,
    .tim_irq_scheduler_tim = &TIM_IRQ_SCHEDULER_TIMER,
    .huart                 = &DEBUG_UART,
    .debuglight            = DEBUG_LIGHT_GPIO,
};

static StepMotor45::cfg_t cfg1 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PB13,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PB15, PD11, PD12, PD13},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg2 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PG1,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PG2, PG3, PG4, PG5},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg3 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PB12,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PG6, PG7, PG8, PC6},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg4 = {
    .max_pos              = -1,
    .enable_zero_limit    = false,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PE0, PE2, PE4, PE6},
    .driverPinMirror = true,
};

static StepMotor45::cfg_t cfg5 = {
    .max_pos              = -1,
    .enable_zero_limit    = false,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PC13, PE5, PE3, PE1},
    .driverPinMirror = true,
};
static StepMotor45::cfg_t cfg6 = {
    .max_pos              = -1,
    .enable_zero_limit    = false,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PC12, PD3, PD5, PD7},
    .driverPinMirror = true,
};

namespace iflytop {
/*******************************************************************************
 *                                  基础组件                                   *
 *******************************************************************************/
ZCanCommnaderMaster m_zcanCommnaderMaster;
CmdScheduler        g_cmdScheduler;
ModbusBlockHost     g_modbusblockhost;
FeiTeServoMotor     g_feiteservomotor_bus;

/*******************************************************************************
 *                                  本地设备                                   *
 *******************************************************************************/
StepMotor45         g_step_motor45[7];
Eq20ServoMotor      g_main_servo_motor;
MiniRobotCtrlModule g_mini_servo[6];
/*******************************************************************************
 *                                   CAN设备                                   *
 *******************************************************************************/
I_XYRobotCtrlModule*   g_xyrobotctrlmodule = nullptr;
I_StepMotorCtrlModule* g_z_step_motor      = nullptr;

/*******************************************************************************
 *                                  串口指令                                   *
 *******************************************************************************/
StepMotorCtrlScriptCmderModule      g_step_motor_ctrl_script_cmder_module;
XYRobotScriptCmderModule            g_script_xyrobot;
ScriptCmderEq20Servomotor           g_script_eq20servomotor;
ScirptCmderMiniServoMotorCtrlModule g_script_mini_servo_motor_ctrl_module;
ScriptCmderStepMotor45              g_script_step_motor45;

}  // namespace iflytop

extern "C" {
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart3_tx;
}

extern "C" {}
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;

extern void step_motor_cmd_reg();
void        Main::run() {
  /*******************************************************************************
   *                                 系统初始化                                  *
   *******************************************************************************/

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  /*******************************************************************************
   *                                 总线初始化                                  *
   *******************************************************************************/
  // 指令总线
  g_cmdScheduler.initialize(&DEBUG_UART, 1000);
  // can总线
  auto* cfg = m_zcanCommnaderMaster.createCFG();
  m_zcanCommnaderMaster.init(cfg);
  // 飞特舵机总线
  g_feiteservomotor_bus.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);
  // modbus总线
  g_modbusblockhost.initialize(&huart2, &hdma_usart2_tx, &hdma_usart2_rx);
  g_main_servo_motor.init(&g_modbusblockhost, 1);
  /*******************************************************************************
   *                              设备构造和初始化                               *
   *******************************************************************************/
  g_z_step_motor = zcancmder_master::create_zcan_master_step_motor_ctrl_module(&m_zcanCommnaderMaster, 1);
  ZASSERT(g_z_step_motor != nullptr);

  g_xyrobotctrlmodule = zcancmder_master::create_xyrobot_ctrl_module(&m_zcanCommnaderMaster, 1);
  g_mini_servo[0].initialize(&g_feiteservomotor_bus, 1);
  g_mini_servo[1].initialize(&g_feiteservomotor_bus, 2);
  g_mini_servo[2].initialize(&g_feiteservomotor_bus, 3);
  g_mini_servo[3].initialize(&g_feiteservomotor_bus, 4);
  g_mini_servo[4].initialize(&g_feiteservomotor_bus, 5);
  g_mini_servo[5].initialize(&g_feiteservomotor_bus, 6);

  StepMotor45Scheduler step_motor45_scheduler;
  step_motor45_scheduler.initialize(&htim10);

  g_step_motor45[0].initialize(&step_motor45_scheduler, cfg1);
  g_step_motor45[1].initialize(&step_motor45_scheduler, cfg2);
  g_step_motor45[2].initialize(&step_motor45_scheduler, cfg3);
  g_step_motor45[3].initialize(&step_motor45_scheduler, cfg4);
  g_step_motor45[4].initialize(&step_motor45_scheduler, cfg5);
  g_step_motor45[5].initialize(&step_motor45_scheduler, cfg6);
  step_motor45_scheduler.start();

  /*******************************************************************************
   *                                     CMD                                     *
   *******************************************************************************/
  g_script_eq20servomotor.initialize(&g_cmdScheduler);
  g_script_eq20servomotor.regmodule(1, &g_main_servo_motor);

  g_step_motor_ctrl_script_cmder_module.initialize(&g_cmdScheduler);
  g_step_motor_ctrl_script_cmder_module.regmodule(1, g_z_step_motor);

  g_script_xyrobot.initialize(&g_cmdScheduler);
  g_script_xyrobot.regmodule(1, g_xyrobotctrlmodule);

  g_script_mini_servo_motor_ctrl_module.initialize(&g_cmdScheduler);
  g_script_mini_servo_motor_ctrl_module.regmodule(1, &g_mini_servo[0]);
  g_script_mini_servo_motor_ctrl_module.regmodule(2, &g_mini_servo[1]);
  g_script_mini_servo_motor_ctrl_module.regmodule(3, &g_mini_servo[2]);
  g_script_mini_servo_motor_ctrl_module.regmodule(4, &g_mini_servo[3]);
  g_script_mini_servo_motor_ctrl_module.regmodule(5, &g_mini_servo[4]);
  g_script_mini_servo_motor_ctrl_module.regmodule(6, &g_mini_servo[5]);

  g_script_step_motor45.initialize(&g_cmdScheduler);
  g_script_step_motor45.regmodule(1, &g_step_motor45[0]);
  g_script_step_motor45.regmodule(2, &g_step_motor45[1]);
  g_script_step_motor45.regmodule(3, &g_step_motor45[2]);
  g_script_step_motor45.regmodule(4, &g_step_motor45[3]);
  g_script_step_motor45.regmodule(5, &g_step_motor45[4]);
  g_script_step_motor45.regmodule(6, &g_step_motor45[5]);

#if 0
  step_motor_cmd_reg();
#endif
  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    g_cmdScheduler.schedule();
    osDelay(1);
  }
}