#include "by_messy.h"

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

void by_messy_init(void)
{
  by_frame_init();
}

void by_messy_loop(void)
{
  uint8_t cmd                      = 0;
  uint8_t buff_t_u8[8]             = {0};
  uint16_t buff_t_u16[4]           = {0};
  float buff_t_f[2]                = {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); // 正确接收后直接返回原文
        buff_t_u8[0] = 0;
        buff_t_u8[1] = 1; // 复位模式下速度设置无效
        buff_t_f[0]  = 0.0;
        memcpy(&buff_t_u8[2], &buff_t_f[0], sizeof(buff_t_f[0]));
        by_can_send_stdd(0x008, buff_t_u8, 1, 100);
        break;

      case 0x42:                  // 设置转台 z 轴复位
        by_frame_send(cmd, buff); // 正确接收后直接返回原文
        buff_t_u8[0] = 0;
        buff_t_u8[1] = 5; // 设置复位速度 5
        by_can_send_stdd(0x008, buff_t_u8, 1, 100);
        break;

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

      case 0x44: // 设置 x 轴位置 (增量)
        by_frame_send(cmd, buff);
        buff_t_u8[0] = 1;                               // 增量模式
        buff_t_u8[1] = (uint8_t)(buff[0] & 0xFF);       // 拷贝设置速度
        memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
        by_can_send_stdd(0x006, buff_t_u8, 8, 100);
        break;

      case 0x46: // 设置 z 轴位置 (增量)
        by_frame_send(cmd, buff);
        buff_t_u8[0] = 1;                               // 增量模式
        buff_t_u8[1] = (uint8_t)(buff[0] & 0xFF);       // 拷贝设置速度
        memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
        by_can_send_stdd(0x008, buff_t_u8, 8, 100);
        break;

      case 0x47: // 设置 x 轴位置 (绝对)
        by_frame_send(cmd, buff);
        buff_t_u8[0] = 0;                               // 位置模式
        buff_t_u8[1] = (uint8_t)(buff[0] & 0xFF);       // 拷贝设置速度
        memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
        by_can_send_stdd(0x006, buff_t_u8, 8, 100);
        break;

      case 0x49: // 设置 z 轴位置 (绝对)
        by_frame_send(cmd, buff);
        buff_t_u8[0] = 0;                               // 增量模式
        buff_t_u8[1] = (uint8_t)(buff[0] & 0xFF);       // 拷贝设置速度
        memcpy(&buff_t_u8[2], &buff[1], sizeof(float)); // 拷贝设置距离
        by_can_send_stdd(0x008, buff_t_u8, 8, 100);
        break;

      case 0x50: // 设置夹爪摇臂角度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x009, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x51: // 设置夹爪角度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x00A, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x52: // 设置摄像头角度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x00B, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x53: // 设置顶端抓取机构角度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x00C, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x54: // 设置托盘角度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x00D, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x55: // 设置转轴速度
        by_frame_send(cmd, buff);
        memcpy(buff_t_f, buff, sizeof(float));
        buff_t_u16[0] = (int16_t)(buff_t_f[0]);
        by_can_send_stdd(0x00E, (uint8_t *)&buff_t_u16, 2, 100);
        break;

      case 0x61: // 设置闪灯
        by_frame_send(cmd, buff);
        memcpy(buff_t_u8, buff, sizeof(buff[1]));
        gpio_bits_write(GPIOB, GPIO_PINS_6, buff_t_u8[0] ? TRUE : FALSE); // 灯条
        break;

      case 0x62: // 设置蜂鸣器
        by_frame_send(cmd, buff);
        memcpy(buff_t_u8, buff, sizeof(buff[1]));
        gpio_bits_write(GPIOB, GPIO_PINS_7, buff_t_u8[0] ? TRUE : FALSE); // 灯条
        break;

      default:
        break;
    }
  }
}