#include "PlocSupvHelper.h"

#include <etl/crc16_ccitt.h>
#include <fcntl.h>  // Contains file controls like O_RDWR
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <unistd.h>

#include <cmath>
#include <filesystem>
#include <fstream>

#include "OBSWConfig.h"
#include "tas/hdlc.h"
#ifdef XIPHOS_Q7S
#include "bsp_q7s/fs/FilesystemHelper.h"
#include "bsp_q7s/fs/SdCardManager.h"
#endif

#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/timemanager/Countdown.h"
#include "fsfw_hal/linux/uart/helper.h"
#include "mission/utility/Filenaming.h"
#include "mission/utility/ProgressPrinter.h"
#include "mission/utility/Timestamp.h"

using namespace returnvalue;

PlocSupvHelper::PlocSupvHelper(object_id_t objectId)
    : SystemObject(objectId), recRingBuf(4096, true) {
  spParams.maxSize = sizeof(commandBuffer);
  resetSpParams();
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  lock = MutexFactory::instance()->createMutex();
}

PlocSupvHelper::~PlocSupvHelper() = default;

ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) {
  UartCookie* uartCookie = dynamic_cast<UartCookie*>(cookie);
  if (uartCookie == nullptr) {
    return FAILED;
  }
  std::string devname = uartCookie->getDeviceFile();
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno
                 << ", " << strerror(errno) << std::endl;
    return FAILED;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;  // Clear parity bit
  uart::setParity(tty, uartCookie->getParity());
  uart::setStopbits(tty, uartCookie->getStopBits());
  uart::setBitsPerWord(tty, BitsPerWord::BITS_8);
  tty.c_cflag &= ~CRTSCTS;  // Disable RTS/CTS hardware flow control
  uart::enableRead(tty);
  uart::ignoreCtrlLines(tty);

  // Use non-canonical mode and clear echo flag
  tty.c_lflag &= ~(ICANON | ECHO);

  // Non-blocking mode, 0.5 seconds timeout
  tty.c_cc[VTIME] = 5;
  tty.c_cc[VMIN] = 0;

  uart::setBaudrate(tty, uartCookie->getBaudrate());
  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error ["
                 << errno << ", " << strerror(errno) << std::endl;
  }
  // Flush received and unread data
  tcflush(serialPort, TCIOFLUSH);
  return OK;
}

ReturnValue_t PlocSupvHelper::initialize() {
#ifdef XIPHOS_Q7S
  sdcMan = SdCardManager::instance();
  if (sdcMan == nullptr) {
    sif::warning << "PlocSupvHelper::initialize: Invalid SD Card Manager" << std::endl;
    return returnvalue::FAILED;
  }
#endif
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) {
  ReturnValue_t result;
  lock->lockMutex();
  internalState = InternalState::IDLE;
  lock->unlockMutex();
  while (true) {
    semaphore->acquire();
    int bytesRead = 0;
    while (true) {
      bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                       static_cast<unsigned int>(recBuf.size()));
      if (bytesRead == 0) {
        {
          MutexGuard mg(lock);
          if (internalState == InternalState::FINISH) {
            // Flush received and unread data
            tcflush(serialPort, TCIOFLUSH);
            internalState = InternalState::IDLE;
            break;
          }
        }
      } else if (bytesRead < 0) {
        sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno
                     << ", " << strerror(errno) << "]" << std::endl;
        break;
      } else if (bytesRead >= static_cast<int>(recBuf.size())) {
        sif::error << "ScexUartReader::performOperation: Receive buffer too small for " << bytesRead
                   << " bytes" << std::endl;
      } else if (bytesRead > 0) {
        if (debugMode) {
          sif::info << "Received " << bytesRead
                    << " bytes from the Solar Cell Experiment:" << std::endl;
        }
        recRingBuf.writeData(recBuf.data(), bytesRead);
        // insert buffer into ring buffer here
        // ReturnValue_t result = dleParser.passData(recBuf.data(), bytesRead);
        // TODO: Parse ring buffer here
      }
      lock->lockMutex();
      InternalState currentState = internalState;
      lock->unlockMutex();
      switch (currentState) {
        case InternalState::IDLE: {
          break;
        }
        case InternalState::UPDATE: {
          result = executeUpdate();
          if (result == returnvalue::OK) {
            triggerEvent(SUPV_UPDATE_SUCCESSFUL, result);
          } else if (result == PROCESS_TERMINATED) {
            // Event already triggered
          } else {
            triggerEvent(SUPV_UPDATE_FAILED, result);
          }
          MutexGuard mg(lock);
          internalState = InternalState::IDLE;
          break;
        }
        case InternalState::CHECK_MEMORY: {
          executeFullCheckMemoryCommand();
          MutexGuard mg(lock);
          internalState = InternalState::IDLE;
          break;
        }
        case InternalState::CONTINUE_UPDATE: {
          result = continueUpdate();
          if (result == returnvalue::OK) {
            triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result);
          } else if (result == PROCESS_TERMINATED) {
            // Event already triggered
          } else {
            triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result);
          }
          MutexGuard mg(lock);
          internalState = InternalState::IDLE;
          break;
        }
        case InternalState::REQUEST_EVENT_BUFFER: {
          result = performEventBufferRequest();
          if (result == returnvalue::OK) {
            triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result);
          } else if (result == PROCESS_TERMINATED) {
            // Event already triggered
            break;
          } else {
            triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result);
          }
          MutexGuard mg(lock);
          internalState = InternalState::IDLE;
          break;
        }
        case InternalState::HANDLER_DRIVEN: {
          continue;
        }
        default:
          sif::debug << "PlocSupvHelper::performOperation: Invalid state" << std::endl;
          break;
      }
    }
  }
}

ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) {
  if (uartComIF_ == nullptr) {
    sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl;
    return returnvalue::FAILED;
  }
  uartComIF = uartComIF_;
  return returnvalue::OK;
}

