#include "ComSubsystem.h"

#include <fsfw/ipc/MutexGuard.h>

#include "mission/config/comCfg.h"

ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                           uint32_t maxNumberOfTables)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) {
  com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK);
}

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;
  }
  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;
  }
  return Subsystem::initialize();
}