#include "EiveSystem.h"

#include <eive/objects.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <mission/acsDefs.h>
#include <mission/comDefs.h>
#include <mission/controller/controllerdefinitions/tcsCtrlDefs.h>

EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                       uint32_t maxNumberOfTables)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {
  auto mqArgs = MqArgs(SubsystemBase::getObjectId(), static_cast<void*>(this));
  eventQueue =
      QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}

void EiveSystem::announceMode(bool recursive) {
  const char* modeStr = "UNKNOWN";
  switch (mode) {
    case (acs::AcsMode::OFF): {
      modeStr = "OFF";
      break;
    }
    case (acs::AcsMode::SAFE): {
      modeStr = "SAFE";
      break;
    }
    case (acs::AcsMode::PTG_IDLE): {
      modeStr = "POINTING IDLE";
      break;
    }
    case (acs::AcsMode::PTG_INERTIAL): {
      modeStr = "POINTING INERTIAL";
      break;
    }
    case (acs::AcsMode::PTG_TARGET): {
      modeStr = "POINTING TARGET";
      break;
    }
    case (acs::AcsMode::PTG_TARGET_GS): {
      modeStr = "POINTING TARGET GS";
      break;
    }
  }
  sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
  return Subsystem::announceMode(recursive);
}

void EiveSystem::performChildOperation() {
  handleEventMessages();
  return Subsystem::performChildOperation();
}

ReturnValue_t EiveSystem::initialize() {
  auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  ReturnValue_t result = manager->registerListener(eventQueue->getId());
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "AcsSubsystem::registerListener: Failed to register as "
                    "listener"
                 << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::SYRLINKS_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(),
                            event::getEventId(tcsCtrl::PCDU_SYSTEM_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));

  return Subsystem::initialize();
}

void EiveSystem::handleEventMessages() {
  EventMessage event;
  for (ReturnValue_t status = eventQueue->receiveMessage(&event); status == returnvalue::OK;
       status = eventQueue->receiveMessage(&event)) {
    switch (event.getMessageId()) {
      case EventMessage::EVENT_MESSAGE:
        switch (event.getEvent()) {
          case tcsCtrl::SYRLINKS_OVERHEATING: {
            CommandMessage msg;
            ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND_FORCED, com::RX_ONLY, 0);
            ReturnValue_t result =
                commandQueue->sendMessage(childrenMap[objects::COM_SUBSYSTEM].commandQueue, &msg);
            if (result != returnvalue::OK) {
              sif::error << "EiveSystem: Commanding COM to RX_ONLY failed" << std::endl;
            }
            break;
          }
          case tcsCtrl::OBC_OVERHEATING:
          case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
            commandSelfToSafe();
            break;
          }
        }
        break;
      default:
        sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
                      "to this event message"
                   << std::endl;
        break;
    }
  }
}
void EiveSystem::commandSelfToSafe() {
  if (fallbackCommandCd.hasTimedOut()) {
    CommandMessage msg;
    ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
    ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
    if (result != returnvalue::OK) {
      sif::error << "EiveSystem: Commanding EIVE-System to SAFE failed" << std::endl;
    }
    fallbackCommandCd.resetTimer();
  }
}