void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; }

ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId,
                                          uint32_t startAddress) {
  supv::UpdateParams params;
  params.file = file;
  params.memId = memoryId;
  params.startAddr = startAddress;
  params.bytesWritten = 0;
  params.seqCount = 1;
  params.deleteMemory = true;
  return performUpdate(params);
}

ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) {
  ReturnValue_t result = returnvalue::OK;
#ifdef XIPHOS_Q7S
  result = FilesystemHelper::checkPath(params.file);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::startUpdate: File " << params.file << " does not exist"
                 << std::endl;
    return result;
  }
  result = FilesystemHelper::fileExists(params.file);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::startUpdate: The file " << params.file << " does not exist"
                 << std::endl;
    return result;
  }
#endif
#ifdef TE0720_1CFA
  if (not std::filesystem::exists(file)) {
    sif::warning << "PlocSupvHelper::startUpdate: The file " << file << " does not exist"
                 << std::endl;
    return returnvalue::FAILED;
  }
#endif
  update.file = params.file;
  update.fullFileSize = getFileSize(update.file);
  if (params.bytesWritten > update.fullFileSize) {
    sif::warning << "Invalid start bytes counter " << params.bytesWritten
                 << ", smaller than full file length" << update.fullFileSize << std::endl;
    return returnvalue::FAILED;
  }
  update.length = update.fullFileSize - params.bytesWritten;
  update.memoryId = params.memId;
  update.startAddress = params.startAddr;
  update.progressPercent = 0;
  update.bytesWritten = params.bytesWritten;
  update.crcShouldBeChecked = true;
  update.packetNum = 1;
  update.deleteMemory = params.deleteMemory;
  update.sequenceCount = params.seqCount;
  internalState = InternalState::UPDATE;
  uartComIF->flushUartTxAndRxBuf(comCookie);
  semaphore->release();
  return result;
}

ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId,
                                              uint32_t startAddress) {
  update.file = file;
  update.fullFileSize = getFileSize(file);
  return performMemCheck(memoryId, startAddress, getFileSize(update.file), true);
}

ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAddress,
                                              size_t sizeToCheck, bool checkCrc) {
  update.memoryId = memoryId;
  update.startAddress = startAddress;
  update.length = sizeToCheck;
  update.crcShouldBeChecked = checkCrc;
  internalState = InternalState::CHECK_MEMORY;
  uartComIF->flushUartTxAndRxBuf(comCookie);
  semaphore->release();
  return returnvalue::OK;
}

