#include "AcsSubsystem.h"

#include <fsfw/events/EventManagerIF.h>
#include <fsfw/ipc/QueueFactory.h>

#include "fsfw/modes/ModeMessage.h"
#include "mission/acsDefs.h"

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

ReturnValue_t AcsSubsystem::initialize() {
  EventManagerIF* 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;
    ;
  }
  result =
      manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION));
  if (result != returnvalue::OK) {
    sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl;
  }
  result =
      manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY));
  if (result != returnvalue::OK) {
    sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl;
  }
  return Subsystem::initialize();
}

void AcsSubsystem::performChildOperation() {
  // sif::debug << "ACS system thread running" << std::endl;
  handleEventMessages();
  return Subsystem::performChildOperation();
}

void AcsSubsystem::handleEventMessages() {
  EventMessage event;
  for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
       result = eventQueue->receiveMessage(&event)) {
    ReturnValue_t status;
    switch (event.getMessageId()) {
      case EventMessage::EVENT_MESSAGE:
        if (event.getEvent() == acs::SAFE_RATE_VIOLATION) {
          CommandMessage msg;
          ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, acs::SafeSubmode::DETUMBLE);
          status = commandQueue->sendMessage(commandQueue->getId(), &msg);
          if (result != returnvalue::OK) {
            sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
          }
        }
        if (event.getEvent() == acs::SAFE_RATE_RECOVERY) {
          CommandMessage msg;
          ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT);
          status = commandQueue->sendMessage(commandQueue->getId(), &msg);
          if (status != returnvalue::OK) {
            sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl;
          }
        }
        break;
      default:
        sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
                      "to this event message"
                   << std::endl;
        break;
    }
  }
}

void AcsSubsystem::announceMode(bool recursive) {
  const char* modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
  sif::info << "ACS subsystem is now in " << modeStr << " mode" << std::endl;
  return Subsystem::announceMode(recursive);
}