#include <mission/devices/HeaterHandler.h>
#include <fsfwconfig/devices/powerSwitcherList.h>

HeaterHandler::HeaterHandler(object_id_t setObjectId, object_id_t gpioDriverId,
        CookieIF * gpioCookie, object_id_t mainLineSwitcherObjectId, uint8_t mainLineSwitch) :
        SystemObject(setObjectId), mode(MODE_OFF), submode(SUBMODE_NONE), gpioDriverId(gpioDriver),
        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;
	}

}

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

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

	IPCStore = objectManager->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->get<PowerSwitchIF>(mainLineSwitcherObjectId);
		if (mainLineSwitcher == nullptr) {
			sif::error << "HeaterHandler::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;
}

ReturnValue_t HeaterHandler::initializeHeaterMap(){
	heaterMapIter = heaterMap.begin();
	HeaterCommandInfo_t heaterCommandInfo;
	for(int 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 (int 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) {
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}
	if (actionId != SWITCH_ON && actionId != SWITCH_OFF) {
		result = COMMAND_NOT_SUPPORTED;
	} else {
		heaterMapIter = heaterMap.find(*data);
		if (heaterMapIter != heaterMap.end()) {
			heaterMapIter.second.action = *(data + 1);
			heaterMapIter.second.active = true;
		}
		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;
	case PowerSwitchIF::SWITCH_OFF:
		commandData[0] = switchNr;
		commandData[1] = SET_SWITCH_OFF;
	default:
		sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request"
				<< std::endl;
	}

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

void HeaterHandler::handleActiveCommands(){
	ReturnValue_t result;

	heaterMapIter = heaterMap.begin();
	for (; heaterMapIter != mapToAdd.end(); heaterMapIter++) {
		if (heaterMapIter.second.active) {
			switch(heaterMapIter.second.action) {
			case SET_SWITCH_ON:
			    int switchNr = heaterMapIter.first();
				/* Check state of main line switch */
				if (mainLineSwitcher->getSwitchState(pcduSwitches::TCS_BOARD_8V_HEATER_IN)
						== SWITCH_ON) {
				    if (!checkSwitchState(switchNr)) {
				        gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
                        result = gpioInterface->pullHigh(gpioId);
                        if (result != RETURN_OK) {
                            triggerEvent(GPIO_PULL_HIGH_FAILED, result);
                        }
				    }
				    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) {
						actionHelper.finish(heaterMapIter.second.replyQueue,
								heaterMapIter.second.action, result);
					}
					heaterMapIter.second.active = false;
				}
				else {
					mainLineSwitcher->sendSwitchCommand(pcduSwitches::TCS_BOARD_8V_HEATER_IN);
				}
				break;
			case SET_SWITCH_OFF:
			    int switchNr = heaterMapIter.first();
			    if (checkSwitchState(switchNr)) {
			        gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
                    result = gpioInterface->pullLow(gpioId);
                    triggerEvent(GPIO_PULL_LOW_FAILED, result);
			    }
			    else {
			        triggerEvent(SWITCH_ALREADY_OFF, switchNr);
			    }
			    if (heaterCommand.replyQueue != NO_COMMANDER) {
                    actionHelper.finish(heaterMapIter.second.replyQueue,
                            heaterMapIter.second.action, result);
                }
				break;
			default:
                sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
                        << std::endl;
                break;
			}
		}
	}
}

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

gpioId_t HeaterHandler::getGpioIdFromSwitchNr(switchNr) {
    gpioId_t gpioId = 0xFFFF;
    switch(switchNr) {
    case heaterSwitches::PAYLOAD_CAMERA:
        gpioId = gpioIds::HEATER_0;
        break;
    default:
        sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
                << std::endl;
        break;
    }
    return gpioId;
}