#include "PdecHandler.h"

#include <fcntl.h>
#include <fsfw/tasks/TaskFactory.h>
#include <poll.h>
#include <sys/mman.h>
#include <unistd.h>

#include <cstring>
#include <sstream>

#include "OBSWConfig.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
#include "fsfw_hal/linux/uio/UioMapper.h"
#include "linux/ipcore/PdecConfig.h"
#include "pdec.h"

using namespace pdec;

// If this is ever shared, protect it with a mutex!
uint32_t PdecHandler::CURRENT_FAR = 0;

PdecHandler::PdecHandler(object_id_t objectId, object_id_t tcDestinationId,
                         LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, UioNames names)
    : SystemObject(objectId),
      tcDestinationId(tcDestinationId),
      gpioComIF(gpioComIF),
      pdecReset(pdecReset),
      actionHelper(this, nullptr),
      uioNames(names),
      paramHelper(this) {
  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      QUEUE_SIZE, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

PdecHandler::~PdecHandler() {}

ReturnValue_t PdecHandler::initialize() {
  tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
  if (tcStore == nullptr) {
    sif::error << "PdecHandler::initialize: Invalid TC store" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  tcDestination = ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestinationId);

  if (tcDestination == nullptr) {
    sif::error << "PdecHandler::initialize: Invalid tc destination specified" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  ReturnValue_t result = returnvalue::OK;

  UioMapper regMapper(uioNames.registers);
  result = regMapper.getMappedAdress(&registerBaseAddress, UioMapper::Permissions::READ_WRITE);
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  UioMapper configMemMapper(uioNames.configMemory);
  result = configMemMapper.getMappedAdress(&memoryBaseAddress, UioMapper::Permissions::READ_WRITE);
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  } else {
    pdecConfig.setMemoryBaseAddress(memoryBaseAddress);
  }
  UioMapper ramMapper(uioNames.ramMemory);
  result = ramMapper.getMappedAdress(&ramBaseAddress, UioMapper::Permissions::READ_WRITE);
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  if (OP_MODE == Modes::IRQ and uioNames.irq == nullptr) {
    sif::error << "Can not use IRQ mode if IRQ UIO name is invalid" << std::endl;
    return returnvalue::FAILED;
  }

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

  result = paramHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }

  return returnvalue::OK;
}

