#include "XiphosWdtHandler.h"

#include "fsfw/ipc/QueueFactory.h"

XiphosWdtHandler::XiphosWdtHandler(object_id_t objectId)
    : SystemObject(objectId),
      requestQueue(QueueFactory::instance()->createMessageQueue()),
      actionHelper(this, requestQueue) {}

ReturnValue_t XiphosWdtHandler::initialize() {
  ReturnValue_t result = actionHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  int retval = xsc_watchdog_init(&wdtHandle);
  if (retval != 0) {
    sif::error << "XiphosWdtHandler: Initiating watchdog failed with code " << retval << ": "
               << strerror(retval) << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  if (wdtHandle == nullptr) {
    sif::error << "XiphosWdtHandler: WDT handle is nullptr!" << std::endl;
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  retval = xsc_watchdog_set_timeout(wdtHandle, timeoutSeconds);
  if (retval != 0) {
    // This propably means that the default timeout is used. Still continue with task init.
    sif::warning << "XiphosWdtHandler: Setting WDT timeout of " << timeoutSeconds
                 << " seconds failed with code " << result << ": " << strerror(retval) << std::endl;
  }
  return enableWdt();
}

ReturnValue_t XiphosWdtHandler::performOperation(uint8_t opCode) {
  CommandMessage command;
  ReturnValue_t result;
  for (result = requestQueue->receiveMessage(&command); result == returnvalue::OK;
       result = requestQueue->receiveMessage(&command)) {
    result = actionHelper.handleActionMessage(&command);
    if (result == returnvalue::OK) {
      continue;
    }
    sif::warning << "Can not handle message with message type " << command.getMessageType()
                 << std::endl;
  }
  int retval = xsc_watchdog_keepalive(wdtHandle);
  if (retval != 0) {
    sif::warning << "XiphosWdtHandler: Feeding WDT failed with code " << retval << ": "
                 << strerror(retval) << std::endl;
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t XiphosWdtHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                              const uint8_t *data, size_t size) {
  switch (actionId) {
    case (ActionId::ENABLE): {
      ReturnValue_t result = enableWdt();
      if (result != returnvalue::OK) {
        return result;
      }
      return EXECUTION_FINISHED;
    }
    case (ActionId::DISABLE): {
      ReturnValue_t result = disableWdt();
      if (result != returnvalue::OK) {
        return result;
      }
      return EXECUTION_FINISHED;
    }
  }
  return HasActionsIF::INVALID_ACTION_ID;
}

ReturnValue_t XiphosWdtHandler::enableWdt() {
  int nowayout = 0;
  int status = 0;
  int retval = xsc_watchdog_get_status(&nowayout, &status);
  // If this fails for whatever reason, just try enabling in any case.
  if (retval != 0) {
    sif::warning << "XiphosWdtHandler: Getting WDT status failed" << std::endl;
  }
  // Of course the enable API will fail if the device is already on, just perfect, love me some
  // good C API... :)))
  if (retval != 0 or status == 0) {
    int retval = xsc_watchdog_enable(wdtHandle);
    if (retval != 0) {
      sif::error << "XiphosWdtHandler: Enabling WDT failed with code " << retval << ": "
                 << strerror(retval) << std::endl;
      return returnvalue::FAILED;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t XiphosWdtHandler::disableWdt() {
  int nowayout = 0;
  int status = 0;
  int retval = xsc_watchdog_get_status(&nowayout, &status);
  // If this fails for whatever reason, just try disabling in any case.
  if (retval != 0) {
    sif::warning << "XiphosWdtHandler: Getting WDT status failed" << std::endl;
  }
  // Of course the disable API will fail if the device is already off, just perfect, love me some
  // good C API... :)))
  if (retval != 0 or status == 1) {
    int retval = xsc_watchdog_disable(wdtHandle);
    if (retval != 0) {
      sif::error << "XiphosWdtHandler: Disabling WDT failed with code " << retval << ": "
                 << strerror(retval) << std::endl;
      return returnvalue::FAILED;
    }
  }
  return returnvalue::OK;
}