#include "CCSDSHandler.h"

#include <linux/obc/PdecHandler.h>
#include <linux/obc/PtmeConfig.h>

#include "fsfw/events/EventManagerIF.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serialize/SerializeAdapter.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"

CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
                           PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock,
                           gpioId_t enTxData, uint32_t transmitterTimeout)
    : SystemObject(objectId),
      ptmeId(ptmeId),
      tcDestination(tcDestination),
      parameterHelper(this),
      actionHelper(this, nullptr),
      ptmeConfig(ptmeConfig),
      gpioIF(gpioIF),
      enTxClock(enTxClock),
      enTxData(enTxData),
      TRANSMITTER_TIMEOUT(transmitterTimeout) {
  commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
  eventQueue =
      QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}

CCSDSHandler::~CCSDSHandler() {}

ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) {
  checkEvents();
  readCommandQueue();
  handleTelemetry();
  handleTelecommands();
  checkTxTimer();
  return RETURN_OK;
}

void CCSDSHandler::handleTelemetry() {
  VirtualChannelMapIter iter;
  for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
    iter->second->performOperation();
  }
}

void CCSDSHandler::handleTelecommands() {}

ReturnValue_t CCSDSHandler::initialize() {
  ReturnValue_t result = RETURN_OK;
  PtmeIF* ptme = ObjectManager::instance()->get<PtmeIF>(ptmeId);
  if (ptme == nullptr) {
    sif::warning << "Invalid PTME object" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  AcceptsTelecommandsIF* tcDistributor =
      ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
  if (tcDistributor == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "CCSDSHandler::initialize: Invalid TC Distributor object" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  tcDistributorQueueId = tcDistributor->getRequestQueue();

  result = parameterHelper.initialize();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }

  result = actionHelper.initialize(commandQueue);
  if (result != RETURN_OK) {
    return result;
  }

  VirtualChannelMapIter iter;
  for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
    result = iter->second->initialize();
    if (result != RETURN_OK) {
      return result;
    }
    iter->second->setPtmeObject(ptme);
  }

  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  result = manager->registerListener(eventQueue->getId());
  if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler 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 != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC "
                  "handler"
               << std::endl;
#endif
    return result;
  }
  result = ptmeConfig->initialize();
  if (result != RETURN_OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

#if OBSW_SYRLINKS_SIMULATED == 1
  ptmeConfig->invertTxClock(true);
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/

  return result;
}

void CCSDSHandler::readCommandQueue(void) {
  CommandMessage commandMessage;
  ReturnValue_t result = RETURN_FAILED;

  result = commandQueue->receiveMessage(&commandMessage);
  if (result == RETURN_OK) {
    result = parameterHelper.handleParameterMessage(&commandMessage);
    if (result == RETURN_OK) {
      return;
    }
    result = actionHelper.handleActionMessage(&commandMessage);
    if (result == RETURN_OK) {
      return;
    }
    CommandMessage reply;
    reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
    commandQueue->reply(&reply);
    return;
  }
}

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

void CCSDSHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
  if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) {
    sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl;
    return;
  }

  if (virtualChannel == nullptr) {
    sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel interface"
                 << std::endl;
    return;
  }

  auto status = virtualChannelMap.emplace(vcId, virtualChannel);
  if (status.second == false) {
    sif::warning << "CCSDSHandler::addVirtualChannel: Failed to add virtual channel to "
                    "virtual channel map"
                 << std::endl;
    return;
  }
}

MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) {
  if (virtualChannel < common::NUMBER_OF_VIRTUAL_CHANNELS) {
    VirtualChannelMapIter iter = virtualChannelMap.find(virtualChannel);
    if (iter != virtualChannelMap.end()) {
      return iter->second->getReportReceptionQueue();
    } else {
      sif::warning << "CCSDSHandler::getReportReceptionQueue: Virtual channel with ID "
                   << static_cast<unsigned int>(virtualChannel) << " not in virtual channel map"
                   << std::endl;
      return MessageQueueIF::NO_QUEUE;
    }
  } else {
    sif::debug << "CCSDSHandler::getReportReceptionQueue: Invalid virtual channel requested";
  }
  return MessageQueueIF::NO_QUEUE;
}

ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                         ParameterWrapper* parameterWrapper,
                                         const ParameterWrapper* newValues, uint16_t startAtIndex) {
  return RETURN_OK;
}

uint16_t CCSDSHandler::getIdentifier() { return 0; }

MessageQueueId_t CCSDSHandler::getRequestQueue() {
  // Forward packets directly to TC distributor
  return tcDistributorQueueId;
}

ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                          const uint8_t* data, size_t size) {
  ReturnValue_t result = RETURN_OK;
  switch (actionId) {
    case SET_LOW_RATE: {
      result = ptmeConfig->setRate(RATE_100KBPS);
      break;
    }
    case SET_HIGH_RATE: {
      result = ptmeConfig->setRate(RATE_500KBPS);
      break;
    }
    case ARBITRARY_RATE: {
      uint32_t bitrate = 0;
      SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
      result = ptmeConfig->setRate(bitrate);
      break;
    }
    case EN_TRANSMITTER: {
      enableTransmit();
      return EXECUTION_FINISHED;
    }
    case DISABLE_TRANSMITTER: {
      disableTransmit();
      return EXECUTION_FINISHED;
    }
    case ENABLE_TX_CLK_MANIPULATOR: {
      result = ptmeConfig->configTxManipulator(true);
      break;
    }
    case DISABLE_TX_CLK_MANIPULATOR: {
      result = ptmeConfig->configTxManipulator(false);
      break;
    }
    case UPDATE_ON_RISING_EDGE: {
      result = ptmeConfig->invertTxClock(false);
      break;
    }
    case UPDATE_ON_FALLING_EDGE: {
      result = ptmeConfig->invertTxClock(true);
      break;
    }
    default:
      return COMMAND_NOT_IMPLEMENTED;
  }
  if (result != RETURN_OK) {
    return result;
  }
  return EXECUTION_FINISHED;
}

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

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

void CCSDSHandler::handleBitLockEvent() {
  if (transmitterCountdown.isBusy()) {
    // Transmitter already enabled
    return;
  }
  enableTransmit();
}

void CCSDSHandler::handleCarrierLockEvent() {
  if (!enableTxWhenCarrierLock) {
    return;
  }
  enableTransmit();
}

void CCSDSHandler::forwardLinkstate() {
  VirtualChannelMapIter iter;
  for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
    iter->second->setLinkState(linkState);
  }
}

void CCSDSHandler::enableTransmit() {
  if (transmitterCountdown.isBusy()) {
    // Transmitter already enabled
    return;
  }
  transmitterCountdown.setTimeout(TRANSMITTER_TIMEOUT);
#if BOARD_TE0720 == 0
  gpioIF->pullHigh(enTxClock);
  gpioIF->pullHigh(enTxData);
#endif /* BOARD_TE0720 == 0 */
  linkState = UP;
  // Set link state of all virtual channels to link up
  forwardLinkstate();
}

void CCSDSHandler::checkTxTimer() {
  if (linkState == DOWN) {
    return;
  }
  if (transmitterCountdown.hasTimedOut()) {
    disableTransmit();
  }
}

void CCSDSHandler::disableTransmit() {
#if BOARD_TE0720 == 0
  gpioIF->pullLow(enTxClock);
  gpioIF->pullLow(enTxData);
#endif /* BOARD_TE0720 == 0 */
  linkState = DOWN;
  forwardLinkstate();
  transmitterCountdown.setTimeout(0);
}