#include "by_messy.h"

#include "lwprintf.h"
#include "by_frame.h"
#include "by_motion.h"

void by_messy_init(void)
{
  by_frame_init();
}

void by_messy_loop(void)
{
  uint8_t cmd                      = 0;
  uint32_t buff[BY_FRAME_DATA_NUM] = {0};

  if (!by_frame_parse(&cmd, buff)) {

    lwprintf("get cmd: %X\r\n", cmd);

    switch (cmd) {
      case 0x31: // 设置速度 x
        memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
        by_motion_set_mode(0);
        break;

      case 0x32: // 设置速度 y
        memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
        by_motion_set_mode(0);
        break;

      case 0x33: // 设置速度 omega
        memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
        by_motion_set_mode(0);
        break;

      case 0x34:                  // 设置移动距离 x
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        memcpy(&motion_speed_struct.v_x, buff, sizeof(motion_speed_struct.v_x));
        by_motion_set_mode(1);
        motion_time_struct.t_x += buff[1];
        break;

      case 0x35:                  // 设置移动距离 y
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        memcpy(&motion_speed_struct.v_y, buff, sizeof(motion_speed_struct.v_y));
        by_motion_set_mode(1);
        motion_time_struct.t_y += buff[1];
        break;

      case 0x36:                  // 设置旋转角度 omega
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        memcpy(&motion_speed_struct.v_w, buff, sizeof(motion_speed_struct.v_w));
        by_motion_set_mode(1);
        motion_time_struct.t_w += buff[1];
        break;

      case 0x41:                  // 设置转台 x 轴复位
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        break;

      case 0x42:                  // 设置转台 z 轴复位
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        break;

      case 0x43:                  // 设置转台末端执行器复位
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        break;

      default:
        break;
    }
  }
}