#include "PlocMPSoCHelper.h"

#include <filesystem>
#include <fstream>

#ifdef XIPHOS_Q7S
#include "bsp_q7s/memory/FileSystemHelper.h"
#endif

#include "mission/utility/Timestamp.h"

PlocMPSoCHelper::PlocMPSoCHelper(object_id_t objectId) : SystemObject(objectId) {}

PlocMPSoCHelper::~PlocMPSoCHelper() {}

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

ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) {
  ReturnValue_t result = RETURN_OK;
  semaphore.acquire();
  while (true) {
    switch (internalState) {
      case InternalState::IDLE: {
        semaphore.acquire();
        break;
      }
      case InternalState::FLASH_WRITE: {
        result = performFlashWrite();
        if (result == RETURN_OK) {
          triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL);
        } else {
          triggerEvent(MPSOC_FLASH_WRITE_FAILED);
        }
        internalState = InternalState::IDLE;
        break;
      }
      default:
        sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl;
        break;
    }
  }
}

ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) {
  uartComIF = dynamic_cast<UartComIF*>(communicationInterface_);
  if (uartComIF == nullptr) {
    sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl;
    return RETURN_FAILED;
  }
  return RETURN_OK;
}

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

void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) {
  sequenceCount = sequenceCount_;
}

ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string file) {
#ifdef XIPHOS_Q7S
  ReturnValue_t result = FilesystemHelper::checkPath(file);
  if (result != RETURN_OK) {
    return result;
  }
  result = FilesystemHelper::fileExists(file);
  if (result != RETURN_OK) {
    return result;
  }
#endif
  flashWrite.file = file;
  internalState = InternalState::FLASH_WRITE;
  semaphore.release();
  terminate = false;
  return RETURN_OK;
}

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

ReturnValue_t PlocMPSoCHelper::performFlashWrite() {
  ReturnValue_t result = RETURN_OK;
  result = flashfopen();
  if (result != RETURN_OK) {
	  return result;
  }
  uint8_t tempData[mpsoc::MAX_DATA_SIZE];
  std::ifstream file(flashWrite.file, std::ifstream::binary);
  // 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;
  while (remainingSize > 0) {
    if (terminate) {
      return RETURN_OK;
    }
    if (remainingSize > mpsoc::MAX_DATA_SIZE) {
      dataLength = mpsoc::MAX_DATA_SIZE;
    } else {
      dataLength = remainingSize;
    }
    file.read(reinterpret_cast<char*>(tempData), dataLength);
    (*sequenceCount)++;
    mpsoc::TcFlashWrite tc(*sequenceCount);
    tc.createPacket(tempData, dataLength);
    result = sendCommand(&tc);
    if (result != RETURN_OK) {
      return result;
    }
    result = handleAck();
    if (result != RETURN_OK) {
      return result;
    }
    result = handleExe();
    if (result != RETURN_OK) {
      return result;
    }
  }
  result = flashfclose();
  if (result != RETURN_OK) {
	  return result;
  }
  return result;
}

ReturnValue_t PlocMPSoCHelper::flashfopen() {
  ReturnValue_t result = RETURN_OK;
  (*sequenceCount)++;
  mpsoc::FlashFopen flashFopen(*sequenceCount);
  result = flashFopen.createPacket(flashWrite.file, mpsoc::FlashFopen::APPEND);
  if (result != RETURN_OK) {
    return result;
  }
  result = sendCommand(&flashFopen);
  if (result != RETURN_OK) {
    return result;
  }
  return RETURN_OK;
}

ReturnValue_t PlocMPSoCHelper::flashfclose() {
	ReturnValue_t result = RETURN_OK;
	(*sequenceCount)++;
	mpsoc::FlashFclose flashFclose(*sequenceCount);
	result = flashFclose.createPacket(flashWrite.file);
	if (result != RETURN_OK) {
		return result;
	}
	result = sendCommand(&flashFclose);
	if (result != RETURN_OK) {
		return result;
	}
	return RETURN_OK;
}

