/*
	HELP
*/

#include <fsfw/devicehandlers/DeviceHandlerFailureIsolation.h>
#include <fsfw/power/Fuse.h>
#include <fsfw/ipc/QueueFactory.h>
#include <mission/Controller/TCS_Heater.h>

TCS_Heater::TCS_Heater(uint32_t objectId, uint8_t switchId) :
		Heater(objectId, switchId, NULL), internalState(STATE_OFF), powerSwitcher(
				NULL), pcduQueueId(0), switchId(switchId), wasOn(
				false), timedOut(false), reactedToBeingFaulty(false), passive(
				false), eventQueue(NULL), heaterOnCountdown(300)/*5 min*/,
				parameterHelper(this) {
	eventQueue = QueueFactory::instance()->createMessageQueue(3, 5);
}

TCS_Heater::~TCS_Heater() {
	QueueFactory::instance()->deleteMessageQueue(eventQueue);
}

ReturnValue_t TCS_Heater::set() {
	passive = false;
	//wait for clear before doing anything
	if (internalState == STATE_WAIT) {
		return HasReturnvaluesIF::RETURN_OK;
	}
	if (healthHelper.healthTable->isHealthy(getObjectId())) {
		doAction(SET);
		if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)){
			return HasReturnvaluesIF::RETURN_FAILED;
		} else {
			return HasReturnvaluesIF::RETURN_OK;
		}
	} else {
		if (healthHelper.healthTable->isFaulty(getObjectId())) {
			if (!reactedToBeingFaulty) {
				reactedToBeingFaulty = true;
				doAction(CLEAR);
			}
		}
		return HasReturnvaluesIF::RETURN_FAILED;
	}
}

void TCS_Heater::clear(bool passive) {
	this->passive = passive;
	//Force switching off
	if (internalState == STATE_WAIT) {
		internalState = STATE_ON;
	}
	if (healthHelper.healthTable->isHealthy(getObjectId())) {
		doAction(CLEAR);
	} else if (healthHelper.healthTable->isFaulty(getObjectId())) {
		if (!reactedToBeingFaulty) {
			reactedToBeingFaulty = true;
			doAction(CLEAR);
		}
	}
}

void TCS_Heater::doAction(Action action) {
	//only act if we are not in the right state or in a transition
	if (action == SET) {
		if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)
				|| (internalState == STATE_EXTERNAL_CONTROL)) {
			/* *****POWER SWITCHER***** */
			//switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
			/* ************************ */
			internalState = STATE_WAIT_FOR_SWITCHES_ON;
			/* *****POWER SWITCHER***** */
			//powerSwitcher->sendSwitchCommand(switchId, PowerSwitchIF::SWITCH_ON);
			/* ************************ */
		}
	} else { //clear
		if ((internalState == STATE_ON) || (internalState == STATE_FAULTY)
				|| (internalState == STATE_EXTERNAL_CONTROL)) {
			internalState = STATE_WAIT_FOR_SWITCHES_OFF;
			/* *****POWER SWITCHER***** */
			//switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
			//powerSwitcher->sendSwitchCommand(switchId, PowerSwitchIF::SWITCH_OFF);
			/* ************************ */
		}
	}
}

ReturnValue_t TCS_Heater::performOperation(uint8_t opCode) {
	Heater::handleQueue();
	Heater::handleEventQueue();

	if (!healthHelper.healthTable->isFaulty(getObjectId())) {
		reactedToBeingFaulty = false;
	}

	/* *****POWER SWITCHER***** */
	/*switch (internalState) {
	case STATE_ON:
		if ((powerSwitcher->getSwitchState(switchId) == PowerSwitchIF::SWITCH_OFF)) {
			if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
				triggerEvent(PowerSwitchIF::SWITCH_WENT_OFF);
			} else {
				internalState = STATE_EXTERNAL_CONTROL;
			}
		}
		break;
	case STATE_OFF:
		if ((!healthHelper.healthTable->isFaulty(getObjectId())) && (powerSwitcher->getSwitchState(switchId) == PowerSwitchIF::SWITCH_ON)) {
			//do not trigger FD events when under external control
			if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
				internalState = STATE_WAIT_FOR_SWITCHES_OFF;
				switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
				powerSwitcher->sendSwitchCommand(switchId, PowerSwitchIF::SWITCH_OFF);
			} else {
				internalState = STATE_EXTERNAL_CONTROL;
			}
		}
		break;
	case STATE_PASSIVE:
		break;
	case STATE_WAIT_FOR_SWITCHES_ON:
		if (switchCountdown.hasTimedOut()) {
			if ((powerSwitcher->getSwitchState(switchId) == PowerSwitchIF::SWITCH_OFF)) {
				triggerEvent(HEATER_STAYED_OFF);
				internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
			} else {
				triggerEvent(HEATER_ON);
				internalState = STATE_ON;
			}
		}
		break;
	case STATE_WAIT_FOR_SWITCHES_OFF:
		if (switchCountdown.hasTimedOut()) {
			if ((powerSwitcher->getSwitchState(switchId) == PowerSwitchIF::SWITCH_ON)) {
				if (healthHelper.healthTable->isFaulty(getObjectId())) {
					if (passive) {
						internalState = STATE_PASSIVE;
					} else {
						internalState = STATE_OFF; //just accept it
					}
					triggerEvent(HEATER_ON); //but throw an event to make it more visible
					break;
				}
				triggerEvent(HEATER_STAYED_ON);
				internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
			} else {
				triggerEvent(HEATER_OFF);
				if (passive) {
					internalState = STATE_PASSIVE;
				} else {
					internalState = STATE_OFF;
				}
			}
		}
		break;
	default:
		break;
	}

	if ((powerSwitcher->getSwitchState(switchId) == PowerSwitchIF::SWITCH_ON)) {
		if (wasOn) {
			if (heaterOnCountdown.hasTimedOut()) {
				//SHOULDDO this means if a heater fails in single mode, the timeout will start again
				//I am not sure if this is a bug, but atm I have no idea how to fix this and think
				//it will be ok. whatcouldpossiblygowrong™
				if (!timedOut) {
					triggerEvent(HEATER_TIMEOUT);
					timedOut = true;
				}
			}
		} else {
			wasOn = true;
			heaterOnCountdown.resetTimer();
			timedOut = false;
		}
	} else {
		wasOn = false;
	}*/
	/* ************************ */

	return HasReturnvaluesIF::RETURN_OK;
}

/*MessageQueueId_t Heater::getCommandQueue() const {
	return commandQueue->getId();
}*/

ReturnValue_t TCS_Heater::initialize() {

	/*ReturnValue_t result = Heater::initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}*/

	ReturnValue_t result = SystemObject::initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	EventManagerIF* manager = objectManager->get<EventManagerIF>(
			objects::EVENT_MANAGER);
	if (manager == NULL) {
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	result = manager->registerListener(eventQueue->getId());
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	/*ConfirmsFailuresIF* pcdu = objectManager->get<ConfirmsFailuresIF>(
			DeviceHandlerFailureIsolation::powerConfirmationId);
	if (pcdu == NULL) {
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	pcduQueueId = pcdu->getEventReceptionQueue();*/

	result = manager->subscribeToAllEventsFrom(eventQueue->getId(),
			getObjectId());
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	result = parameterHelper.initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	result = healthHelper.initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t TCS_Heater::getParameter(uint8_t domainId, uint16_t parameterId,
				ParameterWrapper *parameterWrapper,
				const ParameterWrapper *newValues, uint16_t startAtIndex){
	return HasReturnvaluesIF::RETURN_OK;
}