#include <etl/crc16_ccitt.h>
#include <fcntl.h>  // Contains file controls like O_RDWR
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <linux/payload/PlocSupvUartMan.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_hal/linux/serial/helper.h>

#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/timemanager/Countdown.h"
#include "mission/utility/Filenaming.h"
#include "mission/utility/ProgressPrinter.h"
#include "mission/utility/Timestamp.h"
#include "tas/crc.h"

using namespace returnvalue;
using namespace supv;

PlocSupvUartManager::PlocSupvUartManager(object_id_t objectId)
    : SystemObject(objectId),
      recRingBuf(4096, true),
      decodedRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true),
      ipcRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true) {
  resetSpParams();
  spParams.maxSize = cmdBuf.size();
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  lock = MutexFactory::instance()->createMutex();
  ipcLock = MutexFactory::instance()->createMutex();
}

PlocSupvUartManager::~PlocSupvUartManager() = default;

ReturnValue_t PlocSupvUartManager::initializeInterface(CookieIF* cookie) {
  auto* uartCookie = dynamic_cast<SerialCookie*>(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 << "PlocSupvUartManager::initializeInterface: open call failed with error ["
                 << errno << ", " << strerror(errno) << std::endl;
    return FAILED;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;  // Clear parity bit
  serial::setParity(tty, uartCookie->getParity());
  serial::setStopbits(tty, uartCookie->getStopBits());
  serial::setBitsPerWord(tty, BitsPerWord::BITS_8);
  tty.c_cflag &= ~CRTSCTS;  // Disable RTS/CTS hardware flow control
  serial::enableRead(tty);
  serial::ignoreCtrlLines(tty);

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

  // Blocking mode, 0.2 seconds timeout
  tty.c_cc[VTIME] = 2;
  tty.c_cc[VMIN] = 0;

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

ReturnValue_t PlocSupvUartManager::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 PlocSupvUartManager::performOperation(uint8_t operationCode) {
  bool putTaskToSleep = false;
  while (true) {
    {
      MutexGuard mg(lock);
      state = InternalState::SLEEPING;
    }
    semaphore->acquire();
    putTaskToSleep = false;
#if OBSW_THREAD_TRACING == 1
    trace::threadTrace(opCounter, "PLOC SUPV Helper PST");
#endif
    while (true) {
      if (putTaskToSleep) {
        performUartShutdown();
        break;
      }
      handleUartReception();
      InternalState currentState;
      {
        MutexGuard mg(lock);
        currentState = state;
      }
      switch (currentState) {
        case InternalState::SLEEPING:
        case InternalState::GO_TO_SLEEP: {
          putTaskToSleep = true;
          break;
        }
        case InternalState::DEDICATED_REQUEST: {
          updateVtime(1);
          handleRunningLongerRequest();
          updateVtime(2);
          MutexGuard mg(lock);
          state = InternalState::DEFAULT;
          break;
        }
        case InternalState::DEFAULT: {
          break;
        }
      }
    }
  }
}

ReturnValue_t PlocSupvUartManager::handleUartReception() {
  ReturnValue_t result = OK;
  ReturnValue_t status = OK;
  ssize_t bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                           static_cast<unsigned int>(recBuf.size()));
  if (bytesRead == 0) {
    while (result != NO_PACKET_FOUND) {
      result = tryHdlcParsing();
      if (result != NO_PACKET_FOUND and result != OK) {
        status = result;
      }
    }
  } else if (bytesRead < 0) {
    sif::warning << "PlocSupvHelper::performOperation: read call failed with error [" << errno
                 << ", " << strerror(errno) << "]" << std::endl;
    return FAILED;
  } else if (bytesRead >= static_cast<int>(recBuf.size())) {
    sif::error << "PlocSupvHelper::performOperation: Receive buffer too small for " << bytesRead
               << " bytes" << std::endl;
    return FAILED;
  } else if (bytesRead > 0) {
    if (DEBUG_MODE) {
      sif::info << "Received " << bytesRead << " bytes from the PLOC Supervisor:" << std::endl;
      arrayprinter::print(recBuf.data(), bytesRead);
    }
    recRingBuf.writeData(recBuf.data(), bytesRead);
    status = tryHdlcParsing();
  }
  return status;
}

ReturnValue_t PlocSupvUartManager::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 PlocSupvUartManager::performUpdate(const supv::UpdateParams& params) {
  lock->lockMutex();
  InternalState current = state;
  lock->unlockMutex();
  if (current != InternalState::DEFAULT) {
    return HasActionsIF::IS_BUSY;
  }
  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
  {
    MutexGuard mg(lock);

    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;
    state = InternalState::DEDICATED_REQUEST;
    request = Request::UPDATE;
  }
  return result;
}

ReturnValue_t PlocSupvUartManager::performMemCheck(std::string file, uint8_t memoryId,
                                                   uint32_t startAddress) {
  lock->lockMutex();
  InternalState current = state;
  lock->unlockMutex();
  if (current != InternalState::DEFAULT) {
    return HasActionsIF::IS_BUSY;
  }
  return performMemCheck(file, memoryId, startAddress, getFileSize(update.file), true);
}

ReturnValue_t PlocSupvUartManager::performMemCheck(std::string file, uint8_t memoryId,
                                                   uint32_t startAddress, size_t sizeToCheck,
                                                   bool checkCrc) {
  {
    MutexGuard mg(lock);
    update.file = file;
    update.fullFileSize = getFileSize(file);
    state = InternalState::DEDICATED_REQUEST;
    request = Request::CHECK_MEMORY;
    update.memoryId = memoryId;
    update.startAddress = startAddress;
    update.length = sizeToCheck;
    update.crcShouldBeChecked = checkCrc;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() {
  lock->lockMutex();
  InternalState current = state;
  lock->unlockMutex();
  if (current != InternalState::DEFAULT) {
    return HasActionsIF::IS_BUSY;
  }
  MutexGuard mg(lock);
  state = InternalState::DEDICATED_REQUEST;
  request = Request::CONTINUE_UPDATE;
  return returnvalue::OK;
}

// 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;
//   request = Request::REQUEST_EVENT_BUFFER;
//   //uartComIF->flushUartTxAndRxBuf(comCookie);
//   semaphore->release();
//   return returnvalue::OK;
// }

void PlocSupvUartManager::stop() {
  MutexGuard mg(lock);
  if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) {
    return;
  }
  state = InternalState::GO_TO_SLEEP;
}

void PlocSupvUartManager::start() {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING and state != InternalState::GO_TO_SLEEP) {
      return;
    }
    state = InternalState::DEFAULT;
  }

  semaphore->release();
}

