#include "MpsocCommunication.h"

#include "fsfw/globalfunctions/CRC.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
#include "fsfw/tmtcpacket/ccsds/header.h"
#include "linux/payload/plocMpsocHelpers.h"
#include "unistd.h"

MpsocCommunication::MpsocCommunication(object_id_t objectId, SerialConfig cfg)
    : SystemObject(objectId), readRingBuf(4096, true), helper(cfg) {}

ReturnValue_t MpsocCommunication::initialize() { return helper.initialize(); }

ReturnValue_t MpsocCommunication::send(const uint8_t* data, size_t dataLen) {
  return helper.send(data, dataLen);
}

ReturnValue_t MpsocCommunication::parseAndRetrieveNextPacket() {
  // We do not have a data link layer, so this whole thing is a mess in any case..
  // But basically, we try to parse space packets from the internal ring buffer and trasnfer
  // them to the higher level device handler. The CRC check is performed here as well, with
  // few other ways to detect if we even have a valid packet.
  size_t availableReadData = readRingBuf.getAvailableReadData();
  // Minimum valid size for a space packet header.
  if (availableReadData < ccsds::HEADER_LEN + 1) {
    return returnvalue::OK;
  }
  readRingBuf.readData(readBuf, availableReadData);
  spReader.setReadOnlyData(readBuf, availableReadData);
  auto res = spReader.checkSize();
  if (res != returnvalue::OK) {
    return res;
  }
  // The packet might be garbage, with no way to recover without a data link layer.
  if (spReader.getFullPacketLen() > 4096) {
    readRingBuf.clear();
    // TODO: Maybe we should also clear the serial input buffer in Linux?
    return FAULTY_PACKET_SIZE;
  }
  if (availableReadData < spReader.getFullPacketLen()) {
    // Might be split packet where the rest still has to be read.
    return returnvalue::OK;
  }
  if (CRC::crc16ccitt(readBuf, spReader.getFullPacketLen()) != 0) {
    // Possibly invalid packet. We can not even trust the detected packet length.
    // Just clear the whole read buffer as well.
    readRingBuf.clear();
    triggerEvent(mpsoc::CRC_FAILURE);
    return CRC_CHECK_FAILED;
  }
  readRingBuf.deleteData(spReader.getFullPacketLen());
  return PACKET_RECEIVED;
}

ReturnValue_t MpsocCommunication::readSerialInterface() {
  int bytesRead = read(helper.rawFd(), readBuf, sizeof(readBuf));
  if (bytesRead < 0) {
    return returnvalue::FAILED;
  }
  if (bytesRead > 0) {
    if (MPSOC_LOW_LEVEL_RX_WIRETAPPING) {
      sif::debug << "Read " << bytesRead << " bytes on the MPSoC interface" << std::endl;
    }
    return readRingBuf.writeData(readBuf, bytesRead);
  }
  return returnvalue::OK;
}

const SpacePacketReader& MpsocCommunication::getSpReader() const { return spReader; }

SerialCommunicationHelper& MpsocCommunication::getComHelper() { return helper; }