#include "by_motion.h"

#include <math.h>
#include <string.h>
#include "by_debug.h"
#include "by_can.h"

#define D_X   (0.25f) // 底盘 Y 轴上两轮中心的间距
#define D_Y   (0.28f) // 底盘 X 轴上两轮中心的间距
#define RX_RY ((D_X + D_Y) / 2.f)

/**********************************************
 * v_1 = v_{ty} + v_{tx} - (r_x + r_y) * \omega
 * v_2 = v_{ty} - v_{tx} + (r_x + r_y) * \omega
 * v_3 = v_{ty} + v_{tx} + (r_x + r_y) * \omega
 * v_4 = v_{ty} - v_{tx} - (r_x + r_y) * \omega
 **********************************************/

/*** 控制模式 ***/
uint8_t control_mode = 0; // 0-速度模式 1-位置模式

/*** 位置控制 ***/
uint8_t control_timer      = 0; // 位置控制定时器状态
uint32_t control_timer_cnt = 0; // 位置控制计数

/*** 各轮转速，左上角为 1 号，顺时针标号 ***/
float v_wheel[4] = {0.f};

/** 目标速度 **/
motion_speed_type motion_speed_struct;
motion_speed_type motion_speed_struct_last;
motion_time_type motion_time_struct;

/** 下发数据包 **/
int16_t motion_speed_data[4] = {0};

void by_motion_init(void)
{
  memset(&motion_speed_struct, 0, sizeof(motion_speed_struct));
  memset(&motion_speed_struct_last, 0, sizeof(motion_speed_struct_last));
  memset(&motion_time_struct, 0, sizeof(motion_time_struct));
  memset(motion_speed_data, 0, sizeof(motion_speed_data));
  memset(v_wheel, 0, sizeof(v_wheel));

  by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
}

void by_motion_update_speed(void)
{
  const motion_speed_type *speed = &motion_speed_struct;

  v_wheel[0] = speed->v_x + speed->v_y - RX_RY * speed->v_w;
  v_wheel[1] = speed->v_x - speed->v_y + RX_RY * speed->v_w;
  v_wheel[2] = speed->v_x + speed->v_y + RX_RY * speed->v_w;
  v_wheel[3] = speed->v_x - speed->v_y - RX_RY * speed->v_w;

  // 根据安装方式调整轮子方向
  v_wheel[1] *= -1.0f;
  v_wheel[2] *= -1.0f;

  for (uint8_t i = 0; i < 4; i++) {
    motion_speed_data[i] = (int16_t)v_wheel[i];

    LOGD("MOTION#SPD wheel[%d] - %d", i, motion_speed_data[i]);
  }

  by_can_send_stdd(0x01, (uint8_t *)&motion_speed_data, 8, 100);
  LOGD("MOTION#SPD updated");
}

/**
 * @brief 电机控制 - 主循环中调用
 *
 */
void by_motion_loop(void)
{
  if (control_mode == 0) { // 速度控制模式
    memset(&motion_time_struct, 0, sizeof(motion_time_struct));
  } else { // 位置控制模式
    if (0 == motion_time_struct.t_x) {
      motion_speed_struct.v_x = 0;
    }
    if (0 == motion_time_struct.t_y) {
      motion_speed_struct.v_y = 0;
    }
    if (0 == motion_time_struct.t_w) {
      motion_speed_struct.v_w = 0;
    }
  }

  if ((motion_speed_struct.v_x != motion_speed_struct_last.v_x) || (motion_speed_struct.v_y != motion_speed_struct_last.v_y) || (motion_speed_struct.v_w != motion_speed_struct_last.v_w)) {
    by_motion_update_speed();
    memcpy(&motion_speed_struct_last, &motion_speed_struct, sizeof(motion_speed_type));
  }
}

/**
 * @brief 定时回调
 *
 */
void by_motion_timer_handle(void)
{
  if (control_mode == 0) {
    motion_time_struct.t_x = 0;
    motion_time_struct.t_y = 0;
    motion_time_struct.t_w = 0;
  } else {
    if (motion_time_struct.t_x > 0) {
      motion_time_struct.t_x--;
    }
    if (motion_time_struct.t_y > 0) {
      motion_time_struct.t_y--;
    }
    if (motion_time_struct.t_w > 0) {
      motion_time_struct.t_w--;
    }
  }
}

/**
 * @brief 设置电机控制模式
 *
 * @param mode 0-速度模式 1-位置模式
 */
void by_motion_set_mode(uint8_t mode)
{
  control_mode = mode;
}