void PlocSupvUartManager::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, 1);
    return;
  }
  sif::info << "PLOC SUPV Mem Check: Preparing Update" << std::endl;
  result = prepareUpdate();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_MEM_CHECK_FAIL, result, 2);
    return;
  }
  sif::info << "PLOC SUPV Mem Check: Memory Check" << std::endl;
  handleCheckMemoryCommand(3);
}

ReturnValue_t PlocSupvUartManager::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 PlocSupvUartManager::continueUpdate() {
  ReturnValue_t result = prepareUpdate();
  if (result != returnvalue::OK) {
    return result;
  }
  return updateOperation();
}

ReturnValue_t PlocSupvUartManager::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(0);
}

ReturnValue_t PlocSupvUartManager::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]{};
  if (not std::filesystem::exists(update.file)) {
    sif::error << "PlocSupvUartManager::writeUpdatePackets: File " << update.file
               << " does not exist" << std::endl;
    return HasFileSystemIF::FILE_DOES_NOT_EXIST;
  }
  std::ifstream file(update.file, std::ifstream::binary);
  uint16_t dataLength = 0;
  ccsds::SequenceFlags seqFlags;
  while (update.bytesWritten < update.fullFileSize) {
    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 = handlePacketTransmissionNoReply(packet, 5000);
    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 */
    // TaskFactory::delayTask(1);
  }
  return result;
}

