#include "main.hpp"

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

//
#include "sdk/os/zos.hpp"
#include "sdk\components\cmdscheduler\cmd_scheduler.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"

#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,
};

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PC7, PC8, PC9, PA8},
    .driverPinMirror = false,
};

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

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

    .driverPin       = {PA13, PA14, PA15, PC10},
    .driverPinMirror = false,
};
static StepMotor45::cfg_t cfg6 = {
    .max_pos              = -1,
    .enable_zero_limit    = false,
    .enable_max_pos_limit = false,
    .mirror               = false,

    .zeroPin       = PinNull,
    .zeroPinMirror = false,

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

map<int, float> screw_lead = {
    {1, 10.0},  //
    {2, 10.0},  //
    {3, 10.0},  //
    {4, 10.0},  //
    {5, 10.0},  //
    {6, 10.0},  //
};

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

static StepMotor45  g_step_motor45[7];
StepMotor           g_step_motor[10];
IflytopCanMaster    m_IflytopCanMaster;
static CmdScheduler cmdScheduler;

bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) {
  if (screw_lead.find(motorid) == screw_lead.end()) {
    return false;
  }
  float lead = screw_lead[motorid];
  *distance  = distance_mm / lead * 51200;
  return true;
}

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

  // stepmotor45
  cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid direction
    CHECK_ARGC(2);
    int  motorid   = atoi(argv[1]);
    bool direction = atoi(argv[2]);

    if (motorid < 1 || motorid > 6) {
      ZLOGE(TAG, "motorid out of range");
      return;
    }

    g_step_motor45[motorid].rotate(direction);
  });
  cmdScheduler.registerCmd("stepmotor45_readPos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid direction
    CHECK_ARGC(2);
    int motorid = atoi(argv[1]);

    if (motorid < 1 || motorid > 6) {
      ZLOGE(TAG, "motorid out of range");
      return;
    }

    ZLOGI(TAG, "motorid %d pos %d", motorid, g_step_motor45[motorid].getPos());
  });

  cmdScheduler.registerCmd("stepmotor45_moveTo", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid pos
    CHECK_ARGC(2);
    int motorid = atoi(argv[1]);
    int pos     = atoi(argv[2]);

    if (motorid < 1 || motorid > 6) {
      ZLOGE(TAG, "motorid out of range");
      return;
    }

    g_step_motor45[motorid].moveTo(pos);
  });

  cmdScheduler.registerCmd("stepmotor45_moveBy", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid pos
    CHECK_ARGC(2);
    int motorid = atoi(argv[1]);
    int pos     = atoi(argv[2]);

    if (motorid < 1 || motorid > 6) {
      ZLOGE(TAG, "motorid out of range");
      return;
    }

    g_step_motor45[motorid].moveBy(pos);
  });

  cmdScheduler.registerCmd("stepmotor45_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid pos
    CHECK_ARGC(1);
    int motorid = atoi(argv[1]);

    if (motorid < 1 || motorid > 6) {
      ZLOGE(TAG, "motorid out of range");
      return;
    }

    g_step_motor45[motorid].stop();
  });

/*******************************************************************************
 *                                  步进电机                                   *
 *******************************************************************************/
#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);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->setVelocity(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_set_acc", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->setAcc(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_set_dec", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->setDec(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_moveto", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->moveTo(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_moveby", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->moveBy(atoi(argv[2]));
  });
  cmdScheduler.registerCmd("step_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(2);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    motor->rotate(atoi(argv[2]));
  });
  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);
  });

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

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

  chip_init(&chipcfg);

  zos_cfg_t zoscfg;
  zos_init(&zoscfg);

  auto config = m_IflytopCanMaster.createDefaultConfig(1);
  m_IflytopCanMaster.initialize(config);

  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();

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