#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/TaskFactory.h>
#include <linux/payload/PlocMpsocSpecialComHelper.h>
#include <unistd.h>

#include <filesystem>
#include <fstream>

#include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
#include "linux/payload/MpsocCommunication.h"
#include "linux/payload/plocMpsocHelpers.h"

#ifdef XIPHOS_Q7S
#include "bsp_q7s/fs/FilesystemHelper.h"
#endif

using namespace ploc;

PlocMpsocSpecialComHelper::PlocMpsocSpecialComHelper(object_id_t objectId,
                                                     MpsocCommunication& comInterface)
    : SystemObject(objectId), comInterface(comInterface) {
  spParams.buf = commandBuffer;
  spParams.maxSize = sizeof(commandBuffer);
}

PlocMpsocSpecialComHelper::~PlocMpsocSpecialComHelper() {}

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

ReturnValue_t PlocMpsocSpecialComHelper::performOperation(uint8_t operationCode) {
  ReturnValue_t result = returnvalue::OK;
  semaphore.acquire();
  while (true) {
#if OBSW_THREAD_TRACING == 1
    trace::threadTrace(opCounter, "PLOC MPSOC Helper");
#endif
    switch (internalState) {
      case InternalState::IDLE: {
        semaphore.acquire();
        break;
      }
      case InternalState::FLASH_WRITE: {
        result = performFlashWrite();
        if (result == returnvalue::OK) {
          triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL, txSequenceCount.get());
        } else {
          triggerEvent(MPSOC_FLASH_WRITE_FAILED, txSequenceCount.get());
        }
        internalState = InternalState::IDLE;
        break;
      }
      case InternalState::FLASH_READ: {
        result = performFlashRead();
        if (result == returnvalue::OK) {
          triggerEvent(MPSOC_FLASH_READ_SUCCESSFUL, txSequenceCount.get());
        } else {
          sif::printWarning("PLOC MPSoC Helper: Flash read failed with code %04x\n", result);
          triggerEvent(MPSOC_FLASH_READ_FAILED, txSequenceCount.get(), result);
        }
        internalState = InternalState::IDLE;
        break;
      }
      default:
        sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl;
        break;
    }
  }
}

void PlocMpsocSpecialComHelper::setCommandSequenceCount(uint16_t sequenceCount_) {
  txSequenceCount.set(sequenceCount_);
}

uint16_t PlocMpsocSpecialComHelper::getCommandSequenceCount() const {
  return txSequenceCount.get();
}

ReturnValue_t PlocMpsocSpecialComHelper::startFlashWrite(std::string obcFile,
                                                         std::string mpsocFile) {
  if (internalState != InternalState::IDLE) {
    return returnvalue::FAILED;
  }
  ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile));
  if (result != returnvalue::OK) {
    return result;
  }
  internalState = InternalState::FLASH_WRITE;
  return semaphore.release();
}

ReturnValue_t PlocMpsocSpecialComHelper::startFlashRead(std::string obcFile, std::string mpsocFile,
                                                        size_t readFileSize) {
  if (internalState != InternalState::IDLE) {
    return returnvalue::FAILED;
  }
  ReturnValue_t result = startFlashReadOrWriteBase(std::move(obcFile), std::move(mpsocFile));
  if (result != returnvalue::OK) {
    return result;
  }
  flashReadAndWrite.totalReadSize = readFileSize;
  internalState = InternalState::FLASH_READ;
  return semaphore.release();
}

void PlocMpsocSpecialComHelper::resetHelper() {
  spParams.buf = commandBuffer;
  terminate = false;
  auto& helper = comInterface.getComHelper();
  helper.flushUartRxBuffer();
}

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

ReturnValue_t PlocMpsocSpecialComHelper::performFlashWrite() {
  ReturnValue_t result = returnvalue::OK;
  std::ifstream file(flashReadAndWrite.obcFile, std::ifstream::binary);
  if (file.bad()) {
    return returnvalue::FAILED;
  }
  result = flashfopen(mpsoc::FileAccessModes::WRITE | mpsoc::FileAccessModes::OPEN_ALWAYS);
  if (result != returnvalue::OK) {
    return result;
  }
  // Set position of next character to end of file input stream
  file.seekg(0, file.end);
  // tellg returns position of character in input stream
  size_t remainingSize = file.tellg();
  size_t dataLength = 0;
  size_t bytesRead = 0;
  while (remainingSize > 0) {
    if (terminate) {
      return returnvalue::OK;
    }
    // The minus 4 is necessary for unknown reasons. Maybe some bug in the ILH software?
    if (remainingSize > mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4) {
      dataLength = mpsoc::MAX_FLASH_WRITE_DATA_SIZE - 4;
    } else {
      dataLength = remainingSize;
    }
    if (file.bad() or not file.is_open()) {
      return FILE_WRITE_ERROR;
    }
    file.seekg(bytesRead, file.beg);
    file.read(reinterpret_cast<char*>(fileBuf.data()), dataLength);
    bytesRead += dataLength;
    remainingSize -= dataLength;
    mpsoc::TcFlashWrite tc(spParams, txSequenceCount);
    result = tc.setPayload(fileBuf.data(), dataLength);
    if (result != returnvalue::OK) {
      return result;
    }
    result = tc.finishPacket();
    if (result != returnvalue::OK) {
      return result;
    }
    txSequenceCount.increment();
    result = handlePacketTransmissionNoReply(tc);
    if (result != returnvalue::OK) {
      return result;
    }
  }
  result = flashfclose();
  if (result != returnvalue::OK) {
    return result;
  }
  return result;
}

