#include "PlocMPSoCHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>

#include "OBSWConfig.h"

PlocMPSoCHandler::PlocMPSoCHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie) {
  if (comCookie == NULL) {
    sif::error << "PlocMPSoCHandler: Invalid com cookie" << std::endl;
  }
}

PlocMPSoCHandler::~PlocMPSoCHandler() {}

void PlocMPSoCHandler::doStartUp() {
  if (mode == _MODE_START_UP) {
    setMode(MODE_ON);
  }
}

void PlocMPSoCHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }

ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  return RETURN_OK;
}

ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                        const uint8_t* commandData,
                                                        size_t commandDataLen) {
  switch (deviceCommand) {
    case (PLOC_MPSOC::TC_MEM_WRITE): {
      return prepareTcMemWriteCommand(commandData, commandDataLen);
    }
    case (PLOC_MPSOC::TC_MEM_READ): {
      return prepareTcMemReadCommand(commandData, commandDataLen);
    }
    default:
      sif::debug << "PlocMPSoCHandler::buildCommandFromCommand: Command not implemented"
                 << std::endl;
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_FAILED;
}

void PlocMPSoCHandler::fillCommandAndReplyMap() {
  this->insertInCommandMap(PLOC_MPSOC::TC_MEM_WRITE);
  this->insertInCommandMap(PLOC_MPSOC::TC_MEM_READ);
  this->insertInReplyMap(PLOC_MPSOC::ACK_REPORT, 1, nullptr, PLOC_MPSOC::SIZE_ACK_REPORT);
  this->insertInReplyMap(PLOC_MPSOC::EXE_REPORT, 3, nullptr, PLOC_MPSOC::SIZE_EXE_REPORT);
  this->insertInReplyMap(PLOC_MPSOC::TM_MEMORY_READ_REPORT, 2, nullptr,
                         PLOC_MPSOC::SIZE_TM_MEM_READ_REPORT);
}

ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                             DeviceCommandId_t* foundId, size_t* foundLen) {
  ReturnValue_t result = RETURN_OK;

  uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK;

  switch (apid) {
    case (PLOC_MPSOC::APID_ACK_SUCCESS):
      *foundLen = PLOC_MPSOC::SIZE_ACK_REPORT;
      *foundId = PLOC_MPSOC::ACK_REPORT;
      break;
    case (PLOC_MPSOC::APID_ACK_FAILURE):
      *foundLen = PLOC_MPSOC::SIZE_ACK_REPORT;
      *foundId = PLOC_MPSOC::ACK_REPORT;
      break;
    case (PLOC_MPSOC::APID_TM_MEMORY_READ_REPORT):
      *foundLen = PLOC_MPSOC::SIZE_TM_MEM_READ_REPORT;
      *foundId = PLOC_MPSOC::TM_MEMORY_READ_REPORT;
      break;
    case (PLOC_MPSOC::APID_EXE_SUCCESS):
      *foundLen = PLOC_MPSOC::SIZE_EXE_REPORT;
      *foundId = PLOC_MPSOC::EXE_REPORT;
      break;
    case (PLOC_MPSOC::APID_EXE_FAILURE):
      *foundLen = PLOC_MPSOC::SIZE_EXE_REPORT;
      *foundId = PLOC_MPSOC::EXE_REPORT;
      break;
    default: {
      sif::debug << "PlocMPSoCHandler::scanForReply: Reply has invalid apid" << std::endl;
      *foundLen = remainingSize;
      return INVALID_APID;
    }
  }

  /**
   * This should normally never fail. However, this function is also responsible for incrementing
   * the packet sequence count why it is called here.
   */
  result = checkPacketSequenceCount(start);

  return result;
}

ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  ReturnValue_t result = RETURN_OK;

  switch (id) {
    case PLOC_MPSOC::ACK_REPORT: {
      result = handleAckReport(packet);
      break;
    }
    case (PLOC_MPSOC::TM_MEMORY_READ_REPORT): {
      result = handleMemoryReadReport(packet);
      break;
    }
    case (PLOC_MPSOC::EXE_REPORT): {
      result = handleExecutionReport(packet);
      break;
    }
    default: {
      sif::debug << "PlocMPSoCHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }

  return result;
}

void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {}

uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }

ReturnValue_t PlocMPSoCHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                        LocalDataPoolManager& poolManager) {
  return HasReturnvaluesIF::RETURN_OK;
}

void PlocMPSoCHandler::setModeNormal() { mode = MODE_NORMAL; }

ReturnValue_t PlocMPSoCHandler::prepareTcMemWriteCommand(const uint8_t* commandData,
                                                         size_t commandDataLen) {
  const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 |
                                 *(commandData + 2) << 8 | *(commandData + 3);
  const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16 |
                              *(commandData + 6) << 8 | *(commandData + 7);
  packetSequenceCount = (packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK;
  PLOC_MPSOC::TcMemWrite tcMemWrite(memoryAddress, memoryData, packetSequenceCount);
  if (tcMemWrite.getFullSize() > PLOC_MPSOC::MAX_COMMAND_SIZE) {
    sif::debug << "PlocMPSoCHandler::prepareTcMemWriteCommand: Command too big" << std::endl;
    return RETURN_FAILED;
  }
  memcpy(commandBuffer, tcMemWrite.getWholeData(), tcMemWrite.getFullSize());
  rawPacket = commandBuffer;
  rawPacketLen = tcMemWrite.getFullSize();
  nextReplyId = PLOC_MPSOC::ACK_REPORT;
  return RETURN_OK;
}

ReturnValue_t PlocMPSoCHandler::prepareTcMemReadCommand(const uint8_t* commandData,
                                                        size_t commandDataLen) {
  const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 |
                                 *(commandData + 2) << 8 | *(commandData + 3);
  packetSequenceCount = (packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK;
  PLOC_MPSOC::TcMemRead tcMemRead(memoryAddress, packetSequenceCount);
  if (tcMemRead.getFullSize() > PLOC_MPSOC::MAX_COMMAND_SIZE) {
    sif::debug << "PlocMPSoCHandler::prepareTcMemReadCommand: Command too big" << std::endl;
    return RETURN_FAILED;
  }
  memcpy(commandBuffer, tcMemRead.getWholeData(), tcMemRead.getFullSize());
  rawPacket = commandBuffer;
  rawPacketLen = tcMemRead.getFullSize();
  nextReplyId = PLOC_MPSOC::ACK_REPORT;

  return RETURN_OK;
}

ReturnValue_t PlocMPSoCHandler::verifyPacket(const uint8_t* start, size_t foundLen) {
  uint16_t receivedCrc = *(start + foundLen - 2) << 8 | *(start + foundLen - 1);

  uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2);

  if (receivedCrc != recalculatedCrc) {
    return CRC_FAILURE;
  }

  return RETURN_OK;
}

ReturnValue_t PlocMPSoCHandler::handleAckReport(const uint8_t* data) {
  ReturnValue_t result = RETURN_OK;

  result = verifyPacket(data, PLOC_MPSOC::SIZE_ACK_REPORT);
  if (result == CRC_FAILURE) {
    sif::error << "PlocMPSoCHandler::handleAckReport: CRC failure" << std::endl;
    nextReplyId = PLOC_MPSOC::NONE;
    replyRawReplyIfnotWiretapped(data, PLOC_MPSOC::SIZE_ACK_REPORT);
    triggerEvent(CRC_FAILURE_EVENT);
    sendFailureReport(PLOC_MPSOC::ACK_REPORT, CRC_FAILURE);
    disableAllReplies();
    return IGNORE_REPLY_DATA;
  }

  uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;

  switch (apid) {
    case PLOC_MPSOC::APID_ACK_FAILURE: {
      // TODO: Interpretation of status field in acknowledgment report
      sif::debug << "PlocMPSoCHandler::handleAckReport: Received Ack failure report" << std::endl;
      DeviceCommandId_t commandId = getPendingCommand();
      if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
        triggerEvent(ACK_FAILURE, commandId);
      }
      sendFailureReport(PLOC_MPSOC::ACK_REPORT, RECEIVED_ACK_FAILURE);
      disableAllReplies();
      nextReplyId = PLOC_MPSOC::NONE;
      result = IGNORE_REPLY_DATA;
      break;
    }
    case PLOC_MPSOC::APID_ACK_SUCCESS: {
      setNextReplyId();
      break;
    }
    default: {
      sif::debug << "PlocMPSoCHandler::handleAckReport: Invalid APID in Ack report" << std::endl;
      result = RETURN_FAILED;
      break;
    }
  }

  return result;
}

ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) {
  ReturnValue_t result = RETURN_OK;

  result = verifyPacket(data, PLOC_MPSOC::SIZE_EXE_REPORT);
  if (result == CRC_FAILURE) {
    sif::error << "PlocMPSoCHandler::handleExecutionReport: CRC failure" << std::endl;
    nextReplyId = PLOC_MPSOC::NONE;
    return result;
  }

  uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;

  switch (apid) {
    case (PLOC_MPSOC::APID_EXE_SUCCESS): {
      break;
    }
    case (PLOC_MPSOC::APID_EXE_FAILURE): {
      // TODO: Interpretation of status field in execution report
      sif::error << "PlocMPSoCHandler::handleExecutionReport: Received execution failure report"
                 << std::endl;
      DeviceCommandId_t commandId = getPendingCommand();
      if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
        triggerEvent(EXE_FAILURE, commandId);
      } else {
        sif::debug << "PlocMPSoCHandler::handleExecutionReport: Unknown command id" << std::endl;
      }
      sendFailureReport(PLOC_MPSOC::EXE_REPORT, RECEIVED_EXE_FAILURE);
      disableExeReportReply();
      result = IGNORE_REPLY_DATA;
      break;
    }
    default: {
      sif::error << "PlocMPSoCHandler::handleExecutionReport: Unknown APID" << std::endl;
      result = RETURN_FAILED;
      break;
    }
  }

  nextReplyId = PLOC_MPSOC::NONE;

  return result;
}

