#include "by_cmd.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <pthread.h>
#include "by_frame.h"
#include "logc/log.h"

uint8_t cmd;
uint32_t received_data[2];

uint8_t listerning_cmd;
int received_flag;

int by_cmd_init(void)
{
  log_debug("by_cmd init");
  return (by_frame_init());
}

void *by_cmd_loop(void *timeout)
{
  while (*(int *)timeout > 0) {
    int ret = by_frame_parse(&cmd, received_data);

    if (!ret && cmd == listerning_cmd) {
      //   log_debug("got frame cmd %2X", cmd);
      break;
    }
    usleep(10);
  }

  received_flag  = 1;
  listerning_cmd = 0;

  return NULL;
}

/**
 * @brief 注册监听的
 *s
 * @param cmd
 */
int by_cmd_reg_listerning(uint8_t cmd, int timeout)
{
  listerning_cmd = cmd;
  received_flag  = 0;

  pthread_t thread_listerning;
  pthread_create(&thread_listerning, NULL, by_cmd_loop, &timeout);

  while (timeout--) {
    if (received_flag) {
      // log_debug("received done! using %fSEC", (float)timeout_cnt * 0.00005);
      listerning_cmd = 0;
      received_flag  = 0;
      return 0;
    }
    usleep(10);
  }

  listerning_cmd = 0;
  received_flag  = 0;

  log_warn("get cmd time out");

  return -1;
}

/**
 * @brief 发送 x 方向速度指令（无应答）
 *
 * @param speed
 */
void by_cmd_send_speed_x(float speed)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &speed, 4);

  by_frame_send(0x31, buff, 4);
}

/**
 * @brief 发送 y 方向速度指令（无应答）
 *
 * @param speed
 */
void by_cmd_send_speed_y(float speed)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &speed, 4);

  by_frame_send(0x32, buff, 4);
}

/**
 * @brief 发送角速度指令（无应答）
 *
 * @param speed
 */
void by_cmd_send_speed_omega(float speed)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &speed, 4);

  by_frame_send(0x33, buff, 4);
}

/**
 * @brief 发送 x 方向移动距离指令（等待应答）
 *
 * @param speed
 * @param time
 * @return 0 发送成功
 * @return -1 应答超时
 */
int by_cmd_send_distance_x(float speed, uint32_t time)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 4);
  memcpy(buff + 4, &time, 4);
  by_frame_send(0x34, buff, 8);
  return (by_cmd_reg_listerning(0x34, 1000));
}

/**
 * @brief 发送 y 方向移动距离指令（等待应答）
 *
 * @param speed
 * @param time
 * @return true 发送成功
 * @return false 应答超时
 */
int by_cmd_send_distance_y(float speed, uint32_t time)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 4);
  memcpy(buff + 4, &time, 4);
  by_frame_send(0x35, buff, 8);
  return (by_cmd_reg_listerning(0x35, 1000));
}

/**
 * @brief 发送 omega 方向旋转角度指令（等待应答）
 *
 * @param speed
 * @param time
 * @return true 发送成功
 * @return false 应答超时
 */
int by_cmd_send_angle_omega(float speed, uint32_t time)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &speed, 4);
  memcpy(buff + 4, &time, 4);
  by_frame_send(0x36, buff, 8);
  return (by_cmd_reg_listerning(0x36, 1000));
}

int by_cmd_send_reset_axis_x(void)
{
  uint8_t buff[4] = {0};
  by_frame_send(0x41, buff, 4);
  return (by_cmd_reg_listerning(0x41, 1000));
}

int by_cmd_send_reset_axis_z(void)
{
  uint8_t buff[4] = {0};
  by_frame_send(0x42, buff, 4);
  return (by_cmd_reg_listerning(0x42, 1000));
}

int by_cmd_send_reset_end_effector(void)
{
  uint8_t buff[4] = {0};
  by_frame_send(0x43, buff, 4);
  return (by_cmd_reg_listerning(0x43, 1000));
}

int by_cmd_send_distance_axis_x(uint8_t speed, float distance)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 1);
  memcpy(buff + 4, &distance, 4);
  by_frame_send(0x44, buff, 8);
  return (by_cmd_reg_listerning(0x44, 1000));
}

int by_cmd_send_distance_axis_z(uint8_t speed, float distance)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 1);
  memcpy(buff + 4, &distance, 4);
  by_frame_send(0x46, buff, 8);
  return (by_cmd_reg_listerning(0x46, 1000));
}

int by_cmd_send_position_axis_x(uint8_t speed, float position)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 1);
  memcpy(buff + 4, &position, 4);
  by_frame_send(0x47, buff, 8);
  return (by_cmd_reg_listerning(0x47, 1000));
}

int by_cmd_send_position_axis_z(uint8_t speed, float position)
{
  uint8_t buff[8] = {0};
  memcpy(buff, &speed, 1);
  memcpy(buff + 4, &position, 4);
  by_frame_send(0x49, buff, 8);
  return (by_cmd_reg_listerning(0x49, 1000));
}

int by_cmd_send_angle_claw_arm(float angle)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &angle, 4);
  by_frame_send(0x50, buff, 4);
  return (by_cmd_reg_listerning(0x50, 1000));
}

int by_cmd_send_angle_claw(float angle)
{
  uint8_t buff[4] = {0};
  memcpy(buff, &angle, 4);
  by_frame_send(0x51, buff, 4);
  return (by_cmd_reg_listerning(0x51, 1000));
}

int by_cmd_send_ranging_start(void)
{
  uint8_t buff[4] = {0};
  by_frame_send(0x55, buff, 4);
  return (by_cmd_reg_listerning(0x55, 1000));
}

int by_cmd_recv_ranging_data(float *distance)
{
  uint8_t buff[4] = {0};
  by_frame_send(0x56, buff, 4);
  if (!by_cmd_reg_listerning(0x56, 1000)) {
    memcpy(distance, &received_data[0], 4);
    return 0;
  } else {
    return -1;
  }
}