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

#include "main.hpp"

//
#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"

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

};

using namespace iflytop;
#define CHECK_ARGC(n)            \
  if (argc != (n + 1)) {         \
    ZLOGE(TAG, "argc != %d", n); \
    context->breakflag = true;   \
    return;                      \
  }

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

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

namespace iflytop {
extern StepMotor45      g_step_motor45[7];
extern StepMotor        g_step_motor[10];
extern IflytopCanMaster m_IflytopCanMaster;
extern CmdScheduler     cmdScheduler;

extern ModbusBlockHost g_modbusblockhost;
extern Eq20ServoMotor  g_eq20servomotor;
extern FeiTeServoMotor g_feiteservomotor;
}  // namespace iflytop

static 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 step_motor_cmd_reg() {
#if 0
  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("stepmotor45_wait_to_reach_pos", [](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;
    }
    while (true) {
      if (g_step_motor45[motorid].is_reach_target_pos()) {
        break;
      }
      ZLOGI(TAG, "stepmotor45_wait_to_reach_pos %d", motorid);
      osDelay(100);
    }
  });
  cmdScheduler.registerCmd("stepmotor45_reset_pos_all", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid direction
    CHECK_ARGC(0);
    g_step_motor45[1].setPos(0);
    g_step_motor45[2].setPos(0);
    g_step_motor45[3].setPos(0);
    g_step_motor45[4].setPos(0);
    g_step_motor45[5].setPos(0);
    g_step_motor45[6].setPos(0);
  });

  // stepmotor45
  cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // stepmotor45_rotate motorid direction
    CHECK_ARGC(2);
    int motorid   = atoi(argv[1]);
    int 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) {
    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) {
    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) {
    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();
  });

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

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

    g_step_motor45[motorid].setDefaultSpeed(speed);
  });

/*******************************************************************************
 *                                  步进电机                                   *
 *******************************************************************************/
#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_readpos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);

    StepMotor* motor = NULL;
    GET_MOTOR(motor);

    int32_t pos = motor->readpos();
    ZLOGI(TAG, "step_motor_readpos %d %d", atoi(argv[1]), pos);
  });
  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);
  });

  static int main_servo_motor_velocity = 400;
  cmdScheduler.registerCmd("main_servo_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // main_servo_motor_rotate direction
    CHECK_ARGC(1);
    int direction = atoi(argv[1]);
    if (direction == 0) {
      // 用正转位置0来停止设备
      g_eq20servomotor.writePn(1, 614, 1);
      g_eq20servomotor.writePn(1, 1501, 0);
      g_eq20servomotor.writePn(1, 1501, 1);
      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);

    } else if (direction > 0) {
      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
      g_eq20servomotor.writePn(1, 614, -1);
      g_eq20servomotor.writePn(1, 1501, 0);
      g_eq20servomotor.writePn(1, 1501, 1);
      g_eq20servomotor.writePn(1, 614, 999999);
      g_eq20servomotor.writePn(1, 1501, 0);
      g_eq20servomotor.writePn(1, 1501, 1);
    } else if (direction < 0) {
      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
      g_eq20servomotor.writePn(1, 614, 1);
      g_eq20servomotor.writePn(1, 1501, 0);
      g_eq20servomotor.writePn(1, 1501, 1);
      g_eq20servomotor.writePn(1, 614, -999999);
      g_eq20servomotor.writePn(1, 1501, 0);
      g_eq20servomotor.writePn(1, 1501, 1);
    }
  });
  cmdScheduler.registerCmd("main_servo_motor_set_velocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    CHECK_ARGC(1);
    main_servo_motor_velocity = atoi(argv[1]);
    return;
  });
  cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // main_servo_motor_move_to v
    CHECK_ARGC(0);
    g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
    g_eq20servomotor.writePn(1, 614, 0);
    g_eq20servomotor.writePn(1, 1501, 0);
    g_eq20servomotor.writePn(1, 1501, 1);
  });
  cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
    // main_servo_motor_move_to v
    CHECK_ARGC(0);
    g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
    g_eq20servomotor.writePn(1, 614, 0);
    g_eq20servomotor.writePn(1, 1501, 0);
    g_eq20servomotor.writePn(1, 1501, 1);
  });

  // setzero
  // set4095
  // moveto

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