eive-obsw/mission/devices/HeaterHandler.cpp

#include "HeaterHandler.h"

#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>

#include "devices/gpioIds.h"
#include "devices/powerSwitcherList.h"

HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_,
                             CookieIF* gpioCookie_, object_id_t mainLineSwitcherObjectId_,
                             uint8_t mainLineSwitch_)
    : SystemObject(setObjectId_),
      gpioDriverId(gpioDriverId_),
      gpioCookie(gpioCookie_),
      mainLineSwitcherObjectId(mainLineSwitcherObjectId_),
      mainLineSwitch(mainLineSwitch_),
      actionHelper(this, nullptr) {
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE);
}

HeaterHandler::~HeaterHandler() {}

ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
  if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
    readCommandQueue();
    handleActiveCommands();
    return RETURN_OK;
  }
  return RETURN_OK;
}

ReturnValue_t HeaterHandler::initialize() {
  ReturnValue_t result = SystemObject::initialize();
  if (result != RETURN_OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  result = initializeHeaterMap();
  if (result != RETURN_OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
  if (gpioInterface == nullptr) {
    sif::error << "HeaterHandler::initialize: Invalid Gpio interface." << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
  if (result != RETURN_OK) {
    sif::error << "HeaterHandler::initialize: Failed to initialize Gpio interface" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
  if (IPCStore == nullptr) {
    sif::error << "HeaterHandler::initialize: IPC store not set up in factory." << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
    mainLineSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(mainLineSwitcherObjectId);
    if (mainLineSwitcher == nullptr) {
      sif::error << "HeaterHandler::initialize: Failed to get main line switcher. Make sure "
                 << "main line switcher object is initialized." << std::endl;
      return ObjectManagerIF::CHILD_INIT_FAILED;
    }
  }

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

  return RETURN_OK;
}

ReturnValue_t HeaterHandler::initializeHeaterMap() {
  HeaterCommandInfo_t heaterCommandInfo;
  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
    std::pair status = heaterMap.emplace(switchNr, heaterCommandInfo);
    if (status.second == false) {
      sif::error << "HeaterHandler::initializeHeaterMap: Failed to initialize heater map"
                 << std::endl;
      return RETURN_FAILED;
    }
  }
  return RETURN_OK;
}

void HeaterHandler::setInitialSwitchStates() {
  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
    switchStates[switchNr] = OFF;
  }
}

void HeaterHandler::readCommandQueue() {
  CommandMessage command;
  ReturnValue_t result = commandQueue->receiveMessage(&command);
  if (result != RETURN_OK) {
    return;
  }

  result = actionHelper.handleActionMessage(&command);
  if (result == RETURN_OK) {
    return;
  }
}

ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                           const uint8_t* data, size_t size) {
  ReturnValue_t result;
  if (actionId != SWITCH_HEATER) {
    result = COMMAND_NOT_SUPPORTED;
  } else {
    switchNr_t switchNr = *data;
    HeaterMapIter heaterMapIter = heaterMap.find(switchNr);
    if (heaterMapIter != heaterMap.end()) {
      if (heaterMapIter->second.active) {
        return COMMAND_ALREADY_WAITING;
      }
      heaterMapIter->second.action = *(data + 1);
      heaterMapIter->second.active = true;
      heaterMapIter->second.replyQueue = commandedBy;
    } else {
      sif::error << "HeaterHandler::executeAction: Invalid switchNr" << std::endl;
      return INVALID_SWITCH_NR;
    }
    result = RETURN_OK;
  }
  return result;
}

void HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const {
  ReturnValue_t result;
  store_address_t storeAddress;
  uint8_t commandData[2];

  switch (onOff) {
    case PowerSwitchIF::SWITCH_ON:
      commandData[0] = switchNr;
      commandData[1] = SET_SWITCH_ON;
      break;
    case PowerSwitchIF::SWITCH_OFF:
      commandData[0] = switchNr;
      commandData[1] = SET_SWITCH_OFF;
      break;
    default:
      sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request" << std::endl;
      break;
  }

  result = IPCStore->addData(&storeAddress, commandData, sizeof(commandData));
  if (result == RETURN_OK) {
    CommandMessage message;
    ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress);
    /* Send heater command to own command queue */
    result = commandQueue->sendMessage(commandQueue->getId(), &message, 0);
    if (result != RETURN_OK) {
      sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch"
                 << "message" << std::endl;
    }
  }
}

void HeaterHandler::handleActiveCommands() {
  HeaterMapIter heaterMapIter = heaterMap.begin();
  for (; heaterMapIter != heaterMap.end(); heaterMapIter++) {
    if (heaterMapIter->second.active) {
      switch (heaterMapIter->second.action) {
        case SET_SWITCH_ON:
          handleSwitchOnCommand(heaterMapIter);
          break;
        case SET_SWITCH_OFF:
          handleSwitchOffCommand(heaterMapIter);
          break;
        default:
          sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
                     << std::endl;
          break;
      }
    }
  }
}

