#include "HeaterHandler.h"
#include "devices/gpioIds.h"
#include "devices/powerSwitcherList.h"

#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>

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

HeaterHandler::~HeaterHandler() {
}

ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {

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

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

	result = initializeHeaterMap();
	if (result != RETURN_OK) {
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

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

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

	IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
	if (IPCStore == nullptr) {
		sif::error << "HeaterHandler::initialize: IPC store not set up in factory." << std::endl;
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

	if(mainLineSwitcherObjectId != objects::NO_OBJECT) {
		mainLineSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(mainLineSwitcherObjectId);
		if (mainLineSwitcher == nullptr) {
            sif::error
                    << "HeaterHandler::initialize: Failed to get main line switcher. Make sure "
                    << "main line switcher object is initialized." << std::endl;
			return ObjectManagerIF::CHILD_INIT_FAILED;
		}
	}

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

	return RETURN_OK;
}

ReturnValue_t HeaterHandler::initializeHeaterMap(){
	HeaterCommandInfo_t heaterCommandInfo;
	for(switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
		std::pair status = heaterMap.emplace(switchNr, heaterCommandInfo);
		if (status.second == false) {
			sif::error << "HeaterHandler::initializeHeaterMap: Failed to initialize heater map"
					<< std::endl;
			return RETURN_FAILED;
		}
	}
	return RETURN_OK;
}

void HeaterHandler::setInitialSwitchStates() {
	for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
		switchStates[switchNr] = OFF;
	}
}

void HeaterHandler::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 HeaterHandler::executeAction(ActionId_t actionId,
		MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
    ReturnValue_t result;
    if (actionId != SWITCH_HEATER) {
		result = COMMAND_NOT_SUPPORTED;
	} else {
	    switchNr_t switchNr = *data;
	    HeaterMapIter heaterMapIter = heaterMap.find(switchNr);
		if (heaterMapIter != heaterMap.end()) {
		    if (heaterMapIter->second.active) {
		        return COMMAND_ALREADY_WAITING;
		    }
			heaterMapIter->second.action = *(data + 1);
			heaterMapIter->second.active = true;
			heaterMapIter->second.replyQueue = commandedBy;
		}
		else {
			sif::error << "HeaterHandler::executeAction: Invalid switchNr" << std::endl;
			return INVALID_SWITCH_NR;
		}
		result = RETURN_OK;
	}
	return result;
}


void HeaterHandler::sendSwitchCommand(uint8_t switchNr,
		ReturnValue_t onOff) const {

	ReturnValue_t result;
	store_address_t storeAddress;
	uint8_t commandData[2];

	switch(onOff) {
	case PowerSwitchIF::SWITCH_ON:
		commandData[0] = switchNr;
		commandData[1] = SET_SWITCH_ON;
		break;
	case PowerSwitchIF::SWITCH_OFF:
		commandData[0] = switchNr;
		commandData[1] = SET_SWITCH_OFF;
		break;
	default:
		sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request"
				<< std::endl;
		break;
	}

	result = IPCStore->addData(&storeAddress, commandData, sizeof(commandData));
	if (result == RETURN_OK) {
		CommandMessage message;
		ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress);
		/* Send heater command to own command queue */
		result = commandQueue->sendMessage(commandQueue->getId(), &message, 0);
		if (result != RETURN_OK) {
			sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch"
					<< "message" << std::endl;
		}
	}
}

void HeaterHandler::handleActiveCommands(){

	HeaterMapIter heaterMapIter = heaterMap.begin();
	for (; heaterMapIter != heaterMap.end(); heaterMapIter++) {
		if (heaterMapIter->second.active) {
			switch(heaterMapIter->second.action) {
			case SET_SWITCH_ON:
			    handleSwitchOnCommand(heaterMapIter);
				break;
			case SET_SWITCH_OFF:
			    handleSwitchOffCommand(heaterMapIter);
			    break;
			default:
                sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
                        << std::endl;
                break;
			}
		}
	}
}