uint32_t PlocSupvUartManager::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 PlocSupvUartManager::selectMemory() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::MPSoCBootSelect packet(spParams);
  result = packet.buildPacket(update.memoryId);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handlePacketTransmissionNoReply(packet, 2000);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvUartManager::prepareUpdate() {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::NoPayloadPacket packet(spParams, Apid::BOOT_MAN,
                               static_cast<uint8_t>(tc::BootManId::PREPARE_UPDATE));
  result = packet.buildPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  result = handlePacketTransmissionNoReply(packet, PREPARE_UPDATE_EXECUTION_REPORT);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvUartManager::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 = handlePacketTransmissionNoReply(eraseMemory, supv::timeout::ERASE_MEMORY);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply(
    supv::TcBase& packet, uint32_t timeoutExecutionReport) {
  ReturnValue_t result = returnvalue::OK;
  result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen());
  if (result != returnvalue::OK) {
    return result;
  }
  Countdown countdown(timeoutExecutionReport);
  dur_millis_t currentDelay = 5;
  bool ackReceived = false;
  bool packetWasHandled = false;
  while (true) {
    handleUartReception();
    if (not decodedQueue.empty()) {
      size_t packetLen = 0;
      decodedQueue.retrieve(&packetLen);
      decodedRingBuf.readData(decodedBuf.data(), packetLen, true);
      tmReader.setReadOnlyData(decodedBuf.data(), packetLen);
      result = checkReceivedTm();
      if (result != returnvalue::OK) {
        continue;
      }
      if (tmReader.getModuleApid() == Apid::TMTC_MAN) {
        int retval = 0;
        if (not ackReceived) {
          retval = handleAckReception(packet, packetLen);
          if (retval == 1) {
            ackReceived = true;
            packetWasHandled = true;
          } else if (retval == -1) {
            return returnvalue::FAILED;
          }
        } else {
          retval = handleExeAckReception(packet, packetLen);
          if (retval == 1) {
            break;
          } else if (retval == -1) {
            return returnvalue::FAILED;
          }
        }
      }
      if (not packetWasHandled) {
        pushIpcData(decodedBuf.data(), packetLen);
        decodedRingBuf.deleteData(packetLen);
      }
    } else {
      TaskFactory::delayTask(currentDelay);
      if (currentDelay < 80) {
        currentDelay *= 2;
      }
    }
    if (countdown.hasTimedOut()) {
      return result::NO_REPLY_TIMEOUT;
    }
  }
  return returnvalue::OK;
}

int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, size_t packetLen) {
  uint8_t serviceId = tmReader.getServiceId();
  if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::ACK) or
      serviceId == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
    AcknowledgmentReport ackReport(tmReader);
    ReturnValue_t result = ackReport.parse(false);
    if (result != returnvalue::OK) {
      triggerEvent(ACK_RECEPTION_FAILURE);
      return -1;
    }
    if (ackReport.getRefModuleApid() == tc.getModuleApid() and
        ackReport.getRefServiceId() == tc.getServiceId()) {
      if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::ACK)) {
        return 1;
      } else if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
        ackReport.printStatusInformationAck();
        triggerEvent(
            SUPV_ACK_FAILURE_REPORT,
            buildApidServiceParam1(ackReport.getRefModuleApid(), ackReport.getRefServiceId()),
            ackReport.getStatusCode());
        return -1;
      }
      // Should never happen
      return -1;
    } else {
      pushIpcData(decodedBuf.data(), packetLen);
      decodedRingBuf.deleteData(packetLen);
    }
  }
  return 0;
}

