#include "waveshare_can.hpp"

#include "libzqt\zexception.hpp"
#include "logger.hpp"

using namespace iflytop;
using namespace std;
#define CANUSB_TTY_BAUD_RATE_DEFAULT 2000000

#define TAG "WaveshareCan"

static int generate_checksum(const unsigned char* data, int data_len) {
  int i, checksum;

  checksum = 0;
  for (i = 0; i < data_len; i++) {
    checksum += data[i];
  }

  return checksum & 0xff;
}

static bool isHeader(uint8_t* data) {
  if (data[0] == 0xAA && data[1] == 0x55) {
    return true;
  }
  return false;
}

bool WaveshareCan::init(IDataChannel* ch, frame_callback_t framecb) {
  m_frame_callback = framecb;
  m_ch             = ch;

  m_ch->regRxListener([this](uint8_t* data, size_t len) {
    {
      lock_guard<mutex> lock(lock_);
      if (len + m_rxlen > sizeof(m_rxcache)) {
        m_rxlen = 0;
      }
      memcpy(m_rxcache + m_rxlen, data, len);
      m_rxlen += len;
    }
  });
  m_thread.reset(new thread([this]() {
    while (true) {
      this_thread::sleep_for(chrono::milliseconds(1));

      {
        lock_guard<mutex> lock(lock_);
        // 1.找头部
        int32_t headerpos = -1;
        for (int32_t i = 0; i < m_rxlen - 1; i++) {
          if (isHeader(m_rxcache + i)) {
            headerpos = i;
            break;
          }
        }

        if (headerpos == -1) {
          continue;
        }

        // 将包头移动到缓存头部
        if (headerpos != 0) {
          memmove(m_rxcache, m_rxcache + headerpos, m_rxlen - headerpos);
          m_rxlen = m_rxlen - headerpos;
          continue;
        }

        if (m_rxlen < 20) continue;

        onReceivePacket(m_rxcache, 20);
        memmove(m_rxcache, m_rxcache + 20, m_rxlen - 20);
        m_rxlen = m_rxlen - 20;
      }
    }
  }));
}
void WaveshareCan::setCanpPrameter(CANUSB_SPEED speed, CANUSB_MODE mode, CANUSB_FRAME frame) {
  int           cmd_frame_len;
  unsigned char cmd_frame[20];

  cmd_frame_len = 0;
  cmd_frame[0]  = 0xaa;
  cmd_frame[1]  = 0x55;
  cmd_frame[2]  = 0x02;   // 0x02:用固定20字节协议收发数据 0x12-设置用可变协议收发数据
  cmd_frame[3]  = speed;  // speed
  cmd_frame[4]  = frame;  // 帧类型
  cmd_frame[5]  = 0;      /* Filter ID not handled. */
  cmd_frame[6]  = 0;      /* Filter ID not handled. */
  cmd_frame[7]  = 0;      /* Filter ID not handled. */
  cmd_frame[8]  = 0;      /* Filter ID not handled. */
  cmd_frame[9]  = 0;      /* Mask ID not handled. */
  cmd_frame[10] = 0;      /* Mask ID not handled. */
  cmd_frame[11] = 0;      /* Mask ID not handled. */
  cmd_frame[12] = 0;      /* Mask ID not handled. */
  cmd_frame[13] = mode;   // 模式
  cmd_frame[14] = 0x01;   // 是否自动重发
  cmd_frame[15] = 0;      // 占位，备用
  cmd_frame[16] = 0;      // 占位，备用
  cmd_frame[17] = 0;      // 占位，备用
  cmd_frame[18] = 0;      // 占位，备用
  cmd_frame[19] = generate_checksum(&cmd_frame[2], 17);

  if (!m_ch || !m_ch->isOpen()) {
    throw zexception(ke_channel_is_close, "channel is not open");
  }
  m_ch->send(cmd_frame, 20);
}

bool WaveshareCan::sendframe(CANUSB_FRAME frametype, uint32_t id, unsigned char data[], int data_length_code) {
  int                     data_frame_len = 0;
  unsigned char           data_frame[20] = {0x00};
  waveshare_can_packet_t* packet         = (waveshare_can_packet_t*)data_frame;

  packet->packet_header0    = 0xaa;
  packet->packet_header1    = 0x55;
  packet->type              = 0x01;  // 数据
  packet->frame_type        = frametype;
  packet->frame_format      = 0x01;  // 数据帧
  packet->frame_id_data_1   = id & 0xff;
  packet->frame_id_data_2   = (id >> 8) & 0xff;
  packet->frame_id_data_3   = (id >> 16) & 0xff;
  packet->frame_id_data_4   = (id >> 24) & 0xff;
  packet->frame_data_length = data_length_code;
  packet->frame_data_1      = data[0];
  packet->frame_data_2      = data[1];
  packet->frame_data_3      = data[2];
  packet->frame_data_4      = data[3];
  packet->frame_data_5      = data[4];
  packet->frame_data_6      = data[5];
  packet->frame_data_7      = data[6];
  packet->frame_data_8      = data[7];
  packet->reserve           = 0;
  packet->check_code        = generate_checksum(&data_frame[2], 17);

  if (!m_ch || !m_ch->isOpen()) {
    throw zexception(ke_channel_is_close, "channel is not open");
  }

  ZLOGI(TAG, "tx 0x%08x, %s", id, zhex2str(data, data_length_code).c_str());
  m_ch->send(data_frame, 20);

  return true;
}

int WaveshareCan::onReceivePacket(uint8_t* data, size_t len) {
  waveshare_can_packet_t* rxpacket = (waveshare_can_packet_t*)data;

  can_rx_frame_t canframe;
  canframe.dlc = rxpacket->frame_data_length;
  canframe.id  = rxpacket->frame_id_data_1 | ((uint32_t)rxpacket->frame_id_data_2 << 8) | ((uint32_t)rxpacket->frame_id_data_3 << 16) | ((uint32_t)rxpacket->frame_id_data_4 << 24);

  canframe.data[0] = rxpacket->frame_data_1;
  canframe.data[1] = rxpacket->frame_data_2;
  canframe.data[2] = rxpacket->frame_data_3;
  canframe.data[3] = rxpacket->frame_data_4;
  canframe.data[4] = rxpacket->frame_data_5;
  canframe.data[5] = rxpacket->frame_data_6;
  canframe.data[6] = rxpacket->frame_data_7;
  canframe.data[7] = rxpacket->frame_data_8;

  if (m_frame_callback) {
    m_frame_callback(&canframe);
  }
}