#pragma once

#include <stdint.h>
#define HYDROLINK_HEADER_LEN 4
#define HYDROLINK_CRC_LEN 2
#define HYDROLINK_NON_PAYLOAD_LEN (HYDROLINK_HEADER_LEN + HYDROLINK_CRC_LEN)
#define HYDROLINK_MAX_PAYLOAD_LEN 6
#define HYDROLINK_MAX_MSG_LEN (HYDROLINK_MAX_PAYLOAD_LEN + HYDROLINK_NON_PAYLOAD_LEN)

typedef struct {
  uint8_t msg_id;
  uint8_t crc_extra;
} hydrolink_crc_extra_t;

#define HYDROLINK_MSG_CRCS {{1, 73}, {2, 58}, {3, 250}, {4, 38}, {5, 28}}

/**
* @brief Get the crc_extra byte corresponding to msg_id
*
* @param msg_id the message's id
* @param crc[out] the crc_extra byte as output of this function
* @return Return 1 if successfull, 0 otherwise.
static inline uint8_t hydrolink_get_crc_extra(uint8_t msg_id, uint8_t *crc) {
  static const hydrolink_crc_extra_t crcs[] = HYDROLINK_MSG_CRCS;
  uint8_t left = 0;
  uint8_t right = sizeof(crcs) / sizeof(crcs[0]) - 1;
  while (left < right) {
    uint8_t center = (left+right+1) / 2;
    if (msg_id < crcs[center].msg_id) {
      right = center - 1;
      continue;
    }
    if (msg_id > crcs[center].msg_id) {
      left = center;
      continue;
    }
    left = mid;
    break;
  }
  if (crcs[left].msg_id != msg_id) {
    return 0;
  }
  *crc = crcs[left].crc_extra;
  return 1;
}

struct hydrolink_msg_s;

typedef struct hydrolink_msg_s hydrolink_msg_t;

struct hydrolink_msg_s {
  uint8_t id;
  uint8_t payload_length;
  uint8_t dst_id;
  uint8_t src_id;
  uint8_t payload[HYDROLINK_MAX_PAYLOAD_LEN];
  uint16_t crc;
};

#define HYDROLINK_PAYLOAD(msg) ((const char *)(&((msg)->payload[0])))
#define HYDROLINK_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload[0])))

#define hydrolink_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_char(buf, wire_offset, b) buf[wire_offset] = (char)b
#define hydrolink_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b
#define hydrolink_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
#define hydrolink_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define hydrolink_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)

static inline void byte_swap_2(char *dst, const char *src) {
  dst[0] = src[1];
  dst[1] = src[0];
}

static inline void byte_swap_4(char *dst, const char *src) {
  dst[0] = src[3];
  dst[1] = src[2];
  dst[2] = src[1];
  dst[3] = src[0];
}

static inline void byte_swap_8(char *dst, const char *src) {
  dst[0] = src[7];
  dst[1] = src[6];
  dst[2] = src[5];
  dst[3] = src[4];
  dst[4] = src[3];
  dst[5] = src[2];
  dst[6] = src[1];
  dst[7] = src[0];
}
                
#define HYDROLINK_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE HYDROLINK_RETURN_## TYPE(const hydrolink_msg_t *msg, uint8_t offset) {\
  TYPE ret;  \
  byte_swap_## SIZE((char*)&ret, &HYDROLINK_PAYLOAD(msg)[offset]); \
  return ret; \
}
                
HYDROLINK_MSG_RETURN_TYPE(float, 4)

HYDROLINK_MSG_RETURN_TYPE(double, 8)

#define HYDROLINK_RETURN_char(msg, wire_offset) (char)HYDROLINK_PAYLOAD(msg)[wire_offset]
#define HYDROLINK_RETURN_int8_t(msg, wire_offset) (int8_t)HYDROLINK_PAYLOAD(msg)[wire_offset]
#define HYDROLINK_RETURN_uint8_t(msg, wire_offset) (uint8_t)HYDROLINK_PAYLOAD(msg)[wire_offset]
HYDROLINK_MSG_RETURN_TYPE(int16_t, 2)

HYDROLINK_MSG_RETURN_TYPE(uint16_t, 2)

HYDROLINK_MSG_RETURN_TYPE(int32_t, 4)

HYDROLINK_MSG_RETURN_TYPE(uint32_t, 4)

HYDROLINK_MSG_RETURN_TYPE(int64_t, 8)

HYDROLINK_MSG_RETURN_TYPE(uint64_t, 8)

static inline void crc_xmodem_init(uint16_t *crc) { *crc = 0; }

static inline void crc_xmodem_accumulate(uint8_t data, uint16_t *crc) {
  *crc = *crc ^ ((uint16_t)data << 8);
  for (uint8_t j = 0; j < 8; j++) {
    if (*crc & 0x8000) {
      *crc = (*crc << 1) ^ 0x1021;
    } else {
      *crc <<= 1;
    }
  }
}

static inline uint16_t crc_xmodem_calculate(const uint8_t *data, uint8_t length) {
  uint16_t crc;
  crc_xmodem_init(&crc);
  for (uint8_t i = 0; i < length; i++) {
    crc_xmodem_accumulate(*data, &crc);
  }
  return crc;
}

void hydrolink_fill_header_and_crc(hydrolink_msg_t *msg, uint8_t msg_id, uint8_t payload_length, uint8_t dst_id, uint8_t src_id, uint8_t crc_extra) {
  msg->id = msg_id;
  msg->payload_length = payload_length;
  msg->dst_id = dst_id;
  msg->src_id = src_id;
  uint16_t crc;
  crc_xmodem_init(&crc);
  for(int i = 0; i < msg->payload_length; ++i) {
    crc_xmodem_accumulate(msg->payload[i], &crc);
  }
  crc_xmodem_accumulate(crc_extra, &crc);
  msg->crc = crc;
}

uint8_t hydrolink_serialize_message(hydrolink_msg_t *msg, uint8_t *buffer) {
  buffer[0] = msg->id;
  buffer[1] = msg->payload_length;
  buffer[2] = msg->dst_id;
  buffer[3] = msg->src_id;
  uint8_t *p = &buffer[4];
  for(int i =0; i<msg->payload_length;++i) {
    *p++ = msg->payload[i]; 
  }
  *p++ = (uint8_t)(msg->crc >> 8);
  *p++ = (uint8_t)(msg->crc & 0xff);
  return HYDROLINK_NON_PAYLOAD_LEN + msg->payload_length;
}

uint8_t hydrolink_deserialize_header(hydrolink_msg_t *msg, uint8_t *buffer, uint8_t buffer_length) {
  if (buffer_length < HYDROLINK_NON_PAYLOAD_LEN) {
    return 0;
  }
  msg->id = buffer[0];
  msg->payload_length = buffer[1];
  msg->dst_id = buffer[2];
  msg->src_id = buffer[3];
  msg->crc = buffer[buffer_length-1] | (((uint16_t)buffer[buffer_length-2]) << 8);
  if (msg->payload_length + HYDROLINK_NON_PAYLOAD_LEN != buffer_length) {
    return 0;
  }
  uint8_t *p = &buffer[HYDROLINK_HEADER_LEN];
  for (int i = 0; i < msg->payload_length; ++i) {
    msg->payload[i] = *p++;
  }
  return msg->payload_length + HYDROLINK_NON_PAYLOAD_LEN;
}