#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\scriptcmder_module\xy_robot_script_cmder_module.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\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    = false,
    .enable_max_pos_limit = false,
    .mirror               = true,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .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 {
StepMotor45              g_step_motor45[7];
StepMotor                g_step_motor[10];
ZCanCommnaderMaster      m_zcanCommnaderMaster;
CmdScheduler             cmdScheduler;
XYRobotScriptCmderModule xyRobotScriptCmderModule;

ModbusBlockHost      g_modbusblockhost;
Eq20ServoMotor       g_eq20servomotor;
FeiTeServoMotor      g_feiteservomotor;
I_XYRobotCtrlModule* g_xyrobotctrlmodule = nullptr;

}  // namespace iflytop

void regfn() {
  // setzero
  // set4095
  // moveto

  cmdScheduler.registerCmd("mini_servo_set_zero", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    int id = atoi(argv[1]);
    ZLOGI(TAG, "mini_servo_set_zero %d", id);
    g_feiteservomotor.reCalibration(id, 0);
    ZLOGI(TAG, "mini_servo_set_zero %d done", id);
  });
  cmdScheduler.registerCmd("mini_servo_set_4095", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    int id = atoi(argv[1]);
    ZLOGI(TAG, "mini_servo_set_4095 %d", id);
    g_feiteservomotor.reCalibration(id, 4095);
    ZLOGI(TAG, "mini_servo_set_4095 %d done", id);
  });
  cmdScheduler.registerCmd("mini_servo_move_to", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(3);
    int id     = atoi(argv[1]);
    int pos    = atoi(argv[2]);
    int torque = atoi(argv[3]);

    ZLOGI(TAG, "mini_servo_move_to %d %d %d %d", id, pos, 2700, torque);
    g_feiteservomotor.moveTo(id, pos, 2700, torque);
  });
  cmdScheduler.registerCmd("mini_servo_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    int id    = atoi(argv[1]);
    int speed = atoi(argv[2]);
    // int v   = atoi(3000);
    ZLOGI(TAG, "mini_servo_rotate %d %d %d", id, speed, 10, 2700);
    g_feiteservomotor.rotate(id, speed, 10);
  });
  cmdScheduler.registerCmd("mini_servo_move_with_torque", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    int id     = atoi(argv[1]);
    int torque = atoi(argv[2]);
    // int v   = atoi(3000);
    if (torque == 0) {
      g_feiteservomotor.moveWithTorque(id, 1);
    } else {
      g_feiteservomotor.moveWithTorque(id, torque);
    }
  });

  cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    osDelay(atoi(argv[1]));
  });

  cmdScheduler.registerCmd("hbot_enable", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(0);
    g_xyrobotctrlmodule->enable(false);
  });
}

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

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

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  auto* cfg = m_zcanCommnaderMaster.createCFG();
  m_zcanCommnaderMaster.init(cfg);
  g_xyrobotctrlmodule = zcancmder_master::create_xyrobot_ctrl_module(&m_zcanCommnaderMaster, 1);
  g_xyrobotctrlmodule->enable(false);

  cmdScheduler.initialize(&DEBUG_UART, 1000);
  xyRobotScriptCmderModule.initialize(&cmdScheduler);
  xyRobotScriptCmderModule.regXYRobot(1, g_xyrobotctrlmodule);

  regfn();

#if 0
  int i = 1;
  g_step_motor[i++].initialize(11, 10000, &m_IflytopCanMaster);
  g_step_motor[i++].initialize(12, 10000, &m_IflytopCanMaster);
  g_step_motor[i++].initialize(13, 10000, &m_IflytopCanMaster);
  g_step_motor[i++].initialize(14, 10000, &m_IflytopCanMaster);
  g_step_motor[i++].initialize(15, 10000, &m_IflytopCanMaster);
  g_step_motor[i++].initialize(16, 10000, &m_IflytopCanMaster);

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

  StepMotor45Scheduler step_motor45_scheduler;
  step_motor45_scheduler.initialize(&htim10, 1000);
  step_motor45_scheduler.addMotor(&g_step_motor45[1]);
  step_motor45_scheduler.addMotor(&g_step_motor45[2]);
  step_motor45_scheduler.addMotor(&g_step_motor45[3]);
  step_motor45_scheduler.addMotor(&g_step_motor45[4]);
  step_motor45_scheduler.addMotor(&g_step_motor45[5]);
  step_motor45_scheduler.addMotor(&g_step_motor45[6]);
  //   g_step_motor45_1.rotate(true, 1000);

  step_motor45_scheduler.start();


  g_modbusblockhost.initialize(&huart2);
  g_eq20servomotor.init(&g_modbusblockhost);
  g_feiteservomotor.initialize(&huart3, &hdma_usart3_rx, &hdma_usart3_tx);
#endif

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