#include "ComSubsystem.h"

#include <fsfw/events/EventManagerIF.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <linux/ipcore/PdecHandler.h>
#include <mission/comDefs.h>
#include <mission/config/comCfg.h>

#include <utility>

ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                           uint32_t maxNumberOfTables, uint32_t transmitterTimeout)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) {
  com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK);
  auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
  eventQueue =
      QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
  transmitterCountdown.setTimeout(transmitterTimeout);
}

void ComSubsystem::performChildOperation() {
  readEventQueue();
  // Execute default rate sequence after transition has been completed
  if (rememberBitLock and not isInTransition) {
    startRxAndTxLowRateSeq();
    rememberBitLock = false;
  }
  if (countdownActive) {
    checkTransmitterCountdown();
  }
  Subsystem::performChildOperation();
}

MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }

ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                         ParameterWrapper *parameterWrapper,
                                         const ParameterWrapper *newValues, uint16_t startAtIndex) {
  if ((domainId == 0) and (uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::DATARATE))) {
    uint8_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    if (newVal >= static_cast<uint8_t>(com::Datarate::NUM_DATARATES)) {
      return HasParametersIF::INVALID_VALUE;
    }
    parameterWrapper->set(datarateCfg);
    com::setCurrentDatarate(static_cast<com::Datarate>(newVal));
    return returnvalue::OK;
  } else if ((domainId == 0) and
             (uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::TRANSMITTER_TIMEOUT))) {
    uint32_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    parameterWrapper->set(transmitterTimeout);
    transmitterTimeout = newVal;
    transmitterCountdown.setTimeout(transmitterTimeout);
    return returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t ComSubsystem::handleCommandMessage(CommandMessage *message) {
  ReturnValue_t result = paramHelper.handleParameterMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  return Subsystem::handleCommandMessage(message);
}

ReturnValue_t ComSubsystem::initialize() {
  ReturnValue_t result = paramHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  EventManagerIF *manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  result = manager->registerListener(eventQueue->getId());
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event "
                    "listener"
                 << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  result = manager->subscribeToEventRange(eventQueue->getId(),
                                          event::getEventId(PdecHandler::CARRIER_LOCK),
                                          event::getEventId(PdecHandler::BIT_LOCK_PDEC));
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC "
                  "handler"
               << std::endl;
#endif
    return result;
  }
  return Subsystem::initialize();
}

void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) {
  // Depending on the submode the transmitter timeout is enabled or
  // disabled here
  if (mode == com::Submode::RX_ONLY) {
    transmitterCountdown.timeOut();
    countdownActive = false;
  } else if (isTxMode(mode)) {
    // Only start transmitter countdown if transmitter is not already on
    if (not isTxMode(this->mode)) {
      transmitterCountdown.resetTimer();
      countdownActive = true;
    }
  }
  Subsystem::startTransition(mode, submode);
}

void ComSubsystem::readEventQueue() {
  EventMessage event;
  for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
       result = eventQueue->receiveMessage(&event)) {
    switch (event.getMessageId()) {
      case EventMessage::EVENT_MESSAGE:
        handleEventMessage(&event);
        break;
      default:
        sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
                   << std::endl;
        break;
    }
  }
}

void ComSubsystem::handleEventMessage(EventMessage *eventMessage) {
  Event event = eventMessage->getEvent();
  switch (event) {
    case PdecHandler::BIT_LOCK_PDEC: {
      handleBitLockEvent();
      break;
    }
    case PdecHandler::CARRIER_LOCK: {
      handleCarrierLockEvent();
      break;
    }
    default:
      sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl;
      break;
  }
}

void ComSubsystem::handleBitLockEvent() {
  if (isTxMode(mode)) {
    // Tx already on
    return;
  }
  if (isInTransition) {
    rememberBitLock = true;
    return;
  }
  startRxAndTxLowRateSeq();
}

void ComSubsystem::handleCarrierLockEvent() {
  if (!enableTxWhenCarrierLock) {
    return;
  }
  startRxAndTxLowRateSeq();
}

void ComSubsystem::startRxAndTxLowRateSeq() {
  // Turns transmitter on
  startTransition(com::Submode::RX_AND_TX_LOW_DATARATE, SUBMODE_NONE);
}

void ComSubsystem::checkTransmitterCountdown() {
  if (transmitterCountdown.hasTimedOut()) {
    startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
    countdownActive = false;
  }
}

bool ComSubsystem::isTxMode(Mode_t mode) {
  if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) ||
      (mode == com::Submode::RX_AND_TX_LOW_DATARATE) ||
      (mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) {
    return true;
  }
  return false;
}