#include "CcsdsIpCoreHandler.h"

#include <fsfw/subsystem/helper.h>
#include <linux/ipcore/PtmeConfig.h>
#include <mission/config/comCfg.h>
#include <unistd.h>

#include "eive/definitions.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"
#include "mission/com/syrlinksDefs.h"

CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination,
                                       PtmeConfig& ptmeConfig, std::atomic_bool& linkState,
                                       GpioIF* gpioIF, PtmeGpios gpioIds,
                                       std::atomic_bool& ptmeLocked)
    : SystemObject(objectId),
      linkState(linkState),
      ptmeLocked(ptmeLocked),
      tcDestination(tcDestination),
      parameterHelper(this),
      actionHelper(this, nullptr),
      modeHelper(this),
      ptmeConfig(ptmeConfig),
      ptmeGpios(gpioIds),
      gpioIF(gpioIF) {
  commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
  eventQueue =
      QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
  ptmeLocked = true;
}

CcsdsIpCoreHandler::~CcsdsIpCoreHandler() = default;

ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
  readCommandQueue();
  performPtmeUpdateWhenApplicable();
  return returnvalue::OK;
}

ReturnValue_t CcsdsIpCoreHandler::initialize() {
  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();

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

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

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

  result = ptmeConfig.initialize();
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  // This also pulls the PTME out of reset state.
  updateBatPriorityFromParam();
  ptmeConfig.setPollThreshold(
      static_cast<AxiPtmeConfig::IdlePollThreshold>(params.pollThresholdParam));
  resetPtme();
  ptmeLocked = false;

#if OBSW_SYRLINKS_SIMULATED == 1
  // Update data on rising edge
  ptmeConfig.invertTxClock(false);
  linkState = LINK_UP;
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/

  return result;
}

void CcsdsIpCoreHandler::readCommandQueue(void) {
  CommandMessage commandMessage;
  ReturnValue_t result = returnvalue::FAILED;

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

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

ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                               ParameterWrapper* parameterWrapper,
                                               const ParameterWrapper* newValues,
                                               uint16_t startAtIndex) {
  if (domainId != 0) {
    return HasParametersIF::INVALID_DOMAIN_ID;
  }
  if (uniqueIdentifier == ParamId::BAT_PRIORITY) {
    uint8_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    if (newVal > 1) {
      return HasParametersIF::INVALID_VALUE;
    }
    parameterWrapper->set(params.batPriorityParam);
    if (newVal != params.batPriorityParam) {
      // This ensures that the BAT priority is updated at some point when an update of the PTME is
      // allowed
      updateContext.updateBatPrio = true;
      // If we are off, we can do the update after X cycles. Otherwise, wait until the transmitter
      // goes off.
      if (mode == MODE_OFF) {
        initPtmeUpdateAfterXCycles();
      }
    }
    return returnvalue::OK;
  } else if (uniqueIdentifier == ParamId::POLL_THRESHOLD) {
    uint8_t newVal = 0;
    ReturnValue_t result = newValues->getElement(&newVal);
    if (result != returnvalue::OK) {
      return result;
    }
    if (newVal > static_cast<uint8_t>(AxiPtmeConfig::NEVER)) {
      return HasParametersIF::INVALID_VALUE;
    }
    parameterWrapper->set(newVal);
    if (newVal != params.pollThresholdParam) {
      updateContext.updatePollThreshold = true;
      if (mode == MODE_OFF) {
        initPtmeUpdateAfterXCycles();
      }
    }
    return returnvalue::OK;
  }
  return HasParametersIF::INVALID_IDENTIFIER_ID;
}

uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; }

MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const {
  // Forward packets directly to the CCSDS TC distributor
  return tcDistributorQueueId;
}

ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                                const uint8_t* data, size_t size) {
  ReturnValue_t result = returnvalue::OK;
  switch (actionId) {
    case ARBITRARY_RATE: {
      uint32_t bitrate = 0;
      result = SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
      if (result != returnvalue::OK) {
        return result;
      }
      ptmeConfig.setRate(bitrate);
      updateContext.updateClockRate = true;
      if (mode == MODE_OFF) {
        initPtmeUpdateAfterXCycles();
      }
      break;
    }
    case ENABLE_TX_CLK_MANIPULATOR: {
      ptmeConfig.configTxManipulator(true);
      break;
    }
    case DISABLE_TX_CLK_MANIPULATOR: {
      ptmeConfig.configTxManipulator(false);
      break;
    }
    case UPDATE_ON_RISING_EDGE: {
      ptmeConfig.invertTxClock(false);
      break;
    }
    case UPDATE_ON_FALLING_EDGE: {
      ptmeConfig.invertTxClock(true);
      break;
    }
    default:
      return COMMAND_NOT_IMPLEMENTED;
  }
  if (result != returnvalue::OK) {
    return result;
  }
  return EXECUTION_FINISHED;
}

void CcsdsIpCoreHandler::updateLinkState() { linkState = LINK_UP; }

void CcsdsIpCoreHandler::enableTransmit() {
  gpioIF->pullHigh(ptmeGpios.enableTxClock);
  gpioIF->pullHigh(ptmeGpios.enableTxData);
  linkState = LINK_UP;
}

void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
  *mode = this->mode;
  *submode = this->submode;
}

ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submode,
                                                   uint32_t* msToReachTheMode) {
  if (mode == HasModesIF::MODE_ON) {
    if (submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
        submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
        submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
      return HasModesIF::INVALID_SUBMODE;
    }
  } else if (mode != HasModesIF::MODE_OFF) {
    return returnvalue::FAILED;
  }
  *msToReachTheMode = 2000;
  return returnvalue::OK;
}

