#include <fsfw/health/HealthTableIF.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <mission/tcs/HeaterHandler.h>

#include <stdexcept>

#include "devices/powerSwitcherList.h"
#include "fsfw/subsystem/helper.h"

HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
                             PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
    : SystemObject(setObjectId_),
      helper(helper),
      modeHelper(this),
      gpioInterface(gpioInterface_),
      mainLineSwitcher(mainLineSwitcher_),
      mainLineSwitch(mainLineSwitch_),
      actionHelper(this, nullptr) {
  for (const auto& heater : helper.heaters) {
    if (heater.first == nullptr) {
      throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Heater Object");
    }
  }
  if (gpioInterface_ == nullptr) {
    throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid Gpio IF");
  }
  if (mainLineSwitcher == nullptr) {
    throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
  }
  handlerLock = MutexFactory::instance()->createMutex();
  if (handlerLock == nullptr) {
    throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
  }
  auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

HeaterHandler::~HeaterHandler() = default;

ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
  try {
    readCommandQueue();
    for (const auto& heater : helper.heaters) {
      heater.first->performOperation(0);
    }
    handleSwitchHandling();
    if (waitForSwitchOff) {
      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
        waitForSwitchOff = false;
        mode = MODE_OFF;
        busyWithSwitchCommanding = false;
        modeHelper.modeChanged(mode, submode);
      }
    }
    if (busyWithSwitchCommanding and heaterCmdBusyCd.hasTimedOut()) {
      busyWithSwitchCommanding = false;
    }
  } catch (const std::out_of_range& e) {
    sif::warning << "HeaterHandler::performOperation: "
                    "Out of range error | "
                 << e.what() << std::endl;
  }
  return returnvalue::OK;
}

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

  result = initializeHeaterMap();
  if (result != returnvalue::OK) {
    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;
  }

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

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

ReturnValue_t HeaterHandler::initializeHeaterMap() {
  for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
    heaterVec.push_back(HeaterWrapper(helper.heaters[switchNr], SwitchState::OFF));
  }
  return returnvalue::OK;
}

void HeaterHandler::readCommandQueue() {
  ReturnValue_t result = returnvalue::OK;
  CommandMessage command;
  if (not busyWithSwitchCommanding) {
    result = commandQueue->receiveMessage(&command);
    if (result == MessageQueueIF::EMPTY) {
      return;
    } else if (result != returnvalue::OK) {
      sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
      return;
    }
    result = modeHelper.handleModeCommand(&command);
    if (result == returnvalue::OK) {
      return;
    }
    result = actionHelper.handleActionMessage(&command);
    if (result == returnvalue::OK) {
      return;
    }
  }
}

ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                           const uint8_t* data, size_t size) {
  if (data == nullptr or size < 3) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  if (actionId != SWITCH_HEATER) {
    return COMMAND_NOT_SUPPORTED;
  }
  auto switchNr = data[0];
  if (switchNr > 7) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  auto& heater = heaterVec.at(switchNr);

  auto actionRaw = data[1];
  if (actionRaw != 0 and actionRaw != 1) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  auto action = static_cast<SwitchAction>(data[1]);
  // Always accepts OFF commands
  if (action == SwitchAction::SET_SWITCH_ON) {
    HasHealthIF::HealthState health;
    {
      MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
      health = heater.healthDevice->getHealth();
    }
    if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
        health == HasHealthIF::NEEDS_RECOVERY) {
      return HasHealthIF::OBJECT_NOT_HEALTHY;
    }
    CmdSourceParam cmdSource = CmdSourceParam::EXTERNAL;
    uint8_t cmdSourceRaw = data[2];
    if (cmdSourceRaw > 1) {
      return HasActionsIF::INVALID_PARAMETERS;
    }
    cmdSource = static_cast<CmdSourceParam>(data[2]);
    if (health == HasHealthIF::EXTERNAL_CONTROL and cmdSource == CmdSourceParam::INTERNAL) {
      return HasHealthIF::IS_EXTERNALLY_CONTROLLED;
    }
  }

  if (heater.cmdActive) {
    return COMMAND_ALREADY_WAITING;
  }
  heater.action = action;
  heater.cmdActive = true;
  heater.replyQueue = commandedBy;
  busyWithSwitchCommanding = true;
  heaterCmdBusyCd.resetTimer();
  return returnvalue::OK;
}