ReturnValue_t PdecHandler::firstLoop() {
  ReturnValue_t result = pdecConfig.write();
  if (result != returnvalue::OK) {
    if (result == LocalParameterHandler::SD_NOT_READY) {
      return result;
    } else {
      sif::error << "PdecHandler::firstLoop: Failed to write PDEC config" << std::endl;
    }
    return returnvalue::FAILED;
  }

  result = releasePdec();
  if (result != returnvalue::OK) {
    return result;
  }

  // This configuration must be done while the PDEC is not held in reset.
  if (OP_MODE == Modes::IRQ) {
    // Configure interrupt mask register to enable interrupts
    *(registerBaseAddress + PDEC_IMR_OFFSET) = pdecConfig.getImrReg();
  }
  result = resetFarStatFlag();
  if (result != returnvalue::OK) {
    // Requires reconfiguration and reinitialization of PDEC
    triggerEvent(INVALID_FAR);
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) {
  if (OP_MODE == Modes::POLLED) {
    return polledOperation();
  } else if (OP_MODE == Modes::IRQ) {
    return irqOperation();
  }
  return returnvalue::FAILED;
}

ReturnValue_t PdecHandler::polledOperation() {
  readCommandQueue();

  switch (state) {
    case State::INIT: {
      handleInitState();
      break;
    }
    case State::RUNNING: {
      if (newTcReceived()) {
        handleNewTc();
      }
      doPeriodicWork();
      break;
    }
    case State::PDEC_RESET: {
      ReturnValue_t result = pdecToReset();
      if (result != returnvalue::OK) {
        triggerEvent(PDEC_RESET_FAILED);
      }
      state = State::INIT;
      break;
    }
    case State::WAIT_FOR_RECOVERY:
      break;
    default:
      sif::error << "PdecHandler::performOperation: Invalid state" << std::endl;
      break;
  }

  return returnvalue::OK;
}

// See https://yurovsky.github.io/2014/10/10/linux-uio-gpio-interrupt.html for more information.
ReturnValue_t PdecHandler::irqOperation() {
  ReturnValue_t result = returnvalue::OK;
  int fd = -1;
  // Used to unmask IRQ
  uint32_t info = 1;

  interruptWindowCd.resetTimer();

  // Clear interrupts with dummy read before unmasking the interrupt. Use a volatile to prevent
  // read being optimized away.
  volatile uint32_t dummy = *(registerBaseAddress + PDEC_PIR_OFFSET);

  while (true) {
    // Default value to unmask IRQ on the write call.
    info = 1;
    readCommandQueue();
    switch (state) {
      case State::INIT: {
        result = handleInitState();
        if (result != returnvalue::OK) {
          break;
        }
        openIrqFile(&fd);
        if (ptmeResetWithReinitializationPending) {
          actionHelper.finish(true, commandedBy, pdec::RESET_PDEC_WITH_REINIITALIZATION);
          ptmeResetWithReinitializationPending = false;
        }
        break;
      }
      case State::PDEC_RESET: {
        result = pdecToReset();
        if (result != returnvalue::OK) {
          triggerEvent(PDEC_RESET_FAILED);
        }
        usleep(10);
        state = State::INIT;
        break;
      }
      case State::RUNNING: {
        doPeriodicWork();
        checkAndHandleIrqs(fd, info);
        break;
      }
      case State::WAIT_FOR_RECOVERY:
        TaskFactory::delayTask(400);
        break;
      default:
        // Should never happen.
        sif::error << "PdecHandler::performOperation: Invalid state" << std::endl;
        TaskFactory::delayTask(400);
        break;
    }
  }
  // To avoid compiler warning
  static_cast<void>(dummy);
  return returnvalue::OK;
}

ReturnValue_t PdecHandler::handleInitState() {
  ReturnValue_t result = firstLoop();
  if (result != returnvalue::OK) {
    if (result == LocalParameterHandler::SD_NOT_READY) {
      if (initTries == MAX_INIT_TRIES) {
        sif::error << "PdecHandler::handleInitState: SD card never becomes ready" << std::endl;
        initFailedHandler(result);
        return result;
      }
      state = State::INIT;
      initTries++;
      TaskFactory::delayTask(400);
      return result;
    }
    sif::error << "PDEC: Init failed with reason 0x" << std::hex << std::setw(4) << result
               << std::endl;
    initFailedHandler(result);
    return result;
  }
  state = State::RUNNING;
  return returnvalue::OK;
}

void PdecHandler::openIrqFile(int* fd) {
  *fd = open(uioNames.irq, O_RDWR);
  if (*fd < 0) {
    sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed"
               << std::endl;
    triggerEvent(OPEN_IRQ_FILE_FAILED);
    state = State::WAIT_FOR_RECOVERY;
  }
}

ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
  ssize_t nb = write(fd, &info, sizeof(info));
  if (nb != static_cast<ssize_t>(sizeof(info))) {
    sif::error << "PdecHandler::irqOperation: Unmasking IRQ failed" << std::endl;
    triggerEvent(WRITE_SYSCALL_ERROR_PDEC, errno);
    close(fd);
    state = State::INIT;
    return returnvalue::FAILED;
  }
  struct pollfd fds = {.fd = fd, .events = POLLIN, .revents = 0};
  int ret = poll(&fds, 1, IRQ_TIMEOUT_MS);
  if (ret == 0) {
    // No TCs for timeout period
    genericCheckCd.resetTimer();
    resetIrqLimiters();
  } else if (ret >= 1) {
    // Interrupt handling.
    nb = read(fd, &info, sizeof(info));
    interruptCounter++;
    if (nb == static_cast<ssize_t>(sizeof(info))) {
      uint32_t pisr = *(registerBaseAddress + PDEC_PISR_OFFSET);
      if ((pisr & TC_NEW_MASK) == TC_NEW_MASK) {
        // handle TC
        handleNewTc();
      }
      if ((pisr & TC_ABORT_MASK) == TC_ABORT_MASK) {
        tcAbortCounter += 1;
      }
      if ((pisr & NEW_FAR_MASK) == NEW_FAR_MASK) {
        // Read FAR here
        CURRENT_FAR = readFar();
        checkFrameAna(CURRENT_FAR);
      }
      // Clear interrupts with dummy read. Volatile is important here to prevent
      // compiler opitmizations in release builds!
      volatile uint32_t dummy = *(registerBaseAddress + PDEC_PIR_OFFSET);
      static_cast<void>(dummy);

      if (genericCheckCd.hasTimedOut()) {
        genericCheckCd.resetTimer();
        if (interruptWindowCd.hasTimedOut()) {
          if (interruptCounter >= MAX_ALLOWED_IRQS_PER_WINDOW) {
            sif::error << "PdecHandler::irqOperation: Possible IRQ storm" << std::endl;
            triggerEvent(TOO_MANY_IRQS, MAX_ALLOWED_IRQS_PER_WINDOW);
            resetIrqLimiters();
            TaskFactory::delayTask(400);
            return returnvalue::FAILED;
          }
          resetIrqLimiters();
        }
      }
    }
  } else {
    sif::error << "PdecHandler::irqOperation: Poll error with errno " << errno << ": "
               << strerror(errno) << std::endl;
    triggerEvent(POLL_SYSCALL_ERROR_PDEC, errno);
    close(fd);
    state = State::INIT;
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

void PdecHandler::readCommandQueue(void) {
  CommandMessage message;
  ReturnValue_t result = returnvalue::FAILED;

  result = commandQueue->receiveMessage(&message);
  if (result == returnvalue::OK) {
    result = actionHelper.handleActionMessage(&message);
    if (result == returnvalue::OK) {
      return;
    }
    result = paramHelper.handleParameterMessage(&message);
    if (result == returnvalue::OK) {
      return;
    }
    CommandMessage reply;
    reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, message.getCommand());
    commandQueue->reply(&reply);
    return;
  }
}

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

ReturnValue_t PdecHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                         const uint8_t* data, size_t size) {
  using namespace pdec;
  switch (actionId) {
    case PRINT_CLCW:
      printClcw();
      return EXECUTION_FINISHED;
    case PRINT_PDEC_MON:
      printPdecMon();
      return EXECUTION_FINISHED;
    case RESET_PDEC_NO_REINIITALIZATION: {
      pdecToReset();
      usleep(20);
      releasePdec();
      return EXECUTION_FINISHED;
    }
    case RESET_PDEC_WITH_REINIITALIZATION: {
      state = State::PDEC_RESET;
      ptmeResetWithReinitializationPending = true;
      this->commandedBy = commandedBy;
      return returnvalue::OK;
    }
    default:
      return COMMAND_NOT_IMPLEMENTED;
  }
}