ReturnValue_t PlocMpsocSpecialComHelper::performFlashRead() {
  std::error_code e;
  if (std::filesystem::exists(flashReadAndWrite.obcFile)) {
    // Truncate the file first.
    std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::binary | std::ios::trunc);
  }
  std::ofstream ofile(flashReadAndWrite.obcFile, std::ios::binary | std::ios::app);
  if (ofile.bad() or not ofile.is_open()) {
    return returnvalue::FAILED;
  }
  ReturnValue_t result = flashfopen(mpsoc::FileAccessModes::READ);
  if (result != returnvalue::OK) {
    std::filesystem::remove(flashReadAndWrite.obcFile, e);
    return result;
  }
  size_t readSoFar = 0;
  size_t nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE;
  while (readSoFar < flashReadAndWrite.totalReadSize) {
    if (terminate) {
      std::filesystem::remove(flashReadAndWrite.obcFile, e);
      return returnvalue::OK;
    }
    nextReadSize = mpsoc::MAX_FLASH_READ_DATA_SIZE;
    if (flashReadAndWrite.totalReadSize - readSoFar < mpsoc::MAX_FLASH_READ_DATA_SIZE) {
      nextReadSize = flashReadAndWrite.totalReadSize - readSoFar;
    }
    if (ofile.bad() or not ofile.is_open()) {
      std::filesystem::remove(flashReadAndWrite.obcFile, e);
      return FILE_READ_ERROR;
    }
    mpsoc::TcFlashRead flashReadRequest(spParams, txSequenceCount);
    result = flashReadRequest.setPayload(nextReadSize);
    if (result != returnvalue::OK) {
      std::filesystem::remove(flashReadAndWrite.obcFile, e);
      return result;
    }
    result = flashReadRequest.finishPacket();
    if (result != returnvalue::OK) {
      std::filesystem::remove(flashReadAndWrite.obcFile, e);
      return result;
    }
    txSequenceCount.increment();
    result = handlePacketTransmissionFlashRead(flashReadRequest, ofile, nextReadSize);
    if (result != returnvalue::OK) {
      std::filesystem::remove(flashReadAndWrite.obcFile, e);
      return result;
    }
    readSoFar += nextReadSize;
  }
  result = flashfclose();
  if (result != returnvalue::OK) {
    return result;
  }
  return result;
}