ReturnValue_t HeaterHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) {
  ReturnValue_t result;
  store_address_t storeAddress;
  uint8_t commandData[3] = {};

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

  result = ipcStore->addData(&storeAddress, commandData, sizeof(commandData));
  if (result == returnvalue::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 != returnvalue::OK) {
      sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch"
                 << "message" << std::endl;
    }
  }
  return result;
}

void HeaterHandler::handleSwitchHandling() {
  for (uint8_t idx = 0; idx < heater::NUMBER_OF_SWITCHES; idx++) {
    auto health = heaterVec[idx].healthDevice->getHealth();
    if (heaterVec[idx].switchState == SwitchState::ON) {
      // If a heater is still on but the device was marked faulty by the operator, the SW
      // will shut down the heater
      if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY) {
        heaterVec[idx].cmdActive = true;
        heaterVec[idx].action = SET_SWITCH_OFF;
        triggerEvent(FAULTY_HEATER_WAS_ON, idx, 0);
        handleSwitchOffCommand(static_cast<heater::Switch>(idx));
        continue;
      }
    }
    if (heaterVec[idx].cmdActive) {
      switch (heaterVec[idx].action) {
        case SET_SWITCH_ON:
          handleSwitchOnCommand(static_cast<heater::Switch>(idx));
          break;
        case SET_SWITCH_OFF:
          handleSwitchOffCommand(static_cast<heater::Switch>(idx));
          break;
        default:
          sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
                     << std::endl;
          break;
      }
    }
  }
}

void HeaterHandler::handleSwitchOnCommand(heater::Switch heaterIdx) {
  ReturnValue_t result = returnvalue::OK;
  auto& heater = heaterVec.at(heaterIdx);
  if (waitForSwitchOff) {
    waitForSwitchOff = false;
  }
  /* Check if command waits for main switch being set on and whether the timeout has expired */
  if (heater.waitMainSwitchOn && heater.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;
    heater.cmdActive = false;
    busyWithSwitchCommanding = false;
    heater.waitMainSwitchOn = false;
    if (heater.replyQueue != commandQueue->getId()) {
      actionHelper.finish(false, heater.replyQueue, heater.action, MAIN_SWITCH_SET_TIMEOUT);
    }
    return;
  }

  // Check state of main line switch
  ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
  if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
    gpioId_t gpioId = heater.gpioId;
    result = gpioInterface->pullHigh(gpioId);
    if (result != returnvalue::OK) {
      sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull GPIO with ID " << gpioId
                 << " high" << std::endl;
      triggerEvent(GPIO_PULL_HIGH_FAILED, result);
    }
    if (result == returnvalue::OK) {
      triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
      {
        MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
        heater.switchState = ON;
      }
      EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
                                   MODE_ON, 0);
      busyWithSwitchCommanding = false;
      mode = HasModesIF::MODE_ON;
      modeHelper.modeChanged(mode, submode);
    }
    // There is no need to send action finish replies if the sender was the
    // HeaterHandler itself
    if (heater.replyQueue != commandQueue->getId()) {
      if (result == returnvalue::OK) {
        actionHelper.finish(true, heater.replyQueue, heater.action, result);
      } else {
        actionHelper.finish(false, heater.replyQueue, heater.action, result);
      }
    }
    heater.cmdActive = false;
    heater.waitMainSwitchOn = false;
  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF && heater.waitMainSwitchOn) {
    // Just waiting for the main switch being set on
    return;
  } else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF or
             mainSwitchState == PowerSwitchIF::SWITCH_UNKNOWN) {
    mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
    heater.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
    heater.waitMainSwitchOn = true;
  } else {
    sif::debug << "HeaterHandler::handleSwitchOnCommand: Failed to get state of"
               << " main line switch" << std::endl;
    if (heater.replyQueue != commandQueue->getId()) {
      actionHelper.finish(false, heater.replyQueue, heater.action, mainSwitchState);
    }
    heater.cmdActive = false;
  }
}

