//
// Created by erki on 28.07.22.
//

#include "radio_protocol.hpp"

#include <cstring>

namespace
{

using namespace radio::protocol;

[[nodiscard]] std::uint8_t* serializeAddress(std::uint8_t* data, const Address& address)
{
  std::memmove(data, &address.pan_id, sizeof(address.pan_id));
  data += sizeof(address.pan_id);

  if (address.getAddressingMode() == FrameControlField::ADDRESSING_MODE_SHORT)
  {
    const std::uint16_t mac = address.getShortAddress();
    std::memmove(data, &mac, sizeof(mac));
    data += sizeof(mac);
  }
  else
  {
    const std::uint64_t mac = address.getLongAddress();
    std::memmove(data, &mac, sizeof(mac));
    data += sizeof(mac);
  }

  return data;
}

[[nodiscard]] std::uint8_t* serializePhyHeader(std::uint8_t* data, const PhyHeader& header)
{
  static_assert(sizeof(PhyHeader) == 1, "PHY Header must be 1 byte/octet.");
  std::memmove(data, &header, sizeof(PhyHeader));
  data += sizeof(PhyHeader);

  return data;
}

[[nodiscard]] std::uint8_t* serializeMacProtocolDataUnit(std::uint8_t* data, const FrameStructure& frame)
{
  static_assert(sizeof(frame.frame_control_field) == 2, "FCF must be 2 bytes/octets.");
  std::memmove(data, &frame.frame_control_field, sizeof(frame.frame_control_field));
  data += sizeof(FrameControlField);

  std::memmove(data, &frame.sequence_number, sizeof(frame.sequence_number));
  data += sizeof(frame.sequence_number);

  data = serializeAddress(data, frame.destination_address);
  data = serializeAddress(data, frame.source_address);

  std::memmove(data, frame.payload.data(), frame.payload_length);
  data += frame.payload_length;

  return data;
}

[[nodiscard]] const std::uint8_t* deserializeAddress(const std::uint8_t* data, const FrameControlField::AddressingMode& addressing_mode, Address& address)
{
  std::memmove(&address.pan_id, data, sizeof(address.pan_id));
  data += sizeof(address.pan_id);

  if (addressing_mode == FrameControlField::ADDRESSING_MODE_SHORT)
  {
    std::uint16_t mac = 0;
    std::memmove(&mac, data, sizeof(mac));
    data += sizeof(mac);

    address.setShortAddress(mac);
  }
  else
  {
    std::uint64_t mac = 0;
    std::memmove(&mac, data, sizeof(mac));
    data += sizeof(mac);

    address.setLongAddress(mac);
  }

  return data;
}

[[nodiscard]] const std::uint8_t* deserializePhyHeader(const std::uint8_t* data, PhyHeader& header)
{
  std::memmove(&header, data, sizeof(PhyHeader));
  data += sizeof(PhyHeader);

  return data;
}

[[nodiscard]] FrameStructure deserializeMacProtocolDataUnit(const std::uint8_t* data, const std::uint8_t total_frame_length)
{
  FrameStructure frame;
  std::memmove(&frame.frame_control_field, data, sizeof(frame.frame_control_field));
  data += sizeof(frame.frame_control_field);

  std::memmove(&frame.sequence_number, data, sizeof(frame.sequence_number));
  data += sizeof(frame.sequence_number);

  data = deserializeAddress(data, frame.frame_control_field.destination_addressing_mode, frame.destination_address);
  data = deserializeAddress(data, frame.frame_control_field.source_addressing_mode, frame.source_address);

  frame.payload_length = frame.calculatePayloadLength(total_frame_length);
  std::memmove(frame.payload.data(), data, frame.payload_length);
  data += frame.payload_length;

  std::memmove(&frame.frame_checksum, data, sizeof(frame.frame_checksum));

  return frame;
}

}

namespace radio::protocol
{

std::size_t composeFrameBuffer(std::uint8_t* data, const FrameStructure& frame)
{
  std::uint8_t* const data_start = data;

  PhyHeader header;
  std::memset(&header, 0, sizeof(PhyHeader));
  header.frame_length = frame.getTotalFrameLength();
  data = serializePhyHeader(data, header);

  data = serializeMacProtocolDataUnit(data, frame);

  return data - data_start;
}

FrameStructure decomposeFrameBuffer(const std::uint8_t* data)
{
  PhyHeader header;
  data = deserializePhyHeader(data, header);

  const std::uint8_t total_frame_length = header.frame_length;
  const FrameStructure frame = deserializeMacProtocolDataUnit(data, total_frame_length);

  return frame;
}

}