ReturnValue_t PdecHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                        ParameterWrapper* parameterWrapper,
                                        const ParameterWrapper* newValues, uint16_t startAtIndex) {
  if ((domainId == 0) and (uniqueIdentifier == ParameterId::POSITIVE_WINDOW)) {
    uint8_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    uint8_t positiveWindow = 0;
    result = pdecConfig.getPositiveWindow(positiveWindow);
    if (result != returnvalue::OK) {
      sif::warning << "PdecHandler::getParameter: Failed to get positive window from pdec config"
                   << std::endl;
      return returnvalue::FAILED;
    }
    parameterWrapper->set(positiveWindow);
    result = pdecConfig.setPositiveWindow(newVal);
    if (result != returnvalue::OK) {
      sif::warning << "PdecHandler::getParameter: Failed to set positive window" << std::endl;
      return returnvalue::FAILED;
    }
    // PDEC needs reset to apply this parameter change
    state = State::PDEC_RESET;
    return returnvalue::OK;
  } else if ((domainId == 0) and (uniqueIdentifier == ParameterId::NEGATIVE_WINDOW)) {
    uint8_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    uint8_t negativeWindow = 0;
    result = pdecConfig.getNegativeWindow(negativeWindow);
    if (result != returnvalue::OK) {
      sif::warning << "PdecHandler::getParameter: Failed to get negative window from pdec config"
                   << std::endl;
      return returnvalue::FAILED;
    }
    parameterWrapper->set(negativeWindow);
    result = pdecConfig.setNegativeWindow(newVal);
    if (result != returnvalue::OK) {
      sif::warning << "PdecHandler::getParameter: Failed to set negative window" << std::endl;
      return returnvalue::FAILED;
    }
    // PDEC needs reset to apply this parameter change
    state = State::PDEC_RESET;
    return returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t PdecHandler::resetFarStatFlag() {
  uint32_t pdecFar = readFar();
  if ((pdecFar & FAR_STAT_MASK) != 0) {
    sif::warning << "PdecHandler::resetFarStatFlag: FAR register stat bit is not 0."
                 << " Read value for FAR: 0x" << std::hex << static_cast<unsigned int>(pdecFar)
                 << std::endl;
    CURRENT_FAR = pdecFar;
    return returnvalue::FAILED;
  }
#if OBSW_DEBUG_PDEC_HANDLER == 1
  sif::debug << "PdecHandler::resetFarStatFlag: read FAR with value: 0x" << std::hex << pdecFar
             << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER == 1 */
  CURRENT_FAR = pdecFar;
  return returnvalue::OK;
}

ReturnValue_t PdecHandler::releasePdec() {
  ReturnValue_t result = returnvalue::OK;
  result = gpioComIF->pullHigh(pdecReset);
  if (result != returnvalue::OK) {
    sif::error << "PdecHandler::releasePdec: Failed to release PDEC reset signal" << std::endl;
  }
  return result;
}

ReturnValue_t PdecHandler::pdecToReset() {
  ReturnValue_t result = returnvalue::OK;
  result = gpioComIF->pullLow(pdecReset);
  if (result != returnvalue::OK) {
    sif::error << "PdecHandler::pdecToReset: Failed to pull PDEC reset line"
                  " to low"
               << std::endl;
  }
  return result;
}

bool PdecHandler::newTcReceived() {
  uint32_t pdecFar = readFar();

  if (pdecFar >> STAT_POSITION != NEW_FAR_RECEIVED) {
    CURRENT_FAR = pdecFar;
    return false;
  }
  if (!checkFrameAna(pdecFar)) {
    CURRENT_FAR = pdecFar;
    return false;
  }
  return true;
}

void PdecHandler::doPeriodicWork() { checkLocks(); }

bool PdecHandler::checkFrameAna(uint32_t pdecFar) {
  bool frameValid = false;
  FrameAna_t frameAna = static_cast<FrameAna_t>((pdecFar & FRAME_ANA_MASK) >> FRAME_ANA_POSITION);
  switch (frameAna) {
    case (FrameAna_t::ABANDONED_CLTU): {
      triggerEvent(INVALID_TC_FRAME, ABANDONED_CLTU_RETVAL);
      sif::warning << "PdecHandler::checkFrameAna: Abondoned CLTU" << std::endl;
      break;
    }
    case (FrameAna_t::FRAME_DIRTY): {
      triggerEvent(INVALID_TC_FRAME, FRAME_DIRTY_RETVAL);
      sif::warning << "PdecHandler::checkFrameAna: Frame dirty" << std::endl;
      break;
    }
    case (FrameAna_t::FRAME_ILLEGAL): {
      sif::warning << "PdecHandler::checkFrameAna: Frame illegal for one reason" << std::endl;
      handleIReason(pdecFar, FRAME_ILLEGAL_ONE_REASON);
      break;
    }
    case (FrameAna_t::FRAME_ILLEGAL_MULTI_REASON): {
      sif::warning << "PdecHandler::checkFrameAna: Frame illegal for multiple reasons" << std::endl;
      handleIReason(pdecFar, FRAME_ILLEGAL_MULTIPLE_REASONS);
      break;
    }
    case (FrameAna_t::AD_DISCARDED_LOCKOUT): {
      triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_LOCKOUT_RETVAL);
      sif::warning << "PdecHandler::checkFrameAna: AD frame discarded because of lockout"
                   << std::endl;
      break;
    }
    case (FrameAna_t::AD_DISCARDED_WAIT): {
      triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_LOCKOUT_RETVAL);
      sif::warning << "PdecHandler::checkFrameAna: AD frame discarded because of wait" << std::endl;
      break;
    }
    case (FrameAna_t::AD_DISCARDED_NS_VR): {
      triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_NS_VS);
      sif::warning << "PdecHandler::checkFrameAna: AD frame discarded because N(S) or V(R)"
                   << std::endl;
      break;
    }
    case (FrameAna_t::FRAME_ACCEPTED): {
#if OBSW_DEBUG_PDEC_HANDLER == 1
      sif::info << "PdecHandler::checkFrameAna: Accepted TC frame" << std::endl;
#endif
      frameValid = true;
      break;
    }
    default: {
      sif::debug << "PdecHandler::checkFrameAna: Invalid frame analysis report" << std::endl;
      break;
    }
  }
  return frameValid;
}