void HeaterHandler::handleSwitchOffCommand(heater::Switch heaterIdx) {
  ReturnValue_t result = returnvalue::OK;
  auto& heater = heaterVec.at(heaterIdx);
  gpioId_t gpioId = heater.gpioId;
  result = gpioInterface->pullLow(gpioId);
  if (result != returnvalue::OK) {
    sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id" << gpioId
               << " low" << std::endl;
    triggerEvent(GPIO_PULL_LOW_FAILED, result);
  }
  if (result == returnvalue::OK) {
    // Check whether switch is already off
    if (getSwitchState(heaterIdx) == SwitchState::ON) {
      {
        MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
        heater.switchState = OFF;
      }
      triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
    } else {
      triggerEvent(SWITCH_ALREADY_OFF, heaterIdx);
    }
    EventManagerIF::triggerEvent(helper.heaters[heaterIdx].first->getObjectId(), MODE_INFO,
                                 MODE_OFF, 0);
    // When all switches are off, also main line switch will be turned off
    if (allSwitchesOff()) {
      mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
      waitForSwitchOff = true;
    } else {
      busyWithSwitchCommanding = false;
    }
  }
  if (heater.replyQueue != NO_COMMANDER) {
    // Report back switch command reply if necessary
    if (result == returnvalue::OK) {
      actionHelper.finish(true, heater.replyQueue, heater.action, result);
    } else {
      actionHelper.finish(false, heater.replyQueue, heater.action, result);
    }
  }
  heater.cmdActive = false;
}

HeaterHandler::SwitchState HeaterHandler::getSwitchState(heater::Switch switchNr) const {
  MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
  return heaterVec.at(switchNr).switchState;
}

ReturnValue_t HeaterHandler::switchHeater(heater::Switch heater, SwitchState switchState) {
  if (switchState == SwitchState::ON) {
    return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_ON);
  } else if (switchState == SwitchState::OFF) {
    return sendSwitchCommand(heater, PowerSwitchIF::SWITCH_OFF);
  }
  return returnvalue::FAILED;
}

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

  std::array<SwitchState, 8> states;
  getAllSwitchStates(states);
  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
    if (states[idx] == ON) {
      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
    } else {
      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
                                   0);
    }
  }
}

void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
  if (!mode || !submode) {
    return;
  }
  *mode = this->mode;
  *submode = this->submode;
}

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

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

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

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

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

ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
  {
    MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    if (mg.getLockResult() != returnvalue::OK) {
      return returnvalue::FAILED;
    }
    for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
      statesBuf[idx] = heaterVec[idx].switchState;
    }
  }
  return returnvalue::OK;
}

bool HeaterHandler::allSwitchesOff() {
  bool allSwitchesOrd = false;
  MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
  /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
  for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
    allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
  }
  return !allSwitchesOrd;
}

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

ReturnValue_t HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) { return returnvalue::OK; }

ReturnValue_t HeaterHandler::getSwitchState(uint8_t switchNr) const {
  ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
  if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
    return PowerSwitchIF::SWITCH_OFF;
  }
  if (switchNr > 7) {
    return returnvalue::FAILED;
  }
  if (getSwitchState(static_cast<heater::Switch>(switchNr)) == SwitchState::ON) {
    return PowerSwitchIF::SWITCH_ON;
  }
  return PowerSwitchIF::SWITCH_OFF;
}

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

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

HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switch heater) {
  auto* healthDev = heaterVec.at(heater).healthDevice;
  if (healthDev != nullptr) {
    MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    return healthDev->getHealth();
  }
  return HasHealthIF::HealthState::FAULTY;
}