int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, size_t packetLen) {
  uint8_t serviceId = tmReader.getServiceId();
  if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK) or
      serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
    ExecutionReport exeReport(tmReader);
    ReturnValue_t result = exeReport.parse(false);
    if (result != returnvalue::OK) {
      triggerEvent(EXE_RECEPTION_FAILURE);
      return -1;
    }
    if (exeReport.getRefModuleApid() == tc.getModuleApid() and
        exeReport.getRefServiceId() == tc.getServiceId()) {
      if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
        return 1;
      } else if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
        exeReport.printStatusInformationExe();
        triggerEvent(
            SUPV_EXE_FAILURE_REPORT,
            buildApidServiceParam1(exeReport.getRefModuleApid(), exeReport.getRefServiceId()),
            exeReport.getStatusCode());
        return -1;
      }
      // Should never happen
      return -1;
    } else {
      pushIpcData(decodedBuf.data(), packetLen);
      decodedRingBuf.deleteData(packetLen);
    }
  }
  return 0;
}

ReturnValue_t PlocSupvUartManager::checkReceivedTm() {
  ReturnValue_t result = tmReader.checkSize();
  if (result != returnvalue::OK) {
    triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid);
    return result;
  }
  if (tmReader.checkCrc() != returnvalue::OK) {
    triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid);
    return result;
  }
  return result;
}

ReturnValue_t PlocSupvUartManager::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 PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) {
  ReturnValue_t result = returnvalue::OK;
  resetSpParams();
  supv::CheckMemory tcPacket(spParams);
  result = tcPacket.buildPacket(update.memoryId, update.startAddress, update.fullFileSize);
  if (result != returnvalue::OK) {
    return result;
  }
  result = encodeAndSendPacket(tcPacket.getFullPacket(), tcPacket.getFullPacketLen());
  if (result != returnvalue::OK) {
    return result;
  }
  Countdown countdown(timeout::CRC_EXECUTION_TIMEOUT);
  bool ackReceived = false;
  bool checkReplyReceived = false;
  bool packetWasHandled = false;
  bool exeReceived = false;
  while (true) {
    handleUartReception();
    if (not decodedQueue.empty()) {
      size_t packetLen = 0;
      decodedQueue.retrieve(&packetLen);
      decodedRingBuf.readData(decodedBuf.data(), packetLen, true);
      tmReader.setReadOnlyData(decodedBuf.data(), packetLen);
      result = checkReceivedTm();
      if (result != returnvalue::OK) {
        continue;
      }
      packetWasHandled = false;
      if (tmReader.getModuleApid() == Apid::TMTC_MAN) {
        int retval = 0;
        if (not ackReceived) {
          retval = handleAckReception(tcPacket, packetLen);
          if (retval == 1) {
            packetWasHandled = true;
            ackReceived = true;
          } else if (retval == -1) {
            return returnvalue::FAILED;
          }
        } else {
          retval = handleExeAckReception(tcPacket, packetLen);
          if (retval == 1) {
            packetWasHandled = true;
            exeReceived = true;
          } else if (retval == -1) {
            return returnvalue::FAILED;
          }
        }
      } else if (tmReader.getModuleApid() == Apid::MEM_MAN) {
        if (ackReceived) {
          supv::UpdateStatusReport report(tmReader);
          result = report.parse(false);
          if (result != returnvalue::OK) {
            return result;
          }
          packetWasHandled = true;
          checkReplyReceived = true;
          if (update.crcShouldBeChecked) {
            result = report.verifyCrc(update.crc);
            if (result == returnvalue::OK) {
              triggerEvent(SUPV_MEM_CHECK_OK, result);
            } else {
              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)
                  << report.getCrc() << std::dec << std::endl;
              triggerEvent(SUPV_MEM_CHECK_FAIL, result, failStep);
            }
          }
        }
      }
      if (not packetWasHandled) {
        pushIpcData(decodedBuf.data(), packetLen);
        decodedRingBuf.deleteData(packetLen);
      }
    } else {
      TaskFactory::delayTask(20);
    }
    if (ackReceived and exeReceived and checkReplyReceived) {
      break;
    }
    if (countdown.hasTimedOut()) {
      return result::NO_REPLY_TIMEOUT;
    }
  }
  return returnvalue::OK;
}