void PdecHandler::handleIReason(uint32_t pdecFar, ReturnValue_t parameter1) {
  IReason_t ireason = static_cast<IReason_t>((pdecFar & IREASON_MASK) >> IREASON_POSITION);
  switch (ireason) {
    case (IReason_t::NO_REPORT): {
      triggerEvent(INVALID_TC_FRAME, parameter1, NO_REPORT_RETVAL);
      sif::info << "PdecHandler::handleIReason: No illegal report" << std::endl;
      break;
    }
    case (IReason_t::ERROR_VERSION_NUMBER): {
      triggerEvent(INVALID_TC_FRAME, parameter1, ERROR_VERSION_NUMBER_RETVAL);
      sif::info << "PdecHandler::handleIReason: Error in version number and reserved A and B "
                << "fields" << std::endl;
      break;
    }
    case (IReason_t::ILLEGAL_COMBINATION): {
      triggerEvent(INVALID_TC_FRAME, parameter1, ILLEGAL_COMBINATION_RETVAL);
      sif::info << "PdecHandler::handleIReason: Illegal combination (AC) of bypass and control "
                << "command flags" << std::endl;
      break;
    }
    case (IReason_t::INVALID_SC_ID): {
      triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_SC_ID_RETVAL);
      sif::info << "PdecHandler::handleIReason: Invalid spacecraft identifier " << std::endl;
      break;
    }
    case (IReason_t::INVALID_VC_ID_MSB): {
      triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_MSB_RETVAL);
      sif::info << "PdecHandler::handleIReason: VC identifier bit 0 to 4 did not match "
                << std::endl;
      break;
    }
    case (IReason_t::INVALID_VC_ID_LSB): {
      triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_LSB_RETVAL);
      sif::info << "PdecHandler::handleIReason: VC identifier bit 5 did not match " << std::endl;
      break;
    }
    case (IReason_t::NS_NOT_ZERO): {
      triggerEvent(INVALID_TC_FRAME, parameter1, NS_NOT_ZERO_RETVAL);
      sif::info << "PdecHandler::handleIReason: N(S) of BC or BD frame not set to all zeros"
                << std::endl;
      break;
    }
    case (IReason_t::INCORRECT_BC_CC): {
      triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_BC_CC);
      sif::info << "PdecHandler::handleIReason: Invalid BC control command format" << std::endl;
      break;
    }
    default: {
      sif::info << "PdecHandler::handleIReason: Invalid reason id" << std::endl;
      break;
    }
  }
}

