#include "crsf.hpp"
#include <algorithm>

#define CRSF_VERBOSE(...)
#define CRSF_DEBUG(...)

struct crsf_frame_header_t {
  uint8_t device_address;             ///< @see crsf_address_t
  uint8_t length;                     ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
};

struct crsf_frame_t {
  crsf_frame_header_t header;
  uint8_t type;                       ///< @see crsf_frame_type_t
  uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
};

enum class crsf_frame_type_t : uint8_t {
  gps = 0x02,
  battery_sensor = 0x08,
  link_statistics = 0x14,
  rc_channels_packed = 0x16,
  attitude = 0x1E,
  flight_mode = 0x21,
  
  // Extended Header Frames, range: 0x28 to 0x96
  device_ping = 0x28,
  device_info = 0x29,
  parameter_settings_entry = 0x2B,
  parameter_read = 0x2C,
  parameter_write = 0x2D,
  command = 0x32
};

enum class crsf_payload_size_t : uint8_t {
  gps = 15,
  battery_sensor = 8,
  link_statistics = 10,
  rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
  attitude = 6,
};

enum class crsf_parser_state_t : uint8_t {
  unsynced = 0,
  synced
};

#pragma pack(push, 1)
struct crsf_payload_RC_channels_packed_t {
  // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
  unsigned chan0 : 11;
  unsigned chan1 : 11;
  unsigned chan2 : 11;
  unsigned chan3 : 11;
  unsigned chan4 : 11;
  unsigned chan5 : 11;
  unsigned chan6 : 11;
  unsigned chan7 : 11;
  unsigned chan8 : 11;
  unsigned chan9 : 11;
  unsigned chan10 : 11;
  unsigned chan11 : 11;
  unsigned chan12 : 11;
  unsigned chan13 : 11;
  unsigned chan14 : 11;
  unsigned chan15 : 11;
};
#pragma pack(pop)

static crsf_frame_t	 crsf_frame;
static unsigned current_frame_position = 0;
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;

/**
* parse the current crsf_frame buffer
*/
bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
		uint16_t *num_values, uint16_t max_channels);
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);

bool crsf_parse(const uint8_t *frame, unsigned len, uint16_t *values,
		uint16_t *num_values, uint16_t max_channels)
{
  bool ret = false;
  uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
  
  while (len > 0) {
    
    // fill in the crsf_buffer, as much as we can
    const unsigned current_len = std::min(len, sizeof(crsf_frame_t) - current_frame_position);
    memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
    current_frame_position += current_len;
    
    // protection to guarantee parsing progress
    if (current_len == 0) {
      CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
      
      for (unsigned i = 0; i < current_frame_position; ++i) {
        CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
      }
      
      // reset the parser
      current_frame_position = 0;
      parser_state = crsf_parser_state_t::unsynced;
      return false;
    }
    
    len -= current_len;
    frame += current_len;
    
    if (crsf_parse_buffer(values, num_values, max_channels)) {
      ret = true;
    }
  }
  
  
  return ret;
}

static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
{
  uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
  
  if (parser_state == crsf_parser_state_t::unsynced) {
    // there is no sync byte, try to find an RC packet by searching for a matching frame length and type
    for (unsigned i = 1; i < current_frame_position - 1; ++i) {
      if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
          crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
            parser_state = crsf_parser_state_t::synced;
            unsigned frame_offset = i - 1;
            CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
            
            // move the rest of the buffer to the beginning
            if (frame_offset != 0) {
              memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
              current_frame_position -= frame_offset;
            }
            
            break;
          }
    }
  }
  
  if (parser_state != crsf_parser_state_t::synced) {
    if (current_frame_position >= sizeof(crsf_frame_t)) {
      // discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
      current_frame_position = 3;
      
      for (unsigned i = 0; i < current_frame_position; ++i) {
        crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
      }
      
      CRSF_VERBOSE("Discarding buffer");
    }
    
    return false;
  }
  
  
  if (current_frame_position < 3) {
    // wait until we have the header & type
    return false;
  }
  
  // Now we have at least the header and the type
  
  const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
  
  if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
    // frame too long or bogus -> discard everything and go into unsynced state
    current_frame_position = 0;
    parser_state = crsf_parser_state_t::unsynced;
    CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
    return false;
  }
  
  if (current_frame_position < current_frame_length) {
    // we don't have the full frame yet -> wait for more data
    CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
    return false;
  }
  
  bool ret = false;
  
  // Now we have the full frame
  
  if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
      crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
        const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
        
        if (crc == crsf_frame_CRC(crsf_frame)) {
          const crsf_payload_RC_channels_packed_t *const rc_channels =
            (crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
          *num_values = std::min<uint16_t>(max_channels, 16);
          
          if (max_channels > 0) { values[0] = (rc_channels->chan0); }
          
          if (max_channels > 1) { values[1] = (rc_channels->chan1); }

          if (max_channels > 2) { values[2] = (rc_channels->chan2); }
          
          if (max_channels > 3) { values[3] = (rc_channels->chan3); }
          
          if (max_channels > 4) { values[4] = (rc_channels->chan4); }
          
          if (max_channels > 5) { values[5] = (rc_channels->chan5); }
          
          if (max_channels > 6) { values[6] = (rc_channels->chan6); }
          
          if (max_channels > 7) { values[7] = (rc_channels->chan7); }
          
          if (max_channels > 8) { values[8] = (rc_channels->chan8); }
          
          if (max_channels > 9) { values[9] = (rc_channels->chan9); }
          
          if (max_channels > 10) { values[10] = (rc_channels->chan10); }
          
          if (max_channels > 11) { values[11] = (rc_channels->chan11); }
          
          if (max_channels > 12) { values[12] = (rc_channels->chan12); }
          
          if (max_channels > 13) { values[13] = (rc_channels->chan13); }
          
          if (max_channels > 14) { values[14] = (rc_channels->chan14); }
          
          if (max_channels > 15) { values[15] = (rc_channels->chan15); }
          
          CRSF_VERBOSE("Got Channels");
          
          ret = true;
          
        } else {
          CRSF_DEBUG("CRC check failed");
        }
        
      } else {
        CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
        // We could check the CRC here and reset the parser into unsynced state if it fails.
        // But in practise it's robust even without that.
      }
  
  // Either reset or move the rest of the buffer
  if (current_frame_position > current_frame_length) {
    CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
    memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
    current_frame_position -= current_frame_length;
    
  } else {
    current_frame_position = 0;
  }
  
  return ret;
}

uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
{
	crc ^= a;

	for (int i = 0; i < 8; ++i) {
		if (crc & 0x80) {
			crc = (crc << 1) ^ 0xD5;

		} else {
			crc = crc << 1;
		}
	}

	return crc;
}

uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
{
	uint8_t crc = 0;

	for (int i = 0; i < len; ++i) {
		crc = crc8_dvb_s2(crc, buf[i]);
	}

	return crc;
}

uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
{
	// CRC includes type and payload
	uint8_t crc = crc8_dvb_s2(0, frame.type);

	for (int i = 0; i < frame.header.length - 2; ++i) {
		crc = crc8_dvb_s2(crc, frame.payload[i]);
	}

	return crc;
}