#include "DualLaneAssemblyBase.h"

#include <fsfw/ipc/QueueFactory.h>

#include "OBSWConfig.h"

DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
                                           power::Switches switch1, power::Switches switch2,
                                           Event pwrTimeoutEvent, Event sideSwitchNotAllowedEvent,
                                           Event transitionOtherSideFailedEvent)
    : AssemblyBase(objectId, 20),
      pwrStateMachine(switch1, switch2, pwrSwitcher),
      pwrTimeoutEvent(pwrTimeoutEvent),
      sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent),
      transitionOtherSideFailedEvent(transitionOtherSideFailedEvent) {
  eventQueue = QueueFactory::instance()->createMessageQueue(10);
}

void DualLaneAssemblyBase::performChildOperation() {
  using namespace duallane;
  if (pwrStateMachine.active()) {
    ReturnValue_t result = pwrStateMachineWrapper();
    if (result != returnvalue::OK) {
      handleModeTransitionFailed(result);
    }
  }
  // Only perform the regular child operation if the power state machine is not active.
  // It does not make any sense to command device modes while the power switcher is busy
  // switching off or on devices.
  if (not pwrStateMachine.active()) {
    AssemblyBase::performChildOperation();
    // TODO: Handle Event Queue
  }
}

void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  pwrStateMachine.reset();
  dualToSingleSideTransition = false;
  sideSwitchState = SideSwitchState::NONE;

  if (mode != MODE_OFF) {
    // Special exception: A transition from dual side to single mode must be handled like
    // going OFF.
    if ((this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) and
        this->submode == DUAL_MODE and submode != DUAL_MODE) {
      dualToSingleSideTransition = true;
      AssemblyBase::startTransition(mode, submode);
      return;
    }
    if (sideSwitchState == SideSwitchState::NONE and sideSwitchTransition(mode, submode)) {
      sideSwitchState = SideSwitchState::REQUESTED;
    }
    uint8_t pwrSubmode = submode;
    if (sideSwitchState == SideSwitchState::REQUESTED) {
      pwrSubmode = duallane::DUAL_MODE;
    }
    // If anything other than MODE_OFF is commanded, perform power state machine first
    // Cache the target modes, required by power state machine
    pwrStateMachine.start(mode, pwrSubmode);
    // Cache these for later after the power state machine has finished
    targetMode = mode;
    targetSubmode = submode;
  } else {
    AssemblyBase::startTransition(mode, submode);
  }
}

bool DualLaneAssemblyBase::isModeCommandable(object_id_t object, Mode_t mode) {
  if (healthHelper.healthTable->isFaulty(object)) {
    return false;
  }

  // Check if device is already in target mode
  if (childrenMap[object].mode == mode) {
    return true;
  }
  // Check for external control health state is done by base class.
  return true;
}

ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
  using namespace power;
  OpCodes opCode = pwrStateMachine.fsm();
  if (customRecoveryStates == RecoveryCustomStates::IDLE) {
    if (opCode == OpCodes::NONE) {
      return returnvalue::OK;
    } else if (opCode == OpCodes::TO_OFF_DONE) {
      // Will be called for transitions to MODE_OFF, where everything is done after power switching
      finishModeOp();
    } else if (opCode == OpCodes::TO_NOT_OFF_DONE) {
      if (dualToSingleSideTransition) {
        finishModeOp();
      } else {
        // Will be called for transitions from MODE_OFF to anything else, where the mode still has
        // to be commanded after power switching
        AssemblyBase::startTransition(targetMode, targetSubmode);
      }
    } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
      if (powerRetryCounter == 0) {
        powerRetryCounter++;
        pwrStateMachine.reset();
      } else {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning << "Timeout occured in power state machine" << std::endl;
#endif
        triggerEvent(pwrTimeoutEvent, 0, 0);
        return returnvalue::FAILED;
      }
    }
  }
  return returnvalue::OK;
}

ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  if (mode != MODE_OFF and (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE)) {
    return HasModesIF::INVALID_SUBMODE;
  }
  if(mode == MODE_OFF and submode != SUBMODE_NONE) {
    return HasModesIF::INVALID_SUBMODE;
  }
  return returnvalue::OK;
}

void DualLaneAssemblyBase::handleModeReached() {
  using namespace duallane;
  if (targetMode == MODE_OFF) {
    pwrStateMachine.start(targetMode, targetSubmode);
    // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function
    // will be called
    // Ignore failures for now.
    pwrStateMachineWrapper();
  } else {
    // For dual to single side transition, devices should be logically off, but the switch
    // handling still needs to be done.
    if (dualToSingleSideTransition) {
      if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
        pwrStateMachine.start(targetMode, targetSubmodeForSideSwitch);
      } else {
        pwrStateMachine.start(targetMode, targetSubmode);
      }
      pwrStateMachineWrapper();
      return;
    }
    finishModeOp();
  }
}