void PdecHandler::handleNewTc() {
  ReturnValue_t result = returnvalue::OK;

  uint32_t tcLength = 0;
  result = readTc(tcLength);
  if (result != returnvalue::OK) {
    return;
  }
#if OBSW_DEBUG_PDEC_HANDLER == 1
  unsigned int mapId = tcSegment[0] & MAP_ID_MASK;
  sif::info << "PdecHandler::handleNewTc: Received TC segment with map ID " << mapId << std::endl;
  printTC(tcLength);
#endif /* OBSW_DEBUG_PDEC_HANDLER */

  store_address_t storeId;
  result = tcStore->addData(&storeId, tcSegment + 1, tcLength - 1);
  if (result != returnvalue::OK) {
    sif::warning << "PdecHandler::handleNewTc: Failed to add received space packet to store"
                 << std::endl;
    return;
  }

  TmTcMessage message(storeId);

  result = MessageQueueSenderIF::sendMessage(tcDestination->getRequestQueue(), &message);
  if (result != returnvalue::OK) {
    sif::warning << "PdecHandler::handleNewTc: Failed to send message to TC destination"
                 << std::endl;
    tcStore->deleteData(storeId);
    return;
  }

  return;
}

ReturnValue_t PdecHandler::readTc(uint32_t& tcLength) {
  uint32_t tcOffset = (*(registerBaseAddress + PDEC_BPTR_OFFSET) - PHYSICAL_RAM_BASE_ADDRESS) / 4;

#if OBSW_DEBUG_PDEC_HANDLER == 1
  sif::debug << "PdecHandler::readTc: TC offset: 0x" << std::hex << tcOffset << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER */

  tcLength = *(registerBaseAddress + PDEC_SLEN_OFFSET);

#if OBSW_DEBUG_PDEC_HANDLER == 1
  sif::debug << "PdecHandler::readTc: TC segment length: " << std::dec << tcLength << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER */

  if (tcLength > MAX_TC_SEGMENT_SIZE) {
    sif::warning << "PdecHandler::handleNewTc: Read invalid TC length from PDEC register"
                 << std::endl;
    return returnvalue::FAILED;
  }

  uint32_t idx = 0;
  uint32_t tcData = 0;
  for (idx = 0; idx <= tcLength; idx = idx + 4) {
    tcData = *(ramBaseAddress + tcOffset + idx / 4);
    if (idx == 0) {
      tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
      tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
      tcSegment[idx + 2] = static_cast<uint8_t>(tcData & 0xFF);
    } else if (tcLength - idx + 1 == 3) {
      tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
      tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
      tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
    } else if (tcLength - idx + 1 == 2) {
      tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
      tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
    } else if (tcLength - idx + 1 == 1) {
      tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
    } else {
      tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
      tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
      tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
      tcSegment[idx + 2] = static_cast<uint8_t>(tcData & 0xFF);
    }
  }

  // Backend buffer is handled back to PDEC3
  *(registerBaseAddress + PDEC_BFREE_OFFSET) = 0;

  return returnvalue::OK;
}

