#include <utility>

#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/config/comCfg.h>

extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_ONLY;
//extern std::pair<uint32_t, FixedArrayList<ModeListEntry, 3>> COM_TABLE_RX_ONLY_TGT;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE;
extern std::pair<uint32_t, FixedArrayList<ModeListEntry, 3>> COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_LOW_RATE;
extern std::pair<uint8_t, FixedArrayList<ModeListEntry, 3>> COM_SEQUENCE_RX_AND_TX_HIGH_RATE;

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();
  if (mode != COM_SEQUENCE_RX_ONLY.first) {
  	checkTransmitterCountdown();
  }
}

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))) {
  	uint8_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_SEQUENCE_RX_ONLY.first) {
    transmitterCountdown.timeOut();
  } else if ((mode == COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first) ||
             (mode == COM_SEQUENCE_RX_AND_TX_LOW_RATE.first) ||
             (mode == COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first)) {
    transmitterCountdown.resetTimer();
  }
  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() { startRxAndTxDefaultSeq(); }

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

void ComSubsystem::startRxAndTxDefaultSeq() {
  // Turns transmitter on
  startTransition(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first, SUBMODE_NONE);
}

void ComSubsystem::checkTransmitterCountdown() {
  if (transmitterCountdown.hasTimedOut()) {
    startTransition(COM_SEQUENCE_RX_ONLY.first, SUBMODE_NONE);
  }
}