ReturnValue_t PlocMPSoCHandler::handleMemoryReadReport(const uint8_t* data) {
  ReturnValue_t result = RETURN_OK;

  result = verifyPacket(data, PLOC_MPSOC::SIZE_TM_MEM_READ_REPORT);

  if (result == CRC_FAILURE) {
    sif::error << "PlocMPSoCHandler::handleMemoryReadReport: Memory read report has invalid crc"
               << std::endl;
  }
  /** Send data to commanding queue */
  handleDeviceTM(data + PLOC_MPSOC::DATA_FIELD_OFFSET, PLOC_MPSOC::SIZE_MEM_READ_REPORT_DATA,
                 PLOC_MPSOC::TM_MEMORY_READ_REPORT);

  nextReplyId = PLOC_MPSOC::EXE_REPORT;

  return result;
}

ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command,
                                                      uint8_t expectedReplies, bool useAlternateId,
                                                      DeviceCommandId_t alternateReplyID) {
  ReturnValue_t result = RETURN_OK;

  uint8_t enabledReplies = 0;

  switch (command->first) {
    case PLOC_MPSOC::TC_MEM_WRITE:
      enabledReplies = 2;
      break;
    case PLOC_MPSOC::TC_MEM_READ: {
      enabledReplies = 3;
      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
                                                        PLOC_MPSOC::TM_MEMORY_READ_REPORT);
      if (result != RETURN_OK) {
        sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id "
                   << PLOC_MPSOC::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl;
      }
      break;
    }
    default:
      sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Unknown command id" << std::endl;
      break;
  }

  /**
   * Every command causes at least one acknowledgment and one execution report. Therefore both
   * replies will be enabled here.
   */
  result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
                                                    PLOC_MPSOC::ACK_REPORT);
  if (result != RETURN_OK) {
    sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id "
               << PLOC_MPSOC::ACK_REPORT << " not in replyMap" << std::endl;
  }

  result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
                                                    PLOC_MPSOC::EXE_REPORT);
  if (result != RETURN_OK) {
    sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id "
               << PLOC_MPSOC::EXE_REPORT << " not in replyMap" << std::endl;
  }

  return RETURN_OK;
}

void PlocMPSoCHandler::setNextReplyId() {
  switch (getPendingCommand()) {
    case PLOC_MPSOC::TC_MEM_READ:
      nextReplyId = PLOC_MPSOC::TM_MEMORY_READ_REPORT;
      break;
    default:
      /* If no telemetry is expected the next reply is always the execution report */
      nextReplyId = PLOC_MPSOC::EXE_REPORT;
      break;
  }
}
size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) {
  size_t replyLen = 0;

  if (nextReplyId == PLOC_MPSOC::NONE) {
    return replyLen;
  }

  DeviceReplyIter iter = deviceReplyMap.find(nextReplyId);
  if (iter != deviceReplyMap.end()) {
    if (iter->second.delayCycles == 0) {
      /* Reply inactive */
      return replyLen;
    }
    replyLen = iter->second.replyLen;
  } else {
    sif::debug << "PlocMPSoCHandler::getNextReplyLength: No entry for reply with reply id "
               << std::hex << nextReplyId << " in deviceReplyMap" << std::endl;
  }

  return replyLen;
}