void PdecHandler::printTC(uint32_t tcLength) {
  std::stringstream tcSegmentStream;
  tcSegmentStream << "TC segment data: 0x";
  for (uint32_t idx = 0; idx < tcLength; idx++) {
    tcSegmentStream << std::setfill('0') << std::setw(2) << std::hex
                    << static_cast<unsigned int>(tcSegment[idx]);
  }
  sif::info << tcSegmentStream.str() << std::endl;
}

uint32_t PdecHandler::getClcw() { return *(registerBaseAddress + PDEC_CLCW_OFFSET); }

uint32_t PdecHandler::getPdecMon() { return *(registerBaseAddress + PDEC_MON_OFFSET); }

void PdecHandler::printClcw() {
  uint32_t clcw = getClcw();
  uint8_t type = static_cast<uint8_t>((clcw >> 31) & 0x1);
  uint8_t versionNo = static_cast<uint8_t>((clcw >> 29) & 0x3);
  uint8_t status = static_cast<uint8_t>((clcw >> 26) & 0x7);
  uint8_t cop = static_cast<uint8_t>((clcw >> 24) & 0x3);
  uint8_t vcId = static_cast<uint8_t>((clcw >> 18) & 0x3F);
  uint8_t noRf = static_cast<uint8_t>((clcw >> 15) & 0x1);
  uint8_t noBitLock = static_cast<uint8_t>((clcw >> 14) & 0x1);
  uint8_t lockoutFlag = static_cast<uint8_t>((clcw >> 13) & 0x1);
  uint8_t waitFlag = static_cast<uint8_t>((clcw >> 12) & 0x1);
  uint8_t retransmitFlag = static_cast<uint8_t>((clcw >> 11) & 0x1);
  uint8_t farmBcnt = static_cast<uint8_t>((clcw >> 9) & 0x3);
  // Expected frame sequence number in te next AD frame
  uint8_t repValue = static_cast<uint8_t>(clcw & 0xFF);
  sif::info << std::setw(30) << std::left << "CLCW type: " << std::hex << "0x"
            << static_cast<unsigned int>(type) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW version no: " << std::hex << "0x"
            << static_cast<unsigned int>(versionNo) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW status: " << std::hex << "0x"
            << static_cast<unsigned int>(status) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW COP: " << std::hex << "0x"
            << static_cast<unsigned int>(cop) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW virtual channel ID: " << std::hex << "0x"
            << static_cast<unsigned int>(vcId) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW no RF: " << std::hex << "0x"
            << static_cast<unsigned int>(noRf) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW no bit lock: " << std::hex << "0x"
            << static_cast<unsigned int>(noBitLock) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW lockout flag: " << std::hex << "0x"
            << static_cast<unsigned int>(lockoutFlag) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW wait flag: " << std::hex << "0x"
            << static_cast<unsigned int>(waitFlag) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW retransmit flag: " << std::hex << "0x"
            << static_cast<unsigned int>(retransmitFlag) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW FARM B count: " << std::hex << "0x"
            << static_cast<unsigned int>(farmBcnt) << std::endl;
  sif::info << std::setw(30) << std::left << "CLCW rep value: " << std::hex << "0x"
            << static_cast<unsigned int>(repValue) << std::endl;
}