ReturnValue_t PlocMPSoCHelper::sendCommand(mpsoc::TcBase* tc) {
  ReturnValue_t result = RETURN_OK;
  result = uartComIF->sendMessage(comCookie, tc->getWholeData(), tc->getFullSize());
  if (result != RETURN_OK) {
    sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl;
    triggerEvent(SENDING_COMMAND_FAILED, result, static_cast<uint32_t>(internalState));
    return result;
  }
  return result;
}

ReturnValue_t PlocMPSoCHelper::handleAck() {
  ReturnValue_t result = RETURN_OK;
  mpsoc::TmPacket tmPacket;
  result = handleTmReception(&tmPacket, mpsoc::SIZE_ACK_REPORT);
  if (result != RETURN_OK) {
    return result;
  }
  uint16_t apid = tmPacket.getAPID();
  if (apid != mpsoc::apid::ACK_SUCCESS) {
    handleAckApidFailure(apid);
    return RETURN_FAILED;
  }
  return RETURN_OK;
}

void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) {
  if (apid == mpsoc::apid::ACK_FAILURE) {
    triggerEvent(ACK_FAILURE_REPORT, static_cast<uint32_t>(internalState));
    sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Received acknowledgement failure "
                 << "report" << std::endl;
  } else {
    triggerEvent(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 PlocMPSoCHelper::handleExe() {
  ReturnValue_t result = RETURN_OK;
  mpsoc::TmPacket tmPacket;
  result = handleTmReception(&tmPacket, mpsoc::SIZE_EXE_REPORT);
  if (result != RETURN_OK) {
    return result;
  }
  uint16_t apid = tmPacket.getAPID();
  if (apid != mpsoc::apid::EXE_SUCCESS) {
    handleExeApidFailure(apid);
    return RETURN_FAILED;
  }
  return RETURN_OK;
}

void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) {
  if (apid == mpsoc::apid::EXE_FAILURE) {
    triggerEvent(EXE_FAILURE_REPORT, static_cast<uint32_t>(internalState));
    sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Received execution failure "
                 << "report" << std::endl;
  } else {
    triggerEvent(EXE_INVALID_APID, apid, static_cast<uint32_t>(internalState));
    sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected execution report "
                 << "but received space packet with apid " << std::hex << apid << std::endl;
  }
}

ReturnValue_t PlocMPSoCHelper::handleTmReception(mpsoc::TmPacket* tmPacket, size_t remainingBytes) {
  ReturnValue_t result = RETURN_OK;
  size_t readBytes = 0;
  for (int retries = 0; retries < RETRIES; retries++) {
    result = receive(tmPacket->getWholeData(), &readBytes, remainingBytes);
    if (result != RETURN_OK) {
      return result;
    }
    remainingBytes = remainingBytes - readBytes;
    if (remainingBytes == 0) {
      break;
    }
  }
  if (remainingBytes != 0) {
    sif::warning << "PlocMPSoCHelper::handleTmReception: Failed to receive reply" << std::endl;
    triggerEvent(MISSING_EXE, remainingBytes, static_cast<uint32_t>(internalState));
    return RETURN_FAILED;
  }
  result = tmPacket->checkCrc();
  if (result != RETURN_OK) {
    sif::warning << "PlocMPSoCHelper::handleTmReception: CRC check failed" << std::endl;
    return result;
  }
  uint16_t recvSeqCnt = tmPacket->getPacketSequenceCount();
  if (recvSeqCnt != *sequenceCount) {
    triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt);
    *sequenceCount = recvSeqCnt;
    return result;
  }
  return result;
}

ReturnValue_t PlocMPSoCHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) {
  ReturnValue_t result = RETURN_OK;
  result = uartComIF->requestReceiveMessage(comCookie, requestBytes);
  if (result != RETURN_OK) {
    sif::warning << "PlocMPSoCHelper::receive: Failed to request reply" << std::endl;
    triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result,
                 static_cast<uint32_t>(static_cast<uint32_t>(internalState)));
    return RETURN_FAILED;
  }
  result = uartComIF->readReceivedMessage(comCookie, &data, readBytes);
  if (result != RETURN_OK) {
    sif::warning << "PlocMPSoCHelper::receive: Failed to read received message" << std::endl;
    triggerEvent(MPSOC_HELPER_READING_REPLY_FAILED, result, static_cast<uint32_t>(internalState));
    return RETURN_FAILED;
  }
  return result;
}