ReturnValue_t PlocMpsocSpecialComHelper::flashfopen(uint8_t mode) {
  spParams.buf = commandBuffer;
  mpsoc::TcFlashFopen flashFopen(spParams, txSequenceCount);
  ReturnValue_t result = flashFopen.setPayload(flashReadAndWrite.mpsocFile, mode);
  if (result != returnvalue::OK) {
    return result;
  }
  result = flashFopen.finishPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  txSequenceCount.increment();
  result = handlePacketTransmissionNoReply(flashFopen);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocMpsocSpecialComHelper::flashfclose() {
  spParams.buf = commandBuffer;
  mpsoc::TcFlashFclose flashFclose(spParams, txSequenceCount);
  ReturnValue_t result = flashFclose.finishPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  txSequenceCount.increment();
  result = handlePacketTransmissionNoReply(flashFclose);
  if (result != returnvalue::OK) {
    return result;
  }
  return result;
}

ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionFlashRead(mpsoc::TcFlashRead& tc,
                                                                           std::ofstream& ofile,
                                                                           size_t expectedReadLen) {
  ReturnValue_t result = sendCommand(tc);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleAck();
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleTmReception();
  if (result != returnvalue::OK) {
    return result;
  }
  auto& spReader = comInterface.getSpReader();

  // We have the nominal case where the flash read report appears first, or the case where we
  // get an EXE failure immediately.
  if (spReader.getApid() == mpsoc::apid::TM_FLASH_READ_REPORT) {
    result = handleFlashReadReply(ofile, expectedReadLen);
    if (result != returnvalue::OK) {
      return result;
    }
    return handleExe();
  } else if (spReader.getApid() == mpsoc::apid::EXE_FAILURE) {
    handleExeFailure(spReader);
  } else {
    triggerEvent(MPSOC_EXE_INVALID_APID, spReader.getApid(), static_cast<uint32_t>(internalState));
    sif::warning << "PLOC MPSoC: Expected execution report "
                 << "but received space packet with apid " << std::hex << spReader.getApid()
                 << std::endl;
  }
  return returnvalue::FAILED;
}

ReturnValue_t PlocMpsocSpecialComHelper::handlePacketTransmissionNoReply(ploc::SpTcBase& tc) {
  ReturnValue_t result = sendCommand(tc);
  if (result != returnvalue::OK) {
    return result;
  }
  result = handleAck();
  if (result != returnvalue::OK) {
    return result;
  }
  return handleExe();
}

ReturnValue_t PlocMpsocSpecialComHelper::sendCommand(ploc::SpTcBase& tc) {
  ReturnValue_t result = comInterface.send(tc.getFullPacket(), tc.getFullPacketLen());
  mpsoc::printTxPacket(tc);
  if (result != returnvalue::OK) {
    sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl;
    triggerEvent(MPSOC_SENDING_COMMAND_FAILED, result, static_cast<uint32_t>(internalState));
    return result;
  }
  return result;
}

ReturnValue_t PlocMpsocSpecialComHelper::handleAck() {
  ReturnValue_t result = returnvalue::OK;
  result = handleTmReception();
  if (result != returnvalue::OK) {
    return result;
  }
  result = checkReceivedTm();
  if (result != returnvalue::OK) {
    return result;
  }
  const auto& spReader = comInterface.getSpReader();

  uint16_t apid = spReader.getApid();
  if (apid != mpsoc::apid::ACK_SUCCESS) {
    handleAckApidFailure(spReader);
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

void PlocMpsocSpecialComHelper::handleAckApidFailure(const SpacePacketReader& reader) {
  uint16_t apid = reader.getApid();
  if (apid == mpsoc::apid::ACK_FAILURE) {
    uint16_t status = mpsoc::getStatusFromRawData(reader.getFullData());
    sif::warning << "PLOC MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl;
    triggerEvent(MPSOC_ACK_FAILURE_REPORT, static_cast<uint32_t>(internalState), status);
  } else {
    triggerEvent(MPSOC_ACK_INVALID_APID, apid, static_cast<uint32_t>(internalState));
    sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected acknowledgement report "
                 << "but received space packet with apid " << std::hex << apid << std::endl;
  }
}

ReturnValue_t PlocMpsocSpecialComHelper::handleExe() {
  ReturnValue_t result = returnvalue::OK;

  result = handleTmReception();
  if (result != returnvalue::OK) {
    return result;
  }
  result = checkReceivedTm();
  if (result != returnvalue::OK) {
    return result;
  }
  const auto& spReader = comInterface.getSpReader();
  uint16_t apid = spReader.getApid();
  if (apid == mpsoc::apid::EXE_FAILURE) {
    handleExeFailure(spReader);
    return returnvalue::FAILED;
  } else if (apid != mpsoc::apid::EXE_SUCCESS) {
    triggerEvent(MPSOC_EXE_INVALID_APID, apid, static_cast<uint32_t>(internalState));
    sif::warning << "PLOC MPSoC: Expected execution report "
                 << "but received space packet with apid " << std::hex << apid << std::endl;
  }
  return returnvalue::OK;
}

void PlocMpsocSpecialComHelper::handleExeFailure(const SpacePacketReader& spReader) {
  uint16_t status = mpsoc::getStatusFromRawData(spReader.getFullData());
  sif::warning << "PLOC MPSoC EXE Failure: " << mpsoc::getStatusString(status) << std::endl;
  triggerEvent(MPSOC_EXE_FAILURE_REPORT, static_cast<uint32_t>(internalState));
}

ReturnValue_t PlocMpsocSpecialComHelper::handleTmReception() {
  ReturnValue_t result = returnvalue::OK;
  tmCountdown.resetTimer();
  uint32_t usleepDelay = 5;
  while (true) {
    if (tmCountdown.hasTimedOut()) {
      triggerEvent(MPSOC_READ_TIMEOUT, tmCountdown.getTimeoutMs());
      return returnvalue::FAILED;
    }
    result = tryReceiveNextReply();
    if (result == MpsocCommunication::PACKET_RECEIVED) {
      // Need to convert this, we are faking a synchronous API here.
      result = returnvalue::OK;
      break;
    }
    if (result != returnvalue::OK) {
      if (result == MpsocCommunication::FAULTY_PACKET_SIZE) {
        sif::printWarning("PLOC MPSoC Helper: retrieving next reply failed: faulty packet size\n");
      } else if (result == MpsocCommunication::CRC_CHECK_FAILED) {
        sif::printWarning("PLOC MPSoC Helper: retrieving next reply failed: CRC check failed\n");
      }
      sif::printWarning("PLOC MPSoC Helper: retrieving next reply failed with code %d\n", result);
      return result;
    }
    usleep(usleepDelay);
    if (usleepDelay < 200000) {
      usleepDelay *= 4;
    }
  }
  return result;
}

ReturnValue_t PlocMpsocSpecialComHelper::handleFlashReadReply(std::ofstream& ofile,
                                                              size_t expectedReadLen) {
  ReturnValue_t result = checkReceivedTm();
  if (result != returnvalue::OK) {
    return result;
  }
  auto& spReader = comInterface.getSpReader();
  uint16_t apid = spReader.getApid();
  if (apid != mpsoc::apid::TM_FLASH_READ_REPORT) {
    triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_APID_ERROR);
    sif::warning << "PLOC MPSoC Flash Read: Unexpected APID" << std::endl;
    return result;
  }
  const uint8_t* packetData = spReader.getPacketData();
  size_t deserDummy = spReader.getPacketDataLen() - mpsoc::CRC_SIZE;
  uint32_t receivedReadLen = 0;
  // I think this is buggy, weird stuff in the short name field.
  //  std::string receivedShortName = std::string(reinterpret_cast<const char*>(packetData), 12);
  //  if (receivedShortName != flashReadAndWrite.mpsocFile.substr(0, 11)) {
  //    sif::warning << "PLOC MPSoC Flash Read: Missmatch between request file name and "
  //                    "received file name"
  //                 << std::endl;
  //    triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_FILENAME_ERROR);
  //    return returnvalue::FAILED;
  //  }
  packetData += 12;
  result = SerializeAdapter::deSerialize(&receivedReadLen, &packetData, &deserDummy,
                                         SerializeIF::Endianness::NETWORK);
  if (result != returnvalue::OK) {
    return result;
  }
  if (receivedReadLen != expectedReadLen) {
    sif::warning << "PLOC MPSoC Flash Read: Missmatch between request read length and "
                    "received read length"
                 << std::endl;
    triggerEvent(MPSOC_FLASH_READ_PACKET_ERROR, FlashReadErrorType::FLASH_READ_READLEN_ERROR);
    return returnvalue::FAILED;
  }
  ofile.write(reinterpret_cast<const char*>(packetData), receivedReadLen);
  return returnvalue::OK;
}

ReturnValue_t PlocMpsocSpecialComHelper::fileCheck(std::string obcFile) {
#ifdef XIPHOS_Q7S
  ReturnValue_t result = FilesystemHelper::checkPath(obcFile);
  if (result != returnvalue::OK) {
    return result;
  }
#elif defined(TE0720_1CFA)
  if (not std::filesystem::exists(obcFile)) {
    sif::warning << "PlocMPSoCHelper::startFlashWrite: File " << obcFile << "does not exist"
                 << std::endl;
    return returnvalue::FAILED;
  }
#endif
  return returnvalue::OK;
}

ReturnValue_t PlocMpsocSpecialComHelper::startFlashReadOrWriteBase(std::string obcFile,
                                                                   std::string mpsocFile) {
  ReturnValue_t result = fileCheck(obcFile);
  if (result != returnvalue::OK) {
    return result;
  }

  flashReadAndWrite.obcFile = std::move(obcFile);
  flashReadAndWrite.mpsocFile = std::move(mpsocFile);
  resetHelper();
  return returnvalue::OK;
}

ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm() {
  const auto& spReader = comInterface.getSpReader();
  ReturnValue_t result = spReader.checkSize();
  if (result != returnvalue::OK) {
    sif::error << "PLOC MPSoC: Size check on received TM failed" << std::endl;
    triggerEvent(MPSOC_TM_SIZE_ERROR);
    return result;
  }
  rxSequenceCount = spReader.getSequenceCount();
  mpsoc::printRxPacket(spReader);
  return returnvalue::OK;
}

ReturnValue_t PlocMpsocSpecialComHelper::tryReceiveNextReply() {
  ReturnValue_t result = returnvalue::OK;
  result = comInterface.readSerialInterface();
  if (result != returnvalue::OK) {
    triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result,
                 static_cast<uint32_t>(static_cast<uint32_t>(internalState)));
    return returnvalue::FAILED;
  }
  return comInterface.parseAndRetrieveNextPacket();
}