#include "lora_frame_logic.h"

#include <string.h>

uint16_t lora_crc16_ccitt(const uint8_t *data, size_t len) {
  if (!data && len > 0) {
    return 0;
  }
  uint16_t crc = 0xFFFF;
  for (size_t i = 0; i < len; ++i) {
    crc ^= static_cast<uint16_t>(data[i]) << 8;
    for (uint8_t b = 0; b < 8; ++b) {
      if (crc & 0x8000) {
        crc = (crc << 1) ^ 0x1021;
      } else {
        crc <<= 1;
      }
    }
  }
  return crc;
}

bool lora_build_frame(uint8_t msg_kind, uint16_t device_id_short, const uint8_t *payload, size_t payload_len,
                      uint8_t *out_frame, size_t out_cap, size_t &out_len) {
  out_len = 0;
  if (!out_frame) {
    return false;
  }
  if (payload_len > 0 && !payload) {
    return false;
  }
  if (payload_len > (SIZE_MAX - 5)) {
    return false;
  }
  size_t needed = payload_len + 5;
  if (needed > out_cap) {
    return false;
  }

  size_t idx = 0;
  out_frame[idx++] = msg_kind;
  out_frame[idx++] = static_cast<uint8_t>(device_id_short >> 8);
  out_frame[idx++] = static_cast<uint8_t>(device_id_short & 0xFF);
  if (payload_len > 0) {
    memcpy(&out_frame[idx], payload, payload_len);
    idx += payload_len;
  }
  uint16_t crc = lora_crc16_ccitt(out_frame, idx);
  out_frame[idx++] = static_cast<uint8_t>(crc >> 8);
  out_frame[idx++] = static_cast<uint8_t>(crc & 0xFF);
  out_len = idx;
  return true;
}

LoraFrameDecodeStatus lora_parse_frame(const uint8_t *frame, size_t frame_len, uint8_t max_msg_kind, uint8_t *out_msg_kind,
                                       uint16_t *out_device_id_short, uint8_t *out_payload, size_t payload_cap,
                                       size_t *out_payload_len) {
  if (!frame || !out_msg_kind || !out_device_id_short || !out_payload_len) {
    return LoraFrameDecodeStatus::LengthMismatch;
  }
  if (frame_len < 5) {
    return LoraFrameDecodeStatus::LengthMismatch;
  }

  size_t payload_len = frame_len - 5;
  if (payload_len > payload_cap || (payload_len > 0 && !out_payload)) {
    return LoraFrameDecodeStatus::LengthMismatch;
  }

  uint16_t crc_calc = lora_crc16_ccitt(frame, frame_len - 2);
  uint16_t crc_rx = static_cast<uint16_t>(frame[frame_len - 2] << 8) | frame[frame_len - 1];
  if (crc_calc != crc_rx) {
    return LoraFrameDecodeStatus::CrcFail;
  }

  uint8_t msg_kind = frame[0];
  if (msg_kind > max_msg_kind) {
    return LoraFrameDecodeStatus::InvalidMsgKind;
  }

  *out_msg_kind = msg_kind;
  *out_device_id_short = static_cast<uint16_t>(frame[1] << 8) | frame[2];
  if (payload_len > 0) {
    memcpy(out_payload, &frame[3], payload_len);
  }
  *out_payload_len = payload_len;
  return LoraFrameDecodeStatus::Ok;
}
