#include "EiveSystem.h"

#include <eive/objects.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/tasks/TaskFactory.h>
#include <mission/acs/defs.h>
#include <mission/com/defs.h>
#include <mission/controller/tcsDefs.h>

#include "linux/ipcore/pdec.h"
#include "mission/power/bpxBattDefs.h"
#include "mission/power/defs.h"
#include "mission/sysDefs.h"

EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                       uint32_t maxNumberOfTables, std::atomic_uint16_t& i2cErrors)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables),
      actionHelper(this, commandQueue),
      i2cErrors(i2cErrors) {
  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 (satsystem::Mode::BOOT): {
      modeStr = "OFF/BOOT";
      break;
    }
    case (satsystem::Mode::SAFE): {
      modeStr = "SAFE";
      break;
    }
    case (satsystem::Mode::PTG_IDLE): {
      modeStr = "POINTING IDLE";
      break;
    }
    case (satsystem::Mode::PTG_NADIR): {
      modeStr = "POINTING NADIR";
      break;
    }
    case (satsystem::Mode::PTG_TARGET): {
      modeStr = "POINTING TARGET";
      break;
    }
    case (satsystem::Mode::PTG_TARGET_GS): {
      modeStr = "POINTING TARGET GS";
      break;
    }
    case (satsystem::Mode::PTG_INERTIAL): {
      modeStr = "POINTING INERTIAL";
      break;
    }
  }
  sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
  return Subsystem::announceMode(recursive);
}

void EiveSystem::performChildOperation() {
  Subsystem::performChildOperation();
  handleEventMessages();
  if (not isInTransition and performSafeRecovery) {
    commandSelfToSafe();
    performSafeRecovery = false;
    return;
  }
  pdecRecoveryLogic();
  i2cRecoveryLogic();
}