MessageQueueId_t DualLaneAssemblyBase::getEventReceptionQueue() { return eventQueue->getId(); }

void DualLaneAssemblyBase::handleChildrenLostMode(ReturnValue_t result) {
  using namespace duallane;
  // Some ACS board components are required for Safe-Mode. It would be good if the software
  // transitions from A side to B side and from B side to dual mode autonomously
  // to ensure that that enough sensors are available without an operators intervention.
  // Therefore, the lost mode handler was overwritten to start these transitions
  Submode_t nextSubmode = Submodes::A_SIDE;
  if (submode == Submodes::A_SIDE) {
    nextSubmode = Submodes::B_SIDE;
  }
  if (not tryingOtherSide) {
    triggerEvent(CANT_KEEP_MODE, mode, submode);
    startTransition(mode, nextSubmode);
    tryingOtherSide = true;
  } else {
    // Not sure when this would happen. This flag is reset if the mode was reached. If it
    // was not reached, the transition failure handler should be called.
    sif::error << "DualLaneAssemblyBase::handleChildrenLostMode: Wrong handler called" << std::endl;
    triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
    startTransition(mode, Submodes::DUAL_MODE);
  }
}

void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
  using namespace duallane;
  Submode_t nextSubmode = Submodes::A_SIDE;
  if (submode == Submodes::A_SIDE) {
    nextSubmode = Submodes::B_SIDE;
  }
  // Check whether the transition was started because the mode could not be kept (not commanded).
  // If this is not the case, start transition to other side. If it is the case, start
  // transition to dual mode.
  if (not tryingOtherSide) {
    triggerEvent(CANT_KEEP_MODE, mode, submode);
    startTransition(mode, nextSubmode);
    tryingOtherSide = true;
  } else {
    triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
    startTransition(mode, Submodes::DUAL_MODE);
  }
}

bool DualLaneAssemblyBase::checkAndHandleRecovery() {
  using namespace power;
  OpCodes opCode = OpCodes::NONE;
  if (recoveryState == RECOVERY_IDLE) {
    return AssemblyBase::checkAndHandleRecovery();
  }
  if (customRecoveryStates == IDLE) {
    pwrStateMachine.start(MODE_OFF, 0);
    customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_OFF;
  }
  if (customRecoveryStates == POWER_SWITCHING_OFF) {
    opCode = pwrStateMachine.fsm();
    if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
      customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON;
      pwrStateMachine.start(targetMode, targetSubmode);
    }
  }
  if (customRecoveryStates == POWER_SWITCHING_ON) {
    opCode = pwrStateMachine.fsm();
    if (opCode == OpCodes::TO_NOT_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
      customRecoveryStates = RecoveryCustomStates::DONE;
    }
  }
  if (customRecoveryStates == DONE) {
    bool pendingRecovery = AssemblyBase::checkAndHandleRecovery();
    if (not pendingRecovery) {
      pwrStateMachine.reset();
      customRecoveryStates = RecoveryCustomStates::IDLE;
    }
    // For a recovery on one side, only do the recovery once
    for (auto& child : childrenMap) {
      if (healthHelper.healthTable->getHealth(child.first) == HasHealthIF::NEEDS_RECOVERY) {
        sendHealthCommand(child.second.commandQueue, HEALTHY);
        child.second.healthChanged = false;
      }
    }
    return pendingRecovery;
  }
  return true;
}

bool DualLaneAssemblyBase::sideSwitchTransition(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  if (this->mode == MODE_OFF) {
    return false;
  }
  if (this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) {
    if ((this->submode == Submodes::A_SIDE and submode == Submodes::B_SIDE) or
        (this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE)) {
      return true;
    }
    return false;
  }
  return false;
}

void DualLaneAssemblyBase::handleSideSwitchStates(uint8_t& submode, bool& needsSecondStep) {
  if (sideSwitchState == SideSwitchState::REQUESTED) {
    sideSwitchState = SideSwitchState::TO_DUAL;
  }
  // Switch to dual side first, and later switch back to the otherside
  if (sideSwitchState == SideSwitchState::TO_DUAL) {
    targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
    submode = duallane::Submodes::DUAL_MODE;
    sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE;
    // TODO: Ugly hack. The base class should support arbitrary number of steps..
    needsSecondStep = true;
  } else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
    // Set this flag because the power needs to be switched off.
    dualToSingleSideTransition = true;
    submode = targetSubmodeForSideSwitch;
  }
}

void DualLaneAssemblyBase::finishModeOp() {
  using namespace duallane;
  AssemblyBase::handleModeReached();
  pwrStateMachine.reset();
  powerRetryCounter = 0;
  tryingOtherSide = false;
  sideSwitchState = SideSwitchState::NONE;
  dualToSingleSideTransition = false;
  dualModeErrorSwitch = true;
}

void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) {
  using namespace duallane;
  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
    return;
  }
  this->defaultSubmode = submode;
}