#include "by_motion.h"

#include <math.h>
#include <string.h>

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

#define FP2U16_SCALE (1000.f)                // 浮点转存为整数缩放尺度，所有设备该参数需对应
#define FP2U16(x)    ((int16_t)(x * FP2U16_SCALE)) // 浮点转存为整数

/**********************************************
 * 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
 **********************************************/

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

/** 目标速度 **/
motion_speed_type motion_speed_struct;

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

void by_motion_init(void)
{
  memset(&motion_speed_struct, 0, sizeof(motion_speed_struct));
  memset(v_wheel, 0, sizeof(v_wheel));
}

void by_motion_calc_speed(motion_speed_type *speed)
{
  motion_speed_struct = *speed;

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

/**
 * @brief
 *
 */
void by_motion_pack_speed(void)
{
  for (uint8_t i = 0; i < 4; i++) {
    motion_speed_data[i] = FP2U16(v_wheel[i]);
  }
}