ReturnValue_t EiveSystem::initialize() {
  if (powerSwitcher == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  ReturnValue_t result = actionHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }

  auto* plSs = ObjectManager::instance()->get<HasModesIF>(objects::PL_SUBSYSTEM);
  if (plSs == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  plSsQueueId = plSs->getCommandQueue();

  auto* plPcdu = ObjectManager::instance()->get<HasHealthIF>(objects::PLPCDU_HANDLER);
  if (plPcdu == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  plPcduQueueId = plPcdu->getCommandQueue();

  auto* plocMpsoc = ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_MPSOC_HANDLER);
  if (plocMpsoc == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  plocMpsocQueueId = plocMpsoc->getCommandQueue();

  auto* plocSupervisor =
      ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_SUPERVISOR_HANDLER);
  if (plocSupervisor == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  plocSupervisorQueueId = plocSupervisor->getCommandQueue();

  auto* camera = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
  if (camera == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  cameraQueueId = camera->getCommandQueue();

  auto* scex = ObjectManager::instance()->get<HasHealthIF>(objects::SCEX);
  if (scex == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  scexQueueId = scex->getCommandQueue();

  auto* radSensor = ObjectManager::instance()->get<HasHealthIF>(objects::RAD_SENSOR);
  if (radSensor == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  radSensorQueueId = radSensor->getCommandQueue();

  auto* str = ObjectManager::instance()->get<HasHealthIF>(objects::STAR_TRACKER);
  if (str == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  strQueueId = str->getCommandQueue();

  auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
  if (bpxDest == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  bpxBattQueueId = bpxDest->getCommandQueue();

  auto* coreCtrl = ObjectManager::instance()->get<HasActionsIF>(objects::CORE_CONTROLLER);
  if (coreCtrl == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  coreCtrlQueueId = coreCtrl->getCommandQueue();

  auto* pdecHandler = ObjectManager::instance()->get<HasActionsIF>(objects::PDEC_HANDLER);
  if (pdecHandler == nullptr) {
    return ObjectManager::CHILD_INIT_FAILED;
  }
  pdecHandlerQueueId = pdecHandler->getCommandQueue();

  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;
  }
  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::PCDU_SYSTEM_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
  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 pdec::INVALID_TC_FRAME: {
            if (event.getParameter1() == pdec::FRAME_DIRTY_RETVAL) {
              frameDirtyErrorCounter++;
              // Check whether threshold was reached after 10 seconds.
              if (frameDirtyErrorCounter == 1) {
                frameDirtyCheckCd.resetTimer();
              }
            }
            break;
          }
          case tcsCtrl::OBC_OVERHEATING:
          case tcsCtrl::MGT_OVERHEATING:
          case tcsCtrl::PCDU_SYSTEM_OVERHEATING: {
            if (isInTransition) {
              performSafeRecovery = true;
              return;
            }

            commandSelfToSafe();
            break;
          }
          case power::POWER_LEVEL_LOW: {
            forceOffPayload();
            break;
          }
          case power::POWER_LEVEL_CRITICAL:
            CommandMessage msg;
            ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND_FORCED,
                                        HasModesIF::MODE_OFF, HasModesIF::SUBMODE_NONE);
            HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
            ReturnValue_t result = MessageQueueSenderIF::sendMessage(
                strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
            if (result != returnvalue::OK) {
              sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
                         << std::endl;
            }
            break;
        }
        break;
      default:
        sif::debug << "EiveSystem: Did not subscribe to event " << event.getEvent() << std::endl;
        break;
    }
  }
}

MessageQueueId_t EiveSystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }

ReturnValue_t EiveSystem::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                        const uint8_t* data, size_t size) {
  switch (actionId) {
    case (EXECUTE_I2C_REBOOT): {
      triggerEvent(core::TRYING_I2C_RECOVERY);
      performI2cReboot = true;
      i2cRebootState = I2cRebootState::SYSTEM_MODE_BOOT;
      this->actionCommandedBy = commandedBy;
      return returnvalue::OK;
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
  return returnvalue::OK;
}

void EiveSystem::setI2cRecoveryParams(PowerSwitchIF* pwrSwitcher) {
  this->powerSwitcher = pwrSwitcher;
}

void EiveSystem::i2cRecoveryLogic() {
  ReturnValue_t result;
  if (not performI2cReboot) {
    // If a recovery worked, need to reset these flags and the error count after some time.
    if (i2cRecoveryClearCountdown.hasTimedOut()) {
      i2cErrors = 0;
      alreadyTriedI2cRecovery = false;
      i2cRebootHandlingCountdown.resetTimer();
    }
    // If an I2C recovery is not ongoing and the I2C error counter is above a threshold, try
    // recovery or reboot if recovery was already attempted.
    if (i2cErrors >= 5) {
      if (not alreadyTriedI2cRecovery) {
        // Try recovery.
        executeAction(EXECUTE_I2C_REBOOT, MessageQueueIF::NO_QUEUE, nullptr, 0);
      } else {
        if (waitingForI2cReboot) {
          return;
        }
        triggerEvent(core::I2C_REBOOT);
        // Some delay to ensure that the event is stored in the persistent TM store as well.
        TaskFactory::delayTask(500);
        // We already tried an I2C recovery but the bus is still broken.
        // Send reboot request to core controller.
        result = sendSelfRebootCommand();
        if (result != returnvalue::OK) {
          sif::error << "Sending a reboot command has failed" << std::endl;
          // If the previous operation failed, it should be re-attempted the next task cycle.
          return;
        }
        waitingForI2cReboot = true;
        return;
      }
    }
  }
  if (not isInTransition and performI2cReboot) {
    switch (i2cRebootState) {
      case (I2cRebootState::NONE): {
        break;
      }
      case (I2cRebootState::SYSTEM_MODE_BOOT): {
        startTransition(satsystem::Mode::BOOT, 0);
        i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT;
        i2cRebootHandlingCountdown.resetTimer();
        break;
      }
      case (I2cRebootState::SWITCH_3V3_STACK_OFF_AND_BATT_REBOOT): {
        if (mode == satsystem::Mode::BOOT) {
          result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                                    PowerSwitchIF::SWITCH_OFF);
          if (result != returnvalue::OK) {
            actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
            commonI2cRecoverySequenceFinish();
            return;
          }
          CommandMessage msg;
          store_address_t dummy{};
          ActionMessage::setCommand(&msg, bpxBat::REBOOT, dummy);
          result = commandQueue->sendMessage(bpxBattQueueId, &msg);
          if (result != returnvalue::OK) {
            actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
            commonI2cRecoverySequenceFinish();
            return;
          }
          i2cRebootState = I2cRebootState::WAIT_CYCLE;
        }
        break;
      }
      case (I2cRebootState::WAIT_CYCLE): {
        i2cRebootState = I2cRebootState::SWITCH_3V3_STACK_ON;
        break;
      }
      case (I2cRebootState::SWITCH_3V3_STACK_ON): {
        result = powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                                  PowerSwitchIF::SWITCH_ON);
        if (result != returnvalue::OK) {
          actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, result);
          commonI2cRecoverySequenceFinish();
          return;
        }
        i2cRebootState = I2cRebootState::SYSTEM_MODE_SAFE;
        break;
      }
      case (I2cRebootState::SYSTEM_MODE_SAFE): {
        if (powerSwitcher->getSwitchState(power::Switches::P60_DOCK_3V3_STACK) ==
            PowerSwitchIF::SWITCH_ON) {
          // This should always be accepted
          commonI2cRecoverySequenceFinish();
          actionHelper.finish(true, actionCommandedBy, EXECUTE_I2C_REBOOT);
        }
        break;
      }
      default: {
        sif::error << "EiveSystem: Unexpected I2C reboot state" << std::endl;
        break;
      }
    }
    // Timeout handling for the internal procedure.
    if (i2cRebootState != I2cRebootState::NONE and i2cRebootHandlingCountdown.hasTimedOut()) {
      actionHelper.finish(false, actionCommandedBy, EXECUTE_I2C_REBOOT, returnvalue::FAILED);
      // Command stack back on in any case.
      powerSwitcher->sendSwitchCommand(power::Switches::P60_DOCK_3V3_STACK,
                                       PowerSwitchIF::SWITCH_ON);
      commonI2cRecoverySequenceFinish();
    }
  }
}

void EiveSystem::commandSelfToSafe() { startTransition(satsystem::Mode::SAFE, 0); }

ReturnValue_t EiveSystem::sendFullRebootCommand() {
  CommandMessage msg;
  ActionMessage::setCommand(&msg, core::REBOOT_OBC, store_address_t());
  return commandQueue->sendMessage(coreCtrlQueueId, &msg);
}

void EiveSystem::pdecRecoveryLogic() {
  // PDEC reset has happened too often in the last time. Perform reboot to same image.
  if (pdecResetCounter >= PDEC_RESET_MAX_COUNT_BEFORE_REBOOT) {
    if (waitingForPdecReboot) {
      return;
    }
    triggerEvent(core::PDEC_REBOOT);
    // Some delay to ensure that the event is stored in the persistent TM store as well.
    TaskFactory::delayTask(500);
    // Send reboot command.
    ReturnValue_t result = sendSelfRebootCommand();
    if (result != returnvalue::OK) {
      sif::error << "Sending a reboot command has failed" << std::endl;
      // If the previous operation failed, it should be re-attempted the next task cycle.
      pdecResetCounterResetCd.resetTimer();
      return;
    }
    waitingForPdecReboot = true;
    return;
  }
  if (pdecResetCounterResetCd.hasTimedOut()) {
    pdecResetCounter = 0;
  }
  if (frameDirtyCheckCd.hasTimedOut() and frameDirtyErrorCounter > 0) {
    if (frameDirtyErrorCounter >= FRAME_DIRTY_COM_REBOOT_LIMIT) {
      // Try one full PDEC reset.
      CommandMessage msg;
      store_address_t dummy{};
      ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
      commandQueue->sendMessage(pdecHandlerQueueId, &msg);
      pdecResetCounterResetCd.resetTimer();
      pdecResetCounter++;
    }
    frameDirtyErrorCounter = 0;
  }
}

void EiveSystem::forceOffPayload() {
  CommandMessage msg;
  // turn off PL Subsystem
  ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND_FORCED, HasModesIF::MODE_OFF,
                              HasModesIF::SUBMODE_NONE);
  ReturnValue_t result = commandQueue->sendMessage(plSsQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending OFF command to PL Subsystem failed" << std::endl;
  }
  // set PL to faulty
  HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);

  result = commandQueue->sendMessage(plPcduQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to PL PCDU failed" << std::endl;
  }

  result = commandQueue->sendMessage(plocMpsocQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to PLOC MPSOC failed" << std::endl;
  }

  result = commandQueue->sendMessage(plocSupervisorQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to PLOC SUPERVISOR failed" << std::endl;
  }

  result = commandQueue->sendMessage(cameraQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to PL CAM failed" << std::endl;
  }

  result = commandQueue->sendMessage(scexQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to SCEX failed" << std::endl;
  }

  result = commandQueue->sendMessage(radSensorQueueId, &msg);
  if (result != returnvalue::OK) {
    sif::error << "EIVE System: Sending FAULTY command to RAD SENSOR failed" << std::endl;
  }
}

void EiveSystem::commonI2cRecoverySequenceFinish() {
  alreadyTriedI2cRecovery = true;
  performI2cReboot = false;
  i2cRebootState = I2cRebootState::NONE;
  // Reset this counter and the recovery clear countdown. If I2C devices are still problematic,
  // we will get a full reboot next time this count goes above 5.
  i2cErrors = 0;
  i2cRecoveryClearCountdown.resetTimer();
  // This should always be accepted
  commandSelfToSafe();
}

ReturnValue_t EiveSystem::handleCommandMessage(CommandMessage* message) {
  if (message->getMessageType() == messagetypes::ACTION) {
    return actionHelper.handleActionMessage(message);
  }
  return Subsystem::handleCommandMessage(message);
}

ReturnValue_t EiveSystem::sendSelfRebootCommand() {
  CommandMessage msg;
  uint8_t data[1];
  // This option is used to target the same image.
  data[0] = true;
  store_address_t storeId;
  ReturnValue_t result = IPCStore->addData(&storeId, data, sizeof(data));
  if (result != returnvalue::OK) {
    return result;
  }
  ActionMessage::setCommand(&msg, core::XSC_REBOOT_OBC, storeId);
  return commandQueue->sendMessage(coreCtrlQueueId, &msg);
}