#include "plocMpsocHelpers.h"

#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
#include "mission/payload/plocSpBase.h"

uint16_t mpsoc::getStatusFromRawData(const uint8_t* data) {
  return (*(data + STATUS_OFFSET) << 8) | *(data + STATUS_OFFSET + 1);
}
std::string mpsoc::getStatusString(uint16_t status) {
  switch (status) {
    case (mpsoc::statusCode::UNKNOWN_APID): {
      return "Unknown APID";
      break;
    }
    case (mpsoc::statusCode::INCORRECT_LENGTH): {
      return "Incorrect length";
      break;
    }
    case (mpsoc::statusCode::FLASH_DRIVE_ERROR): {
      return "flash drive error";
      break;
    }
    case (mpsoc::statusCode::INCORRECT_CRC): {
      return "Incorrect crc";
      break;
    }
    case (mpsoc::statusCode::INCORRECT_PKT_SEQ_CNT): {
      return "Incorrect packet sequence count";
      break;
    }
    case (mpsoc::statusCode::TC_NOT_ALLOWED_IN_MODE): {
      return "TC not allowed in this mode";
      break;
    }
    case (mpsoc::statusCode::TC_EXEUTION_DISABLED): {
      return "TC execution disabled";
      break;
    }
    case (mpsoc::statusCode::FLASH_MOUNT_FAILED): {
      return "Flash mount failed";
      break;
    }
    case (mpsoc::statusCode::FLASH_FILE_ALREADY_OPEN): {
      return "Flash file already open";
      break;
    }
    case (mpsoc::statusCode::FLASH_FILE_ALREADY_CLOSED): {
      return "Flash file already closed";
      break;
    }
    case (mpsoc::statusCode::FLASH_FILE_OPEN_FAILED): {
      return "Flash file open failed";
      break;
    }
    case (mpsoc::statusCode::FLASH_FILE_NOT_OPEN): {
      return "Flash file not open";
      break;
    }
    case (mpsoc::statusCode::FLASH_UNMOUNT_FAILED): {
      return "Flash unmount failed";
      break;
    }
    case (mpsoc::statusCode::HEAP_ALLOCATION_FAILED): {
      return "Heap allocation failed";
      break;
    }
    case (mpsoc::statusCode::INVALID_PARAMETER): {
      return "Invalid parameter";
      break;
    }
    case (mpsoc::statusCode::NOT_INITIALIZED): {
      return "Not initialized";
      break;
    }
    case (mpsoc::statusCode::REBOOT_IMMINENT): {
      return "Reboot imminent";
      break;
    }
    case (mpsoc::statusCode::CORRUPT_DATA): {
      return "Corrupt data";
      break;
    }
    case (mpsoc::statusCode::FLASH_CORRECTABLE_MISMATCH): {
      return "Flash correctable mismatch";
      break;
    }
    case (mpsoc::statusCode::FLASH_UNCORRECTABLE_MISMATCH): {
      return "Flash uncorrectable mismatch";
      break;
    }
    case (mpsoc::statusCode::DEFAULT_ERROR_CODE): {
      return "Default error code";
      break;
    }
    default:
      std::stringstream ss;
      ss << "0x" << std::hex << status;
      return ss.str().c_str();
      break;
  }
  return "";
}

void mpsoc::printRxPacket(const SpacePacketReader& spReader) {
  if (mpsoc::MPSOC_RX_WIRETAPPING) {
    sif::debug << "RECV MPSOC packet. APID 0x" << std::hex << std::setw(3) << spReader.getApid()
               << std::dec << " Size " << spReader.getFullPacketLen() << " SSC "
               << spReader.getSequenceCount() << std::endl;
  }
}

void mpsoc::printTxPacket(const ploc::SpTcBase& tcBase) {
  if (mpsoc::MPSOC_TX_WIRETAPPING) {
    sif::debug << "SEND MPSOC packet. APID 0x" << std::hex << std::setw(3) << tcBase.getApid()
               << " Size " << std::dec << tcBase.getFullPacketLen() << " SSC "
               << tcBase.getSeqCount() << std::endl;
  }
}