#include "ScexHelper.h"

#include <fsfw/globalfunctions/CRC.h>

#include "fsfw/serviceinterface.h"

ScexHelper::ScexHelper() {}

ReturnValue_t ScexHelper::serialize(uint8_t** buffer, size_t* size, size_t maxSize,
                                    Endianness streamEndianness) const {
  return RETURN_FAILED;
}

size_t ScexHelper::getSerializedSize() const { return totalPacketLen; }

ReturnValue_t ScexHelper::deSerialize(const uint8_t** buffer, size_t* size,
                                      Endianness streamEndianness) {
  if (buffer == nullptr or size == nullptr) {
    return RETURN_FAILED;
  }
  if (*size < 7) {
    return STREAM_TOO_SHORT;
  }
  start = *buffer;
  cmdByteRaw = **buffer;
  cmd = static_cast<scex::Cmds>((cmdByteRaw >> 1) & 0b11111);

  *buffer += 1;
  packetCounter = **buffer;

  *buffer += 1;
  totalPacketCounter = **buffer;

  *buffer += 1;
  payloadLen = (**buffer << 8) | *(*buffer + 1);

  *buffer += 2;
  payloadStart = *buffer;
  totalPacketLen = payloadLen + scex::HEADER_LEN + scex::CRC_LEN;
  if (totalPacketLen >= *size) {
    return STREAM_TOO_SHORT;
  }
  *buffer += payloadLen;
  crc = (**buffer << 8) | *(*buffer + 1);
  if (CRC::crc16ccitt(start, totalPacketLen) != 0) {
    return INVALID_CRC;
  }
  return RETURN_OK;
}

scex::Cmds ScexHelper::getCmd() const { return cmd; }

uint8_t ScexHelper::getCmdByteRaw() const { return cmdByteRaw; }

uint16_t ScexHelper::getCrc() const { return crc; }

size_t ScexHelper::getExpectedPacketLen() const { return totalPacketLen; }

uint8_t ScexHelper::getPacketCounter() const { return packetCounter; }

uint16_t ScexHelper::getPayloadLen() const { return payloadLen; }

const uint8_t* ScexHelper::getStart() const { return start; }

uint8_t ScexHelper::getTotalPacketCounter() const { return totalPacketCounter; }

std::ostream& operator<<(std::ostream& os, const ScexHelper& h) {
  using namespace std;
  sif::info << "Command Byte Raw: 0x" << std::setw(2) << std::setfill('0') << std::hex
            << (int)h.cmdByteRaw << " | Command: 0x" << std::setw(2) << std::setfill('0')
            << std::hex << static_cast<int>(h.cmd) << std::dec << std::endl;
  sif::info << "PacketCounter: " << h.packetCounter << endl;
  sif::info << "TotalPacketCount: " << h.totalPacketCounter << endl;
  sif::info << "PayloadLength: " << h.payloadLen << endl;
  sif::info << "TotalPacketLength: " << h.totalPacketLen;

  return os;
}

std::ofstream& operator<<(std::ofstream& of, const ScexHelper& h) {
  of.write(reinterpret_cast<const char*>(h.start), h.getSerializedSize());

  return of;
}