#include "main.hpp"

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

//
#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\tmc\ic\ztmc5130.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) { umain(); }
}
/*******************************************************************************
 *                                    配置                                     *
 *******************************************************************************/
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,
};
TMC5130        m_motor1;
static int32_t m_velocity;

/*******************************************************************************
 *                                    代码                                     *
 *******************************************************************************/

static StepMotor45  g_step_motor45[7];
static CmdScheduler cmdScheduler;

void regfn() {
  cmdScheduler.registerCmd("help",  //
                           [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); });
  cmdScheduler.registerCmd("reset_board",  //
                           [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { NVIC_SystemReset(); });
  cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    osDelay(atoi(argv[1]));
  });

/*******************************************************************************
 *                                  步进电机                                   *
 *******************************************************************************/
#define GET_MOTOR(motor)                         \
  {                                              \
    int motorid = atoi(argv[1]);                 \
    motor       = &g_step_motor[motorid];        \
    if (motorid >= 6 || motorid < 1) {           \
      ZLOGE(TAG, "motor %d not found", motorid); \
      context->breakflag = true;                 \
      return;                                    \
    }                                            \
  }

  /**
   * @brief STEPMOTOR
   */
  cmdScheduler.registerCmd("step_motor_setvelocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    int motorid = atoi(argv[1]);
    m_velocity  = atoi(argv[2]);
  });
  cmdScheduler.registerCmd("step_motor_set_acc", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    m_motor1.setAcceleration(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_set_dec", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    m_motor1.setDeceleration(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_moveto", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    ZLOGI(TAG, "step_motor_moveto %d %d", atoi(argv[2]), m_velocity);
    m_motor1.moveTo(atoi(argv[2]), m_velocity);
  });
  cmdScheduler.registerCmd("step_motor_moveby", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    ZLOGI(TAG, "step_motor_moveby %d %d", atoi(argv[2]), m_velocity);
    m_motor1.moveBy(atoi(argv[2]), m_velocity);
  });
  cmdScheduler.registerCmd("step_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);
    ZLOGI(TAG, "step_motor_rotate %d %d", atoi(argv[2]), m_velocity);
    if (atoi(argv[2]) > 0) {
      m_motor1.rotate(m_velocity);
    } else if (atoi(argv[2]) == 0) {
      m_motor1.stop();
    } else {
      m_motor1.rotate(-m_velocity);
    }
  });
#if 0

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

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->moveToZero();
  });
  cmdScheduler.registerCmd("step_motor_wait_for_idle", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);
    HAL_Delay(100);
    while (!motor->isIdleState()) {
      HAL_Delay(300);
      ZLOGI(TAG, "step_motor_wait_for_idle %d", atoi(argv[1]));
    }
  });
  cmdScheduler.registerCmd("step_motor_clear_exception", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->clearException();
  });

  cmdScheduler.registerCmd("step_motor_moveto_mm", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    int32_t step;
    if (!distance_mm_to_step(atoi(argv[1]), atof(argv[2]), &step)) {
      ZLOGE(TAG, "step_motor_moveto_mm %d %f failed", atoi(argv[1]), atof(argv[2]));
      context->breakflag = true;
      return;
    }
    motor->moveTo(step);
  });
#endif
}

#define MOTOR_SPI              hspi1
#define MOTOR0_CSN             PA4
#define MOTOR0_ENN             PB7
#define MOTOR1_SPI_MODE_SELECT PB4
// TMC5130HImpl m_tmc5130motor;

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

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  // MOTOR1_SPI_MODE_SELECT.initAsOutput(false);  // 选择SPI模式还是SD模式，默认给false即可。
  // m_tmc5130motor.initialize("motor1", this, &MOTOR_SPI, &MOTOR0_CSN, &MOTOR0_ENN);

  {
    TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR0_ENN, .csn_pin = MOTOR0_CSN};

    m_motor1.initialize(&cfg);
    int32_t chipv = m_motor1.readChipVERSION();
    ZLOGI(TAG, "m_motor1:%lx", chipv);
    m_motor1.setIHOLD_IRUN(1, 20, 0);
    m_motor1.setMotorShaft(true);

    m_motor1.setIHOLD_IRUN(MOTOR_IHOLD, MOTOR_IRUN, 0);

    m_motor1.setAcceleration(CFG_ACC);
    m_motor1.setDeceleration(CFG_DEC);
    m_velocity = CFG_VELOCITY;
  }

  int i = 1;
  cmdScheduler.initialize(&DEBUG_UART, 1000);
  regfn();
  while (true) {
    OSDefaultSchduler::getInstance()->loop();
    cmdScheduler.schedule();
    // m_IflytopCanMaster.writeReg(1,1,1,10);
    // osDelay(1);
  }
}