void HeaterHandler::handleSwitchOnCommand(HeaterMapIter heaterMapIter) {
  ReturnValue_t result = RETURN_OK;
  switchNr_t switchNr;

  /* Check if command waits for main switch being set on and whether the timeout has expired */
  if (heaterMapIter->second.waitMainSwitchOn &&
      heaterMapIter->second.mainSwitchCountdown.hasTimedOut()) {
    // TODO - This requires the initiation of an FDIR procedure
    triggerEvent(MAIN_SWITCH_TIMEOUT);
    sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
               << std::endl;
    heaterMapIter->second.active = false;
    heaterMapIter->second.waitMainSwitchOn = false;
    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                          MAIN_SWITCH_SET_TIMEOUT);
    }
    return;
  }

  switchNr = heaterMapIter->first;
  /* Check state of main line switch */
  ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
  if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
    if (!checkSwitchState(switchNr)) {
      gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
      result = gpioInterface->pullHigh(gpioId);
      if (result != RETURN_OK) {
        sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id " << gpioId
                   << " high" << std::endl;
        triggerEvent(GPIO_PULL_HIGH_FAILED, result);
      } else {
        switchStates[switchNr] = ON;
      }
    } else {
      triggerEvent(SWITCH_ALREADY_ON, switchNr);
    }
    /* There is no need to send action finish replies if the sender was the
     * HeaterHandler itself. */
    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
      if (result == RETURN_OK) {
        actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                            result);
      } else {
        actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                            result);
      }
    }
    heaterMapIter->second.active = false;
    heaterMapIter->second.waitMainSwitchOn = false;
  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF &&
             heaterMapIter->second.waitMainSwitchOn) {
    /* Just waiting for the main switch being set on */
    return;
  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
    mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
    heaterMapIter->second.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
    heaterMapIter->second.waitMainSwitchOn = true;
  } else {
    sif::debug << "HeaterHandler::handleActiveCommands: Failed to get state of"
               << " main line switch" << std::endl;
    if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                          mainSwitchState);
    }
    heaterMapIter->second.active = false;
  }
}

void HeaterHandler::handleSwitchOffCommand(HeaterMapIter heaterMapIter) {
  ReturnValue_t result = RETURN_OK;
  switchNr_t switchNr = heaterMapIter->first;
  /* Check whether switch is already off */
  if (checkSwitchState(switchNr)) {
    gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
    result = gpioInterface->pullLow(gpioId);
    if (result != RETURN_OK) {
      sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
                 << " low" << std::endl;
      triggerEvent(GPIO_PULL_LOW_FAILED, result);
    } else {
      switchStates[switchNr] = OFF;
      /* When all switches are off, also main line switch will be turned off */
      if (allSwitchesOff()) {
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
      }
    }
  } else {
    sif::info << "HeaterHandler::handleSwitchOffCommand: Switch already off" << std::endl;
    triggerEvent(SWITCH_ALREADY_OFF, switchNr);
  }
  if (heaterMapIter->second.replyQueue != NO_COMMANDER) {
    /* Report back switch command reply if necessary */
    if (result == HasReturnvaluesIF::RETURN_OK) {
      actionHelper.finish(true, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                          result);
    } else {
      actionHelper.finish(false, heaterMapIter->second.replyQueue, heaterMapIter->second.action,
                          result);
    }
  }
  heaterMapIter->second.active = false;
}

bool HeaterHandler::checkSwitchState(int switchNr) { return switchStates[switchNr]; }

bool HeaterHandler::allSwitchesOff() {
  bool allSwitchesOrd = false;
  /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
  for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
    allSwitchesOrd = allSwitchesOrd || switchStates[switchNr];
  }
  return !allSwitchesOrd;
}

gpioId_t HeaterHandler::getGpioIdFromSwitchNr(int switchNr) {
  gpioId_t gpioId = 0xFFFF;
  switch (switchNr) {
    case heaterSwitches::HEATER_0:
      gpioId = gpioIds::HEATER_0;
      break;
    case heaterSwitches::HEATER_1:
      gpioId = gpioIds::HEATER_1;
      break;
    case heaterSwitches::HEATER_2:
      gpioId = gpioIds::HEATER_2;
      break;
    case heaterSwitches::HEATER_3:
      gpioId = gpioIds::HEATER_3;
      break;
    case heaterSwitches::HEATER_4:
      gpioId = gpioIds::HEATER_4;
      break;
    case heaterSwitches::HEATER_5:
      gpioId = gpioIds::HEATER_5;
      break;
    case heaterSwitches::HEATER_6:
      gpioId = gpioIds::HEATER_6;
      break;
    case heaterSwitches::HEATER_7:
      gpioId = gpioIds::HEATER_7;
      break;
    default:
      sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
                 << std::endl;
      break;
  }
  return gpioId;
}

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

void HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) const {}

ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const { return 0; }

ReturnValue_t HeaterHandler::getFuseState(uint8_t fuseNr) const { return 0; }

uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 0; }