void PlocSupvHelper::initiateUpdateContinuation() {
  internalState = InternalState::CONTINUE_UPDATE;
  semaphore->release();
}

ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) {
#ifdef XIPHOS_Q7S
  ReturnValue_t result = FilesystemHelper::checkPath(path);
  if (result != returnvalue::OK) {
    return result;
  }
#endif
  if (not std::filesystem::exists(path)) {
    return PATH_NOT_EXISTS;
  }
  eventBufferReq.path = path;
  internalState = InternalState::REQUEST_EVENT_BUFFER;
  uartComIF->flushUartTxAndRxBuf(comCookie);
  semaphore->release();
  return returnvalue::OK;
}

void PlocSupvHelper::stopProcess() { terminate = true; }

void PlocSupvHelper::executeFullCheckMemoryCommand() {
  ReturnValue_t result;
  if (update.crcShouldBeChecked) {
    sif::info << "PLOC SUPV Mem Check: Calculating Image CRC" << std::endl;
    result = calcImageCrc();
    if (result != returnvalue::OK) {
      triggerEvent(SUPV_MEM_CHECK_FAIL, result);
      return;
    }
  }
  sif::info << "PLOC SUPV Mem Check: Selecting Memory" << std::endl;
  result = selectMemory();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_MEM_CHECK_FAIL, result);
    return;
  }
  sif::info << "PLOC SUPV Mem Check: Preparing Update" << std::endl;
  result = prepareUpdate();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_MEM_CHECK_FAIL, result);
    return;
  }
  sif::info << "PLOC SUPV Mem Check: Memory Check" << std::endl;
  result = handleCheckMemoryCommand();
  if (result == returnvalue::OK) {
    triggerEvent(SUPV_MEM_CHECK_OK, result);
  } else {
    triggerEvent(SUPV_MEM_CHECK_FAIL, result);
  }
}

ReturnValue_t PlocSupvHelper::executeUpdate() {
  ReturnValue_t result = returnvalue::OK;
  sif::info << "PLOC SUPV Update MPSoC: Calculating Image CRC" << std::endl;
  result = calcImageCrc();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::info << "PLOC SUPV Update MPSoC: Selecting Memory" << std::endl;
  result = selectMemory();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::info << "PLOC SUPV Update MPSoC: Preparing Update" << std::endl;
  result = prepareUpdate();
  if (result != returnvalue::OK) {
    return result;
  }
  if (update.deleteMemory) {
    sif::info << "PLOC SUPV Update MPSoC: Erasing Memory" << std::endl;
    result = eraseMemory();
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return updateOperation();
}

ReturnValue_t PlocSupvHelper::continueUpdate() {
  ReturnValue_t result = prepareUpdate();
  if (result != returnvalue::OK) {
    return result;
  }
  return updateOperation();
}

ReturnValue_t PlocSupvHelper::updateOperation() {
  sif::info << "PlocSupvHelper::performUpdate: Writing Update Packets" << std::endl;
  auto result = writeUpdatePackets();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::info << "PlocSupvHelper::performUpdate: Memory Check" << std::endl;
  return handleCheckMemoryCommand();
}

ReturnValue_t PlocSupvHelper::writeUpdatePackets() {
  ReturnValue_t result = returnvalue::OK;
#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
  ProgressPrinter progressPrinter("Supervisor update", update.fullFileSize,
                                  ProgressPrinter::HALF_PERCENT);
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
  uint8_t tempData[supv::WriteMemory::CHUNK_MAX + 1]{};
  std::ifstream file(update.file, std::ifstream::binary);
  uint16_t dataLength = 0;
  ccsds::SequenceFlags seqFlags;
  while (update.bytesWritten < update.fullFileSize) {
    if (terminate) {
      terminate = false;
      triggerEvent(TERMINATED_UPDATE_PROCEDURE);
      return PROCESS_TERMINATED;
    }
    size_t remainingSize = update.fullFileSize - update.bytesWritten;
    bool lastSegment = false;
    if (remainingSize > supv::WriteMemory::CHUNK_MAX) {
      dataLength = supv::WriteMemory::CHUNK_MAX;
    } else {
      lastSegment = true;
      dataLength = static_cast<uint16_t>(remainingSize);
    }
    if (file.is_open()) {
      file.seekg(update.bytesWritten, std::ios::beg);
      file.read(reinterpret_cast<char*>(tempData), dataLength);
      if (!file) {
        sif::warning << "PlocSupvHelper::performUpdate: Read only " << file.gcount() << " of "
                     << dataLength << " bytes" << std::endl;
        sif::info << "PlocSupvHelper::performUpdate: Failed when trying to read byte "
                  << update.bytesWritten << std::endl;
      }
    } else {
      return FILE_CLOSED_ACCIDENTALLY;
    }
    if (update.bytesWritten == 0) {
      seqFlags = ccsds::SequenceFlags::FIRST_SEGMENT;
    } else if (lastSegment) {
      seqFlags = ccsds::SequenceFlags::LAST_SEGMENT;
    } else {
      seqFlags = ccsds::SequenceFlags::CONTINUATION;
    }
    resetSpParams();
    float progress = static_cast<float>(update.bytesWritten) / update.fullFileSize;
    uint8_t progPercent = std::floor(progress * 100);
    if (progPercent > update.progressPercent) {
      update.progressPercent = progPercent;
      if (progPercent % 5 == 0) {
        // Useful to allow restarting the update
        triggerEvent(SUPV_UPDATE_PROGRESS, buildProgParams1(progPercent, update.sequenceCount),
                     update.bytesWritten);
      }
    }
    supv::WriteMemory packet(spParams);
    result = packet.buildPacket(seqFlags, update.sequenceCount, update.memoryId,
                                update.startAddress + update.bytesWritten, dataLength, tempData);
    if (result != returnvalue::OK) {
      triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount),
                   update.bytesWritten);
      return result;
    }
    result = handlePacketTransmission(packet);
    if (result != returnvalue::OK) {
      triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount),
                   update.bytesWritten);
      return result;
    }
    update.sequenceCount++;
    update.packetNum += 1;
    update.bytesWritten += dataLength;