uint32_t PlocSupvUartManager::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 PlocSupvUartManager::handleEventBufferReception(ploc::SpTmReader& reader) {
  ReturnValue_t result = returnvalue::OK;
  // TODO: Fix
  // #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;
  //    }
  //    // TODO: Fix
  //    //    file.write(reinterpret_cast<const char*>(reader.getPacketData()),
  //    //               reader.getPayloadDataLength());
  //  }
  return result;
}

void PlocSupvUartManager::resetSpParams() { spParams.buf = cmdBuf.data(); }

ReturnValue_t PlocSupvUartManager::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                               size_t sendLen) {
  if (sendData == nullptr or sendLen == 0) {
    return FAILED;
  }
  {
    MutexGuard mg(lock);
    if (state == InternalState::SLEEPING or state == InternalState::DEDICATED_REQUEST) {
      return FAILED;
    }
  }
  return encodeAndSendPacket(sendData, sendLen);
}

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

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

ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() {
  ReturnValue_t result = OK;
  switch (request) {
    case Request::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);
      }
      break;
    }
    case Request::CHECK_MEMORY: {
      executeFullCheckMemoryCommand();
      break;
    }
    case Request::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);
      }
      break;
    }
    case Request::REQUEST_EVENT_BUFFER: {
      sif::error << "Requesting event buffer is not implemented" << std::endl;
      break;
    }
    case Request::DEFAULT: {
      break;
    }
  }
  return false;
}

ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) {
  size_t encodedLen = 0;
  addHdlcFraming(sendData, sendLen, encodedSendBuf.data(), &encodedLen, encodedSendBuf.size());
  if (PRINT_TC) {
    sif::debug << "Sending TC" << std::endl;
    arrayprinter::print(encodedSendBuf.data(), encodedLen);
  }
  size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen);
  if (bytesWritten != encodedLen) {
    sif::warning
        << "PlocSupvUartManager::sendMessage: Sending ping command to solar experiment failed"
        << std::endl;
    return FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupvUartManager::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
  MutexGuard mg(ipcLock);
  if (ipcQueue.empty()) {
    *size = 0;
    return OK;
  }
  ipcQueue.retrieve(size);
  if (*size > ipcBuffer.size()) {
    return FAILED;
  }
  *buffer = ipcBuffer.data();
  ReturnValue_t result = ipcRingBuf.readData(ipcBuffer.data(), *size, true);
  if (result != OK) {
    sif::warning << "PlocSupvHelper::readReceivedMessage: Reading RingBuffer failed" << std::endl;
  }
  return OK;
}

ReturnValue_t PlocSupvUartManager::tryHdlcParsing() {
  size_t bytesRead = 0;
  size_t decodedLen = 0;
  ReturnValue_t result = parseRecRingBufForHdlc(bytesRead, decodedLen);
  if (result == returnvalue::OK) {
    // Packet found, advance read pointer.
    if (state == InternalState::DEDICATED_REQUEST) {
      decodedRingBuf.writeData(decodedBuf.data(), decodedLen);
      decodedQueue.insert(decodedLen);
    } else {
      MutexGuard mg(ipcLock);
      ipcRingBuf.writeData(decodedBuf.data(), decodedLen);
      ipcQueue.insert(decodedLen);
    }
    recRingBuf.deleteData(bytesRead);
  } else if (result != NO_PACKET_FOUND) {
    sif::warning << "PlocSupvUartMan::performOperation: Possible packet loss" << std::endl;
    // Markers found at wrong place
    // which might be a hint for a possibly lost packet.
    // Maybe trigger event?
    recRingBuf.deleteData(bytesRead);
  }
  return result;
}

ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size_t& decodedLen) {
  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;
  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
        int retval = removeHdlcFramingWithCrcCheck(encodedBuf.data() + startIdx, idx + 1 - startIdx,
                                                   decodedBuf.data(), &decodedLen);
        readSize = idx + 1;
        if (retval == -1 or retval == -2) {
          triggerEvent(HDLC_FRAME_REMOVAL_ERROR, retval);
        } else if (retval == 1) {
          triggerEvent(HDLC_CRC_ERROR);
        }
        if (retval != 0) {
          readSize = ++idx;
          return HDLC_ERROR;
        }
        return returnvalue::OK;
      } else {
        readSize = ++idx;
        return POSSIBLE_PACKET_LOSS_CONSECUTIVE_END;
      }
    }
  }
  return NO_PACKET_FOUND;
}