void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
  triggerEvent(CHANGING_MODE, mode, submode);
  if (mode == HasModesIF::MODE_ON) {
    if (this->submode != submode) {
      initPtmeUpdateAfterXCycles();
      updateContext.enableTransmitAfterPtmeUpdate = true;
      updateContext.updateClockRate = true;
      this->submode = submode;
      this->mode = mode;
      updateContext.setModeAfterUpdate = true;
      return;
    }
    // No rate change, so enable transmitter right away.
    enableTransmit();
  } else if (mode == HasModesIF::MODE_OFF) {
    disableTransmit();
  }
  setMode(mode, submode);
}

void CcsdsIpCoreHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }

void CcsdsIpCoreHandler::disableTransmit() {
#ifndef TE0720_1CFA
  gpioIF->pullLow(ptmeGpios.enableTxClock);
  gpioIF->pullLow(ptmeGpios.enableTxData);
#endif
  linkState = LINK_DOWN;
  // Some parameters need update and transmitter is off now.
  if (updateContext.updateBatPrio or updateContext.updateClockRate) {
    initPtmeUpdateAfterXCycles();
  }
}

const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }

const HasHealthIF* CcsdsIpCoreHandler::getOptHealthIF() const { return nullptr; }

const HasModesIF& CcsdsIpCoreHandler::getModeIF() const { return *this; }

ReturnValue_t CcsdsIpCoreHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
}

ModeTreeChildIF& CcsdsIpCoreHandler::getModeTreeChildIF() { return *this; }

object_id_t CcsdsIpCoreHandler::getObjectId() const { return SystemObject::getObjectId(); }

void CcsdsIpCoreHandler::enablePrioritySelectMode() { ptmeConfig.enableBatPriorityBit(true); }

void CcsdsIpCoreHandler::disablePrioritySelectMode() { ptmeConfig.enableBatPriorityBit(false); }

void CcsdsIpCoreHandler::updateBatPriorityFromParam() {
  if (params.batPriorityParam == 0) {
    disablePrioritySelectMode();
  } else {
    enablePrioritySelectMode();
  }
}

void CcsdsIpCoreHandler::setMode(Mode_t mode, Submode_t submode) {
  this->submode = submode;
  this->mode = mode;
  modeHelper.modeChanged(mode, submode);
  announceMode(false);
}

void CcsdsIpCoreHandler::performPtmeUpdateWhenApplicable() {
  if (not updateContext.performPtmeUpdateAfterXCycles) {
    return;
  }
  if (updateContext.ptmeUpdateCycleCount >= 2) {
    bool doResetPtme = false;
    if (updateContext.updateBatPrio) {
      updateBatPriorityFromParam();
      updateContext.updateBatPrio = false;
      doResetPtme = true;
    }
    if (updateContext.updatePollThreshold) {
      ptmeConfig.setPollThreshold(
          static_cast<AxiPtmeConfig::IdlePollThreshold>(params.pollThresholdParam));
      updateContext.updatePollThreshold = false;
      doResetPtme = true;
    }
    ReturnValue_t result = returnvalue::OK;
    if (updateContext.updateClockRate) {
      if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
        com::Datarate currentDatarate = com::getCurrentDatarate();
        if (currentDatarate == com::Datarate::LOW_RATE_MODULATION_BPSK) {
          result = ptmeConfig.setRate(RATE_100KBPS);
        } else if (currentDatarate == com::Datarate::HIGH_RATE_MODULATION_0QPSK) {
          result = ptmeConfig.setRate(RATE_500KBPS);
        }
      } else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH)) {
        result = ptmeConfig.setRate(RATE_500KBPS);
      } else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW)) {
        result = ptmeConfig.setRate(RATE_100KBPS);
      }
      if (result != returnvalue::OK) {
        sif::error << "CcsdsIpCoreHandler: Setting datarate failed" << std::endl;
      }
      updateContext.updateClockRate = false;
      doResetPtme = true;
    }
    finishPtmeUpdateAfterXCycles(doResetPtme);
    return;
  }
  updateContext.ptmeUpdateCycleCount++;
}

void CcsdsIpCoreHandler::resetPtme() {
  gpioIF->pullLow(ptmeGpios.ptmeResetn);
  usleep(10);
  gpioIF->pullHigh(ptmeGpios.ptmeResetn);
}

void CcsdsIpCoreHandler::initPtmeUpdateAfterXCycles() {
  if (not updateContext.performPtmeUpdateAfterXCycles) {
    updateContext.performPtmeUpdateAfterXCycles = true;
    updateContext.ptmeUpdateCycleCount = 0;
    ptmeLocked = true;
  }
}

void CcsdsIpCoreHandler::finishPtmeUpdateAfterXCycles(bool doResetPtme) {
  if (doResetPtme) {
    resetPtme();
  }
  ptmeLocked = false;
  updateContext.performPtmeUpdateAfterXCycles = false;
  updateContext.ptmeUpdateCycleCount = 0;
  if (updateContext.enableTransmitAfterPtmeUpdate) {
    enableTransmit();
    updateContext.enableTransmitAfterPtmeUpdate = false;
  }
  if (updateContext.setModeAfterUpdate) {
    setMode(mode, submode);
    updateContext.setModeAfterUpdate = false;
  }
}