#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
    progressPrinter.print(update.bytesWritten);
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
  }
  return result;
}

uint32_t PlocSupvHelper::buildProgParams1(uint8_t percent, uint16_t seqCount) {
  return (static_cast<uint32_t>(percent) << 24) | static_cast<uint32_t>(seqCount);
}

ReturnValue_t PlocSupvHelper::performEventBufferRequest() {
  using namespace supv;
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  RequestLoggingData packet(spParams);
  result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_EVENT_BUFFERS);
  if (result != returnvalue::OK) {
    return result;
  }
  result = sendCommand(packet);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleAck();
  if (result != returnvalue::OK) {
    return result;
  }
  result =
      handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), supv::recv_timeout::UPDATE_STATUS_REPORT);
  if (result != returnvalue::OK) {
    return result;
  }
  ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size());
  bool exeAlreadyReceived = false;
  if (spReader.getApid() == supv::APID_EXE_FAILURE) {
    exeAlreadyReceived = true;
    result = handleRemainingExeReport(spReader);
  } else if (spReader.getApid() == supv::APID_MRAM_DUMP_TM) {
    result = handleEventBufferReception(spReader);
  }

  if (not exeAlreadyReceived) {
    result = handleExe();
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return result;
}

ReturnValue_t PlocSupvHelper::handleRemainingExeReport(ploc::SpTmReader& reader) {
  size_t remBytes = reader.getPacketDataLen() + 1;
  ReturnValue_t result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN);
  if (result != returnvalue::OK) {
    sif::warning << "Reading exe failure report failed" << std::endl;
  }
  result = exeReportHandling();
  if (result != returnvalue::OK) {
    sif::warning << "Handling exe report failed" << std::endl;
  }
  return result;
}

