#include "PlocMemoryDumper.h"

#include <fsfw/src/fsfw/serialize/SerializeAdapter.h>

#include <filesystem>
#include <fstream>
#include <string>

#include "fsfw/ipc/QueueFactory.h"

PlocMemoryDumper::PlocMemoryDumper(object_id_t objectId)
    : SystemObject(objectId), commandActionHelper(this), actionHelper(this, nullptr) {
  auto mqArgs = MqArgs(this->getObjectId());
  commandQueue = QueueFactory::instance()->createMessageQueue(
      QUEUE_SIZE, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

PlocMemoryDumper::~PlocMemoryDumper() {}

ReturnValue_t PlocMemoryDumper::initialize() {
  ReturnValue_t result = SystemObject::initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  result = commandActionHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  result = actionHelper.initialize(commandQueue);
  if (result != returnvalue::OK) {
    return result;
  }

  return returnvalue::OK;
}

ReturnValue_t PlocMemoryDumper::performOperation(uint8_t operationCode) {
  readCommandQueue();
  doStateMachine();
  return returnvalue::OK;
}

ReturnValue_t PlocMemoryDumper::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                              const uint8_t* data, size_t size) {
  if (state != State::IDLE) {
    return IS_BUSY;
  }

  switch (actionId) {
    case DUMP_MRAM: {
      size_t deserializeSize = sizeof(mram.startAddress) + sizeof(mram.endAddress);
      SerializeAdapter::deSerialize(&mram.startAddress, &data, &deserializeSize,
                                    SerializeIF::Endianness::BIG);
      SerializeAdapter::deSerialize(&mram.endAddress, &data, &deserializeSize,
                                    SerializeIF::Endianness::BIG);
      if (mram.endAddress > MAX_MRAM_ADDRESS) {
        return MRAM_ADDRESS_TOO_HIGH;
      }
      if (mram.endAddress <= mram.startAddress) {
        return MRAM_INVALID_ADDRESS_COMBINATION;
      }
      state = State::COMMAND_FIRST_MRAM_DUMP;
      break;
    }
    default: {
      sif::warning << "PlocMemoryDumper::executeAction: Received command with invalid action id"
                   << std::endl;
      return INVALID_ACTION_ID;
    }
  }

  return EXECUTION_FINISHED;
}

MessageQueueId_t PlocMemoryDumper::getCommandQueue() const { return commandQueue->getId(); }

MessageQueueIF* PlocMemoryDumper::getCommandQueuePtr() { return commandQueue; }

void PlocMemoryDumper::readCommandQueue() {
  CommandMessage message;
  ReturnValue_t result = returnvalue::OK;

  for (result = commandQueue->receiveMessage(&message); result == returnvalue::OK;
       result = commandQueue->receiveMessage(&message)) {
    if (result != returnvalue::OK) {
      continue;
    }
    result = actionHelper.handleActionMessage(&message);
    if (result == returnvalue::OK) {
      continue;
    }

    result = commandActionHelper.handleReply(&message);
    if (result == returnvalue::OK) {
      continue;
    }

    sif::debug << "PlocMemoryDumper::readCommandQueue: Received message with invalid format"
               << std::endl;
  }
}

void PlocMemoryDumper::doStateMachine() {
  switch (state) {
    case State::IDLE:
      break;
    case State::COMMAND_FIRST_MRAM_DUMP:
      commandNextMramDump(supv::FIRST_MRAM_DUMP);
      break;
    case State::COMMAND_CONSECUTIVE_MRAM_DUMP:
      commandNextMramDump(supv::CONSECUTIVE_MRAM_DUMP);
      break;
    case State::EXECUTING_MRAM_DUMP:
      break;
    default:
      sif::debug << "PlocMemoryDumper::doStateMachine: Invalid state" << std::endl;
      break;
  }
}

void PlocMemoryDumper::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) {}

void PlocMemoryDumper::stepFailedReceived(ActionId_t actionId, uint8_t step,
                                          ReturnValue_t returnCode) {
  triggerEvent(MRAM_DUMP_FAILED);
  state = State::IDLE;
}

void PlocMemoryDumper::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) {}

void PlocMemoryDumper::completionSuccessfulReceived(ActionId_t actionId) {
  switch (pendingCommand) {
    case (supv::FIRST_MRAM_DUMP):
    case (supv::CONSECUTIVE_MRAM_DUMP):
      if (mram.endAddress == mram.startAddress) {
        triggerEvent(MRAM_DUMP_FINISHED);
        state = State::IDLE;
      } else {
        state = State::COMMAND_CONSECUTIVE_MRAM_DUMP;
      }
      break;
    default:
      sif::debug << "PlocMemoryDumper::completionSuccessfulReceived: Invalid pending command"
                 << std::endl;
      state = State::IDLE;
      break;
  }
}

void PlocMemoryDumper::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) {
  switch (pendingCommand) {
    case (supv::FIRST_MRAM_DUMP):
    case (supv::CONSECUTIVE_MRAM_DUMP):
      triggerEvent(MRAM_DUMP_FAILED, mram.lastStartAddress);
      pendingCommand = NONE;
      break;
    default:
      break;
  }
  state = State::IDLE;
}

void PlocMemoryDumper::commandNextMramDump(ActionId_t dumpCommand) {
  ReturnValue_t result = returnvalue::OK;

  uint32_t tempStartAddress = 0;
  uint32_t tempEndAddress = 0;

  if (mram.endAddress - mram.startAddress > MAX_MRAM_DUMP_SIZE) {
    tempStartAddress = mram.startAddress;
    tempEndAddress = mram.startAddress + MAX_MRAM_DUMP_SIZE;
    mram.startAddress += MAX_MRAM_DUMP_SIZE;
  } else {
    tempStartAddress = mram.startAddress;
    tempEndAddress = mram.endAddress;
    mram.startAddress = mram.endAddress;
  }
  mram.lastStartAddress = tempStartAddress;

  MemoryParams params(tempStartAddress, tempEndAddress);

  result =
      commandActionHelper.commandAction(objects::PLOC_SUPERVISOR_HANDLER, dumpCommand, &params);
  if (result != returnvalue::OK) {
    sif::warning << "PlocMemoryDumper::commandNextMramDump: Failed to send mram dump command "
                 << "with start address " << tempStartAddress << " and end address "
                 << tempEndAddress << std::endl;
    triggerEvent(SEND_MRAM_DUMP_FAILED, result, tempStartAddress);
    state = State::IDLE;
    pendingCommand = NONE;
    return;
  }
  state = State::EXECUTING_MRAM_DUMP;
  pendingCommand = dumpCommand;
  return;
}