
///加入和上层控制器协议解析，可收命令和发送命令返回 ---以前用于和飞控通讯目前已废弃改用mavlink
///使用commser.cpp里面封装的mavlink协议通讯,
///这个库用Serial2，目前Serial2用来播放语音
/// @file Serialcommand.cpp
#include "Serialcommand.h"
#include "Arduino.h"
#include "CRC.h"
#include "CRC.h"
#define HEAD_HI 0xA5
#define HEAD_LI 0x5A
#define DEBUG_OUT false
Serialcommand::Serialcommand()
{
  // 连接控制器
  Serial2.begin(115200);
  // clear serialport
  while (Serial2.read() >= 0)
  {
  }
  CmdState = 0;
  bufferIndex = 0;
  recv_flag = false;
  outBuffer[0] = HEAD_HI;
  outBuffer[1] = HEAD_LI;
}
Serialcommand::~Serialcommand()
{
}

void Serialcommand::Uart_Command_Rev(void)
{
  while (Serial2.available() > 0)
  {
    ch = Serial2.read();
    if (DEBUG_OUT)
      printf("rev:%d\n", ch);
    switch (CmdState)
    {
      // 开始标志1
    case 0:
    {
      if (HEAD_HI == ch)
      {
        CmdState = 1;
        if (DEBUG_OUT)
          printf("st:%d\n", CmdState);
      }
    }
    break;
    // 开始标志2
    case 1:
    {
      if (HEAD_LI == ch)
      {
        bufferIndex = 0;
        CmdState = 2;
        if (DEBUG_OUT)
          printf("st:%d\n", CmdState);
      }
      else
        CmdState = 0;
    }
    break;
    // 取长度，准备收数据
    case 2:
    {
      datalen = ch;
      if (DEBUG_OUT)
        printf("datalen:%d\n", datalen);
      if (datalen <= inbuffersize)
      {
        CmdState = 3;
        bufferIndex = 0;
        inBuffer[bufferIndex] = ch;
        if (DEBUG_OUT)
          printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
      }
      else // 超长
      {
        CmdState = 0;
      }
    }
    break;
    // 收数据直到结束
    case 3:
    {
      if (DEBUG_OUT)
        printf("st3:%d:%d\n", bufferIndex, datalen);

      if (bufferIndex <= datalen)
      {
        bufferIndex++;
        inBuffer[bufferIndex] = ch;
        if (DEBUG_OUT)
          printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
        if (bufferIndex == datalen) // 收完了
        {
          CmdState = 0;
          recv_flag = true;
          bufferIndex = 0;
          return; // 不收数据了，先出去解析
        }
      }
      else // 异常
      {
        if (DEBUG_OUT)
          printf("buf error:%d:%d\n", bufferIndex, datalen);
        CmdState = 0;
        bufferIndex = 0;
        recv_flag = false;
        return;
      }
    }
    break;
    }
  }
}
// CRC8
// 多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107）
// https://zhuanlan.zhihu.com/p/114049042
uint8_t Serialcommand::getsum(const uint8_t *buffer, size_t size)
{
  return crc8(buffer, size, 0x07, 0x00, 0x00, false, false);
}
bool Serialcommand::checksum()
{
  uint8_t bufsum = inBuffer[datalen];             // 校验和
  uint8_t chksum = getsum(inBuffer, datalen - 1); // 不算sum字节
  if (chksum != bufsum)
  {
    printf("sum fault :data_sum:0x%02X,add_sum:0x%02X\n", bufsum, chksum);
  }
  return bufsum == chksum;
}
void Serialcommand::sendinfo(HookStatus status, int linelength, int pullweight)
{

  // 长度
  outBuffer[2] = 0x07;
  outBuffer[3] = 0x02;
  outBuffer[4] = status;

  outBuffer[5] = (uint8_t)((linelength >> 8) & 0xff);
  outBuffer[6] = (uint8_t)(linelength & 0xff);

  outBuffer[7] = (uint8_t)((pullweight >> 8) & 0xff);
  outBuffer[8] = (uint8_t)(pullweight & 0xff);

  outBuffer[9] = getsum(outBuffer + 2, 7);
  Serial2.write(outBuffer, 10);
}

void Serialcommand::sendcmdret(uint8_t cmdret)
{
  // 长度
  outBuffer[2] = 0x03;
  // 标识命令返回
  outBuffer[3] = 0x01;
  // 返回数据
  outBuffer[4] = cmdret;
  outBuffer[5] = getsum(outBuffer + 2, 3);
  Serial2.write(outBuffer, 6);
}
void Serialcommand::getcommand()
{
  Uart_Command_Rev();
  // 收到有效数据
  if (recv_flag)
  {
    if (DEBUG_OUT)
      printf("rev end len:%d\n", datalen);
    recv_flag = false;
    // 处理数据
    if (checksum())
    {
      uint8_t commandid = inBuffer[1];
      printf("rev command id:0x%02X\n", commandid);
      switch (commandid)
      {
      // 放钩
      case 0x01:
      {
        sendcmdret(0); // 成功
      }
      break;
      // 收钩
      case 0x02:
      {
        sendcmdret(0); // 成功
      }
      break;
      // 停止
      case 0x03:
      {
        sendcmdret(0); // 成功
      }
      break;
      // 查询当前状态
      case 0x04:
      {
        sendcmdret(0); //  发送状态
      }
      break;
      // 设置速度
      case 0x05:
      {
        sendcmdret(0); // 成功
      }
      break;
      default:
        sendcmdret(100); // 失败，未知命令
      }
    }
    memset(inBuffer, 0x00, sizeof(inBuffer));
  }
}