ReturnValue_t PlocSupvHelper::selectMemory() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::MPSoCBootSelect packet(spParams);
  result = packet.buildPacket(update.memoryId);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handlePacketTransmission(packet);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::prepareUpdate() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::ApidOnlyPacket packet(spParams, supv::APID_PREPARE_UPDATE);
  result = packet.buildPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  result = handlePacketTransmission(packet, PREPARE_UPDATE_EXECUTION_REPORT);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::eraseMemory() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::EraseMemory eraseMemory(spParams);
  result = eraseMemory.buildPacket(update.memoryId, update.startAddress + update.bytesWritten,
                                   update.length);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handlePacketTransmission(eraseMemory, supv::recv_timeout::ERASE_MEMORY);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::handlePacketTransmission(ploc::SpTcBase& packet,
                                                       uint32_t timeoutExecutionReport) {
  ReturnValue_t result = returnvalue::OK;
  result = sendCommand(packet);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleAck();
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleExe(timeoutExecutionReport);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) {
  ReturnValue_t result = returnvalue::OK;
  rememberApid = packet.getApid();
  result = uartComIF->sendMessage(comCookie, packet.getFullPacket(), packet.getFullPacketLen());
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::sendCommand: Failed to send command" << std::endl;
    triggerEvent(SUPV_SENDING_COMMAND_FAILED, result, static_cast<uint32_t>(internalState));
    return result;
  }
  return result;
}

ReturnValue_t PlocSupvHelper::handleAck() {
  ReturnValue_t result = returnvalue::OK;

  result = handleTmReception(supv::SIZE_ACK_REPORT);
  if (result != returnvalue::OK) {
    triggerEvent(ACK_RECEPTION_FAILURE, result, static_cast<uint32_t>(rememberApid));
    sif::warning << "PlocSupvHelper::handleAck: Error in reception of acknowledgment report"
                 << std::endl;
    return result;
  }
  supv::AcknowledgmentReport ackReport(tmBuf.data(), tmBuf.size());
  result = checkReceivedTm(ackReport);
  if (result != returnvalue::OK) {
    return result;
  }
  result = ackReport.checkApid();
  if (result != returnvalue::OK) {
    if (result == SupvReturnValuesIF::RECEIVED_ACK_FAILURE) {
      triggerEvent(SUPV_ACK_FAILURE_REPORT, static_cast<uint32_t>(ackReport.getRefApid()));
    } else if (result == SupvReturnValuesIF::INVALID_APID) {
      triggerEvent(SUPV_ACK_INVALID_APID, static_cast<uint32_t>(rememberApid));
    }
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::handleExe(uint32_t timeout) {
  ReturnValue_t result = returnvalue::OK;

  result = handleTmReception(supv::SIZE_EXE_REPORT, tmBuf.data(), timeout);
  if (result != returnvalue::OK) {
    triggerEvent(EXE_RECEPTION_FAILURE, result, static_cast<uint32_t>(rememberApid));
    sif::warning << "PlocSupvHelper::handleExe: Error in reception of execution report"
                 << std::endl;
    return result;
  }

  return exeReportHandling();
}

ReturnValue_t PlocSupvHelper::exeReportHandling() {
  supv::ExecutionReport exeReport(tmBuf.data(), tmBuf.size());

  ReturnValue_t result = checkReceivedTm(exeReport);
  if (result != returnvalue::OK) {
    return result;
  }
  result = exeReport.checkApid();
  if (result != returnvalue::OK) {
    if (result == SupvReturnValuesIF::RECEIVED_EXE_FAILURE) {
      triggerEvent(SUPV_EXE_FAILURE_REPORT, static_cast<uint32_t>(exeReport.getRefApid()));
    } else if (result == SupvReturnValuesIF::INVALID_APID) {
      triggerEvent(SUPV_EXE_INVALID_APID, static_cast<uint32_t>(rememberApid));
    }
    return result;
  }
  return result;
}

ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* readBuf,
                                                uint32_t timeout) {
  ReturnValue_t result = returnvalue::OK;
  size_t readBytes = 0;
  size_t currentBytes = 0;
  Countdown countdown(timeout);
  if (readBuf == nullptr) {
    readBuf = tmBuf.data();
  }
  while (!countdown.hasTimedOut()) {
    result = receive(readBuf + readBytes, &currentBytes, remainingBytes);
    if (result != returnvalue::OK) {
      return result;
    }
    readBytes += currentBytes;
    remainingBytes = remainingBytes - currentBytes;
    if (remainingBytes == 0) {
      break;
    }
  }
  if (remainingBytes != 0) {
    sif::warning << "PlocSupvHelper::handleTmReception: Failed to read " << std::dec
                 << remainingBytes << " remaining bytes" << std::endl;
    return returnvalue::FAILED;
  }
  return result;
}

ReturnValue_t PlocSupvHelper::checkReceivedTm(ploc::SpTmReader& reader) {
  ReturnValue_t result = reader.checkSize();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid);
    return result;
  }
  result = reader.checkCrc();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid);
    return result;
  }
  return result;
}