void PlocSupvUartManager::pushIpcData(const uint8_t* data, size_t len) {
  MutexGuard mg(ipcLock);
  ipcRingBuf.writeData(data, len);
  ipcQueue.insert(len);
}

uint32_t PlocSupvUartManager::buildApidServiceParam1(uint8_t apid, uint8_t serviceId) {
  return (apid << 8) | serviceId;
}

void PlocSupvUartManager::performUartShutdown() {
  tcflush(serialPort, TCIOFLUSH);
  // Clear ring buffers
  recRingBuf.clear();
  decodedRingBuf.clear();
  while (not decodedQueue.empty()) {
    decodedQueue.pop();
  }
  {
    MutexGuard mg0(ipcLock);
    ipcRingBuf.clear();
    while (not ipcQueue.empty()) {
      ipcQueue.pop();
    }
  }
  MutexGuard mg1(lock);
  state = InternalState::GO_TO_SLEEP;
}

void PlocSupvUartManager::addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst,
                                         size_t* dlen, size_t maxDest) {
  size_t tlen = 0;
  uint16_t ii;
  uint8_t bt;

  // calc crc16
  uint16_t crc16 = calc_crc16_buff_reflected(src, slen);

  dst[tlen++] = 0x7E;
  for (ii = 0; ii < slen; ii++) {
    bt = *src++;
    hdlc_add_byte(bt, dst, &tlen);
  }

  size_t dummy = 0;
  uint8_t crcRaw[2];
  // hdlc crc16 is in little endian format
  SerializeAdapter::serialize(&crc16, crcRaw, &dummy, maxDest, SerializeIF::Endianness::LITTLE);
  hdlc_add_byte(crcRaw[0], dst, &tlen);
  hdlc_add_byte(crcRaw[1], dst, &tlen);

  dst[tlen++] = 0x7C;
  *dlen = tlen;
}

int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen,
                                                       uint8_t* dst, size_t* dlen) {
  uint16_t tlen = 0;
  uint16_t ii;
  uint8_t bt;

  *dlen = 0;
  if (slen < 4) return -1;
  if ((src[tlen] != 0x7E) && (src[slen - 1] != 0x7C)) return -2;
  src++;
  for (ii = 1; ii < slen - 1; ii++) {
    bt = *src++;

    if (bt == 0x7D) {
      bt = *src++ ^ 0x20;
      ii++;
    }
    dst[tlen++] = bt;
  }
  // calc crc16
  uint16_t calcCrc = calc_crc16_buff_reflected(dst, tlen - 2);
  uint16_t crc = 0;
  size_t dummy;
  SerializeAdapter::deSerialize(&crc, dst + tlen - 2, &dummy, SerializeIF::Endianness::LITTLE);
  if (calcCrc != crc) {
    return 1;
  }
  // if(calc_crc16_buff_reflected(dst, tlen) != 0) {
  //  return 1;
  // }
  *dlen = tlen - 2;
  return 0;
}

bool PlocSupvUartManager::longerRequestActive() const {
  MutexGuard mg(lock);
  return state == InternalState::DEDICATED_REQUEST;
}

void PlocSupvUartManager::updateVtime(uint8_t vtime) {
  tcgetattr(serialPort, &tty);
  tty.c_cc[VTIME] = vtime;
  tcsetattr(serialPort, TCSANOW, &tty);
}