void PlocMPSoCHandler::handleDeviceTM(const uint8_t* data, size_t dataSize,
                                      DeviceCommandId_t replyId) {
  ReturnValue_t result = RETURN_OK;

  if (wiretappingMode == RAW) {
    /* Data already sent in doGetRead() */
    return;
  }

  DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
  if (iter == deviceReplyMap.end()) {
    sif::debug << "PlocMPSoCHandler::handleDeviceTM: Unknown reply id" << std::endl;
    return;
  }
  MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;

  if (queueId == NO_COMMANDER) {
    return;
  }

  result = actionHelper.reportData(queueId, replyId, data, dataSize);
  if (result != RETURN_OK) {
    sif::debug << "PlocMPSoCHandler::handleDeviceTM: Failed to report data" << std::endl;
  }
}

void PlocMPSoCHandler::disableAllReplies() {
  DeviceReplyMap::iterator iter;

  /* Disable ack reply */
  iter = deviceReplyMap.find(PLOC_MPSOC::ACK_REPORT);
  DeviceReplyInfo* info = &(iter->second);
  info->delayCycles = 0;
  info->command = deviceCommandMap.end();

  DeviceCommandId_t commandId = getPendingCommand();

  /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */
  switch (commandId) {
    case PLOC_MPSOC::TC_MEM_WRITE:
      break;
    case PLOC_MPSOC::TC_MEM_READ: {
      iter = deviceReplyMap.find(PLOC_MPSOC::TM_MEMORY_READ_REPORT);
      info = &(iter->second);
      info->delayCycles = 0;
      info->command = deviceCommandMap.end();
      break;
    }
    default: {
      sif::debug << "PlocMPSoCHandler::disableAllReplies: Unknown command id" << commandId
                 << std::endl;
      break;
    }
  }

  /* We must always disable the execution report reply here */
  disableExeReportReply();
}

void PlocMPSoCHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) {
  DeviceReplyIter iter = deviceReplyMap.find(replyId);

  if (iter == deviceReplyMap.end()) {
    sif::debug << "PlocMPSoCHandler::sendFailureReport: Reply not in reply map" << std::endl;
    return;
  }

  DeviceCommandInfo* info = &(iter->second.command->second);

  if (info == nullptr) {
    sif::debug << "PlocMPSoCHandler::sendFailureReport: Reply has no active command" << std::endl;
    return;
  }

  if (info->sendReplyTo != NO_COMMANDER) {
    actionHelper.finish(false, info->sendReplyTo, iter->first, status);
  }
  info->isExecuting = false;
}

void PlocMPSoCHandler::disableExeReportReply() {
  DeviceReplyIter iter = deviceReplyMap.find(PLOC_MPSOC::EXE_REPORT);
  DeviceReplyInfo* info = &(iter->second);
  info->delayCycles = 0;
  info->command = deviceCommandMap.end();
  /* Expected replies is set to one here. The value will set to 0 in replyToReply() */
  info->command->second.expectedReplies = 0;
}

ReturnValue_t PlocMPSoCHandler::checkPacketSequenceCount(const uint8_t* data) {
  uint16_t receivedSequenceCount = (*(data + 2) << 8 | *(data + 3)) & PACKET_SEQUENCE_COUNT_MASK;
  uint16_t expectedPacketSequenceCount = ((packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK);
  if (receivedSequenceCount != expectedPacketSequenceCount) {
    sif::debug << "PlocMPSoCHandler::checkPacketSequenceCount: Packet sequence count mismatch. "
               << std::endl;
    sif::debug << "Received sequence count: " << receivedSequenceCount << ". OBSW sequence "
               << "count: " << expectedPacketSequenceCount << std::endl;
  }
  packetSequenceCount = receivedSequenceCount;
  return RETURN_OK;
}