ReturnValue_t PlocSupvHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) {
  ReturnValue_t result = returnvalue::OK;
  uint8_t* buffer = nullptr;
  result = uartComIF->requestReceiveMessage(comCookie, requestBytes);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::receive: Failed to request reply" << std::endl;
    triggerEvent(SUPV_HELPER_REQUESTING_REPLY_FAILED, result,
                 static_cast<uint32_t>(static_cast<uint32_t>(internalState)));
    return returnvalue::FAILED;
  }
  result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::receive: Failed to read received message" << std::endl;
    triggerEvent(SUPV_HELPER_READING_REPLY_FAILED, result, static_cast<uint32_t>(internalState));
    return returnvalue::FAILED;
  }
  if (*readBytes > 0) {
    std::memcpy(data, buffer, *readBytes);
  } else {
    TaskFactory::delayTask(40);
  }
  return result;
}

ReturnValue_t PlocSupvHelper::calcImageCrc() {
  ReturnValue_t result = returnvalue::OK;
  if (update.fullFileSize == 0) {
    return returnvalue::FAILED;
  }
#ifdef XIPHOS_Q7S
  result = FilesystemHelper::checkPath(update.file);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupvHelper::calcImageCrc: File " << update.file << " does not exist"
                 << std::endl;
    return result;
  }
#endif

  auto crc16Calcer = etl::crc16_ccitt();
  std::ifstream file(update.file, std::ifstream::binary);
  std::array<uint8_t, 1025> crcBuf{};
#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
  ProgressPrinter progress("Supervisor update crc calculation", update.fullFileSize,
                           ProgressPrinter::ONE_PERCENT);
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
  uint32_t byteCount = 0;
  size_t bytesToRead = 1024;
  while (byteCount < update.fullFileSize) {
    size_t remLen = update.fullFileSize - byteCount;
    if (remLen < 1024) {
      bytesToRead = remLen;
    } else {
      bytesToRead = 1024;
    }
    file.seekg(byteCount, file.beg);
    file.read(reinterpret_cast<char*>(crcBuf.data()), bytesToRead);
    crc16Calcer.add(crcBuf.begin(), crcBuf.begin() + bytesToRead);

#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
    progress.print(byteCount);
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
    byteCount += bytesToRead;
  }
#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
  progress.print(byteCount);
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
  update.crc = crc16Calcer.value();
  return result;
}

ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  // Will hold status report for later processing
  std::array<uint8_t, 32> statusReportBuf{};
  supv::UpdateStatusReport updateStatusReport(tmBuf.data(), tmBuf.size());
  // Verification of update write procedure
  supv::CheckMemory packet(spParams);
  result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize);
  if (result != returnvalue::OK) {
    return result;
  }
  result = sendCommand(packet);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleAck();
  if (result != returnvalue::OK) {
    return result;
  }

  bool exeAlreadyHandled = false;
  uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::recv_timeout::UPDATE_STATUS_REPORT);
  result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), timeout);
  ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size());
  if (spReader.getApid() == supv::APID_EXE_FAILURE) {
    exeAlreadyHandled = true;
    result = handleRemainingExeReport(spReader);
  } else if (spReader.getApid() == supv::APID_UPDATE_STATUS_REPORT) {
    size_t remBytes = spReader.getPacketDataLen() + 1;
    result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN,
                               supv::recv_timeout::UPDATE_STATUS_REPORT);
    if (result != returnvalue::OK) {
      sif::warning
          << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report"
          << std::endl;
      return result;
    }
    result = updateStatusReport.checkCrc();
    if (result != returnvalue::OK) {
      sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC check failed" << std::endl;
      return result;
    }
    // Copy into other buffer because data will be overwritten when reading execution report
    std::memcpy(statusReportBuf.data(), tmBuf.data(), updateStatusReport.getNominalSize());
  }

  if (not exeAlreadyHandled) {
    result = handleExe(CRC_EXECUTION_TIMEOUT);
    if (result != returnvalue::OK) {
      return result;
    }
  }

  // Now process the status report
  updateStatusReport.setData(statusReportBuf.data(), statusReportBuf.size());
  result = updateStatusReport.parseDataField();
  if (result != returnvalue::OK) {
    return result;
  }
  if (update.crcShouldBeChecked) {
    result = updateStatusReport.verifycrc(update.crc);
    if (result != returnvalue::OK) {
      sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x"
                   << std::setfill('0') << std::hex << std::setw(4)
                   << static_cast<uint16_t>(update.crc) << " but received CRC 0x" << std::setw(4)
                   << updateStatusReport.getCrc() << std::dec << std::endl;
      return result;
    }
  }
  return result;
}

