#include <devices/powerSwitcherList.h>
#include <devices/gpioIds.h>

#include <mission/devices/SolarArrayDeploymentHandler.h>
#include <linux/gpio/GpioCookie.h>
#include <fsfw/ipc/QueueFactory.h>


SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
        object_id_t gpioDriverId_, CookieIF * gpioCookie_, object_id_t mainLineSwitcherObjectId_,
        uint8_t mainLineSwitch_, gpioId_t deplSA1, gpioId_t deplSA2, uint32_t burnTimeMs) :
        SystemObject(setObjectId_), gpioDriverId(gpioDriverId_), gpioCookie(gpioCookie_),
        mainLineSwitcherObjectId(mainLineSwitcherObjectId_), mainLineSwitch(mainLineSwitch_),
        deplSA1(deplSA1), deplSA2(deplSA2), burnTimeMs(burnTimeMs), actionHelper(this, nullptr) {
    commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
            MessageQueueMessage::MAX_MESSAGE_SIZE);
}

SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {
}

ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {

    if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
        handleStateMachine();
        return RETURN_OK;
    }
    return RETURN_OK;
}

ReturnValue_t SolarArrayDeploymentHandler::initialize() {
    ReturnValue_t result = SystemObject::initialize();
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    gpioInterface = objectManager->get<GpioIF>(gpioDriverId);
    if (gpioInterface == nullptr) {
        sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface."
                << std::endl;
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
    if (result != RETURN_OK) {
        sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
                << std::endl;
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
        mainLineSwitcher = objectManager->get<PowerSwitchIF>(mainLineSwitcherObjectId);
        if (mainLineSwitcher == nullptr) {
            sif::error
                    << "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object"
                    << "from object ID." << std::endl;
            return ObjectManagerIF::CHILD_INIT_FAILED;
        }
    }

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

    return RETURN_OK;
}

void SolarArrayDeploymentHandler::handleStateMachine() {
    switch (stateMachine) {
    case WAIT_ON_DELOYMENT_COMMAND:
        readCommandQueue();
        break;
    case SWITCH_8V_ON:
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
        mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
        stateMachine = WAIT_ON_8V_SWITCH;
        break;
    case WAIT_ON_8V_SWITCH:
        performWaitOn8VActions();
        break;
    case SWITCH_DEPL_GPIOS:
        switchDeploymentTransistors();
        break;
    case WAIT_ON_DEPLOYMENT_FINISH:
        handleDeploymentFinish();
        break;
    case WAIT_FOR_MAIN_SWITCH_OFF:
        if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) {
            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
        } else if (mainSwitchCountdown.hasTimedOut()) {
            triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
            sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main"
                    << " switch off" << std::endl;
            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
        }
        break;
    default:
        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" << std::endl;
        break;
    }
}

void SolarArrayDeploymentHandler::performWaitOn8VActions() {
    if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
        stateMachine = SWITCH_DEPL_GPIOS;
    } else {
        if (mainSwitchCountdown.hasTimedOut()) {
            triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
            actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
                    MAIN_SWITCH_TIMEOUT_FAILURE);
            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
        }
    }
}

void SolarArrayDeploymentHandler::switchDeploymentTransistors() {
    ReturnValue_t result = RETURN_OK;
    result = gpioInterface->pullHigh(deplSA1);
    if (result != RETURN_OK) {
        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                " array deployment switch 1 high " << std::endl;
        /* If gpio switch high failed, state machine is reset to wait for a command reinitiating
         * the deployment sequence. */
        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
        triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
            SWITCHING_DEPL_SA2_FAILED);
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
    }
    result = gpioInterface->pullHigh(deplSA2);
    if (result != RETURN_OK) {
        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                " array deployment switch 2 high " << std::endl;
        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
        triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
            SWITCHING_DEPL_SA2_FAILED);
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
    }
    deploymentCountdown.setTimeout(burnTimeMs);
    stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
}

void SolarArrayDeploymentHandler::handleDeploymentFinish() {
    ReturnValue_t result = RETURN_OK;
    if (deploymentCountdown.hasTimedOut()) {
        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS, RETURN_OK);
        result = gpioInterface->pullLow(deplSA1);
        if (result != RETURN_OK) {
            sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                    " array deployment switch 1 low " << std::endl;
        }
        result = gpioInterface->pullLow(deplSA2);
        if (result != RETURN_OK) {
            sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
                    " array deployment switch 2 low " << std::endl;
        }
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
        mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
        stateMachine = WAIT_FOR_MAIN_SWITCH_OFF;
    }
}

void SolarArrayDeploymentHandler::readCommandQueue() {
    CommandMessage command;
    ReturnValue_t result = commandQueue->receiveMessage(&command);
    if (result != RETURN_OK) {
        return;
    }

    result = actionHelper.handleActionMessage(&command);
    if (result == RETURN_OK) {
        return;
    }
}

ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
        MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
    ReturnValue_t result;
    if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
        sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
                << "waiting-on-command-state" << std::endl;
        return DEPLOYMENT_ALREADY_EXECUTING;
    }
    if (actionId != DEPLOY_SOLAR_ARRAYS) {
        sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command"
                << std::endl;
        result = COMMAND_NOT_SUPPORTED;
    } else {
        stateMachine = SWITCH_8V_ON;
        rememberCommanderId = commandedBy;
        result = RETURN_OK;
    }
    return result;
}

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