void HeaterHandler::handleSwitchOnCommand(HeaterMapIter heaterMapIter) {

    ReturnValue_t result = RETURN_OK;
    switchNr_t switchNr;

    /* Check if command waits for main switch being set on and whether the timeout has expired */
    if (heaterMapIter->second.waitMainSwitchOn
            && heaterMapIter->second.mainSwitchCountdown.hasTimedOut()) {
        //TODO - This requires the initiation of an FDIR procedure
        triggerEvent(MAIN_SWITCH_TIMEOUT);
        sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
                << std::endl;
        heaterMapIter->second.active = false;
        heaterMapIter->second.waitMainSwitchOn = false;
        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
            actionHelper.finish(false, heaterMapIter->second.replyQueue,
                    heaterMapIter->second.action, MAIN_SWITCH_SET_TIMEOUT );
        }
        return;
    }

    switchNr = heaterMapIter->first;
    /* Check state of main line switch */
    ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
    if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
        if (!checkSwitchState(switchNr)) {
            gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
            result = gpioInterface->pullHigh(gpioId);
            if (result != RETURN_OK) {
                sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id "
                        << gpioId << " high" << std::endl;
                triggerEvent(GPIO_PULL_HIGH_FAILED, result);
            }
            else {
                switchStates[switchNr] = ON;
            }
        }
        else {
            triggerEvent(SWITCH_ALREADY_ON, switchNr);
        }
        /* There is no need to send action finish replies if the sender was the
         * HeaterHandler itself. */
        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
            if(result == RETURN_OK) {
                actionHelper.finish(true, heaterMapIter->second.replyQueue,
                        heaterMapIter->second.action, result);
            }
            else {
                actionHelper.finish(false, heaterMapIter->second.replyQueue,
                        heaterMapIter->second.action, result);
            }

        }
        heaterMapIter->second.active = false;
        heaterMapIter->second.waitMainSwitchOn = false;
    }
    else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF
            && heaterMapIter->second.waitMainSwitchOn) {
        /* Just waiting for the main switch being set on */
        return;
    }
    else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
        mainLineSwitcher->sendSwitchCommand(mainLineSwitch,
                PowerSwitchIF::SWITCH_ON);
        heaterMapIter->second.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
        heaterMapIter->second.waitMainSwitchOn = true;
    }
    else {
        sif::debug << "HeaterHandler::handleActiveCommands: Failed to get state of"
                << " main line switch" << std::endl;
        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
            actionHelper.finish(false, heaterMapIter->second.replyQueue,
                    heaterMapIter->second.action, mainSwitchState);
        }
        heaterMapIter->second.active = false;
    }
}

void HeaterHandler::handleSwitchOffCommand(HeaterMapIter heaterMapIter) {
    ReturnValue_t result = RETURN_OK;
    switchNr_t switchNr = heaterMapIter->first;
    /* Check whether switch is already off */
    if (checkSwitchState(switchNr)) {
        gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
        result = gpioInterface->pullLow(gpioId);
        if (result != RETURN_OK) {
            sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id"
                    << gpioId << " low" << std::endl;
            triggerEvent(GPIO_PULL_LOW_FAILED, result);
        }
        else {
            switchStates[switchNr] = OFF;
            /* When all switches are off, also main line switch will be turned off */
            if (allSwitchesOff()) {
                mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
            }
        }
    }
    else {
        sif::info << "HeaterHandler::handleSwitchOffCommand: Switch already off" << std::endl;
        triggerEvent(SWITCH_ALREADY_OFF, switchNr);
    }
    if (heaterMapIter->second.replyQueue != NO_COMMANDER) {
        /* Report back switch command reply if necessary */
        if(result == HasReturnvaluesIF::RETURN_OK) {
            actionHelper.finish(true, heaterMapIter->second.replyQueue,
                    heaterMapIter->second.action, result);
        }
        else {
            actionHelper.finish(false, heaterMapIter->second.replyQueue,
                    heaterMapIter->second.action, result);
        }
    }
    heaterMapIter->second.active = false;
}

bool HeaterHandler::checkSwitchState(int switchNr) {
    return switchStates[switchNr];
}

bool HeaterHandler::allSwitchesOff() {
    bool allSwitchesOrd = false;
    /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
    for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
        allSwitchesOrd = allSwitchesOrd  || switchStates[switchNr];
    }
    return !allSwitchesOrd;
}

gpioId_t HeaterHandler::getGpioIdFromSwitchNr(int switchNr) {
    gpioId_t gpioId = 0xFFFF;
    switch(switchNr) {
    case heaterSwitches::HEATER_0:
        gpioId = gpioIds::HEATER_0;
        break;
    case heaterSwitches::HEATER_1:
        gpioId = gpioIds::HEATER_1;
        break;
    case heaterSwitches::HEATER_2:
        gpioId = gpioIds::HEATER_2;
        break;
    case heaterSwitches::HEATER_3:
        gpioId = gpioIds::HEATER_3;
        break;
    case heaterSwitches::HEATER_4:
        gpioId = gpioIds::HEATER_4;
        break;
    case heaterSwitches::HEATER_5:
        gpioId = gpioIds::HEATER_5;
        break;
    case heaterSwitches::HEATER_6:
        gpioId = gpioIds::HEATER_6;
        break;
    case heaterSwitches::HEATER_7:
        gpioId = gpioIds::HEATER_7;
        break;
    default:
        sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
                << std::endl;
        break;
    }
    return gpioId;
}

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

void HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) const {
}

ReturnValue_t HeaterHandler::getSwitchState( uint8_t switchNr ) const {
    return 0;
}

ReturnValue_t HeaterHandler::getFuseState( uint8_t fuseNr ) const {
    return 0;
}

uint32_t HeaterHandler::getSwitchDelayMs(void) const {
    return 0;
}