uint32_t PlocSupvHelper::getFileSize(std::string filename) {
  std::ifstream file(filename, std::ifstream::binary);
  file.seekg(0, file.end);
  uint32_t size = file.tellg();
  file.close();
  return size;
}

ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reader) {
  ReturnValue_t result = returnvalue::OK;
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  std::string filename = Filenaming::generateAbsoluteFilename(
      eventBufferReq.path, eventBufferReq.filename, timestamping);
  std::ofstream file(filename, std::ios_base::app | std::ios_base::out);
  uint32_t packetsRead = 0;
  size_t requestLen = 0;
  bool firstPacket = true;
  for (packetsRead = 0; packetsRead < NUM_EVENT_BUFFER_PACKETS; packetsRead++) {
    if (terminate) {
      triggerEvent(SUPV_EVENT_BUFFER_REQUEST_TERMINATED, packetsRead - 1);
      file.close();
      return PROCESS_TERMINATED;
    }
    if (packetsRead == NUM_EVENT_BUFFER_PACKETS - 1) {
      requestLen = SIZE_EVENT_BUFFER_LAST_PACKET;
    } else {
      requestLen = SIZE_EVENT_BUFFER_FULL_PACKET;
    }
    if (firstPacket) {
      firstPacket = false;
      requestLen -= 6;
    }
    result = handleTmReception(requestLen);
    if (result != returnvalue::OK) {
      sif::debug << "PlocSupvHelper::handleEventBufferReception: Failed while trying to read packet"
                 << " " << packetsRead + 1 << std::endl;
      file.close();
      return result;
    }
    ReturnValue_t result = reader.checkCrc();
    if (result != returnvalue::OK) {
      triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid);
      return result;
    }
    uint16_t apid = reader.getApid();
    if (apid != supv::APID_MRAM_DUMP_TM) {
      sif::warning << "PlocSupvHelper::handleEventBufferReception: Did not expect space packet "
                   << "with APID 0x" << std::hex << apid << std::endl;
      file.close();
      return EVENT_BUFFER_REPLY_INVALID_APID;
    }
    file.write(reinterpret_cast<const char*>(reader.getPacketData()),
               reader.getPayloadDataLength());
  }
  return result;
}

void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; }

ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                          size_t sendLen) {
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }

ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                  size_t* size) {
  return returnvalue::OK;
}

ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) {
  size_t availableData = recRingBuf.getAvailableReadData();
  if (availableData == 0) {
    return NO_PACKET_FOUND;
  }
  if (availableData > encodedBuf.size()) {
    return DECODE_BUF_TOO_SMALL;
  }
  ReturnValue_t result = recRingBuf.readData(encodedBuf.data(), availableData);
  if (result != returnvalue::OK) {
    return result;
  }
  bool startMarkerFound = false;
  size_t startIdx = 0;
  return returnvalue::OK;
  for (size_t idx = 0; idx < availableData; idx++) {
    // handle start marker
    if (encodedBuf[idx] == HDLC_START_MARKER) {
      if (not startMarkerFound) {
        startMarkerFound = true;
        startIdx = idx;
      } else {
        readSize = idx;
        return POSSIBLE_PACKET_LOSS_CONSECUTIVE_START;
      }
    }
    if (encodedBuf[idx] == HDLC_END_MARKER) {
      if (startMarkerFound) {
        // Probably a packet, so decode it
        size_t decodedLen = 0;
        hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1, decodedBuf.data(), &decodedLen);
        readSize = decodedLen;
        return returnvalue::OK;
      } else {
        readSize = ++idx;
        return POSSIBLE_PACKET_LOSS_CONSECUTIVE_END;
      }
    }
  }
  return NO_PACKET_FOUND;
}