void PdecHandler::printPdecMon() {
  uint32_t pdecMon = getPdecMon();
  uint32_t tc0ChannelStatus = (pdecMon & TC0_STATUS_MASK) >> TC0_STATUS_POS;
  uint32_t tc1ChannelStatus = (pdecMon & TC1_STATUS_MASK) >> TC1_STATUS_POS;
  uint32_t tc2ChannelStatus = (pdecMon & TC2_STATUS_MASK) >> TC2_STATUS_POS;
  uint32_t tc3ChannelStatus = (pdecMon & TC3_STATUS_MASK) >> TC3_STATUS_POS;
  uint32_t tc4ChannelStatus = (pdecMon & TC4_STATUS_MASK) >> TC4_STATUS_POS;
  uint32_t tc5ChannelStatus = (pdecMon & TC5_STATUS_MASK) >> TC5_STATUS_POS;
  uint32_t lock = (pdecMon & LOCK_MASK) >> LOCK_POS;
  sif::info << std::setw(30) << std::left << "TC0 status: " << getMonStatusString(tc0ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "TC1 status: " << getMonStatusString(tc1ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "TC2 status: " << getMonStatusString(tc2ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "TC3 status: " << getMonStatusString(tc3ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "TC4 status: " << getMonStatusString(tc4ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "TC5 status: " << getMonStatusString(tc5ChannelStatus)
            << std::endl;
  sif::info << std::setw(30) << std::left << "Start sequence lock: " << lock << std::endl;
}

uint32_t PdecHandler::readFar() { return *(registerBaseAddress + PDEC_FAR_OFFSET); }

void PdecHandler::resetIrqLimiters() {
  interruptWindowCd.resetTimer();
  interruptCounter = 0;
}

void PdecHandler::checkLocks() {
  uint32_t clcw = getClcw();
  if (not(clcw & NO_RF_MASK) && not carrierLock) {
    triggerEvent(CARRIER_LOCK);
    carrierLock = true;
  } else if ((clcw & NO_RF_MASK) && carrierLock) {
    carrierLock = false;
    triggerEvent(LOST_CARRIER_LOCK_PDEC);
  }
  if (not(clcw & NO_BITLOCK_MASK) && not bitLock) {
    triggerEvent(BIT_LOCK_PDEC);
    bitLock = true;
  } else if ((clcw & NO_BITLOCK_MASK) && bitLock) {
    bitLock = false;
    triggerEvent(LOST_BIT_LOCK_PDEC);
  }
}

void PdecHandler::initFailedHandler(ReturnValue_t reason) {
  triggerEvent(pdec::PDEC_INIT_FAILED, reason, 0);
  if (ptmeResetWithReinitializationPending) {
    actionHelper.finish(false, commandedBy, pdec::RESET_PDEC_WITH_REINIITALIZATION, reason);
    ptmeResetWithReinitializationPending = false;
  }
  state = State::WAIT_FOR_RECOVERY;
}

std::string PdecHandler::getMonStatusString(uint32_t status) {
  switch (status) {
    case TC_CHANNEL_INACTIVE:
      return std::string("inactive");
    case TC_CHANNEL_ACTIVE:
      return std::string("active");
    case TC_CHANNEL_TIMEDOUT:
      return std::string("timed out");
    default:
      sif::warning << "PdecHandler::getMonStatusString: Invalid status" << std::endl;
      return std::string();
      break;
  }
}