#include <mission/devices/HeaterHandler.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),
healthHelper(this,setObjectId), modeHelper(this), parameterHelper(this),
actionHelper(this, nullptr), hkManager(this, nullptr),
childTransitionFailure(RETURN_OK), fdirInstance(fdirInstance),
hkSwitcher(this), defaultFDIRUsed(fdirInstance == nullptr),
switchOffWasReported(false), childTransitionDelay(5000),
transitionSourceMode(_MODE_POWER_DOWN),
transitionSourceSubMode(SUBMODE_NONE) {
	commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
				MessageQueueMessage::MAX_MESSAGE_SIZE);
	powerSwitcher = objectManager->get<PowerSwitcherIF>(
			mainLineSwitcherObjectId);
}

HeaterHandler::~HeaterHandler() {
}

ReturnValue_t performOperation(uint8_t operationCode) {

	if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
		readCommandQueue();
		handlePendingCommand();
		doStateMachine();

		checkSwitchState();
		decrementDeviceReplyMap();
		fdirInstance->checkForFailures();
		hkSwitcher.performOperation();
		performOperationHook();
		return RETURN_OK;
	}
}

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

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

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

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

	if(powerSwitcherId != objects::NO_OBJECT) {
		powerSwitcher = objectManager->get<PowerSwitchIF>(powerSwitcherId);
		if (powerSwitcher == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
			sif::error << "HeaterHandler::initialize: Power switcher "
					<< "object ID set but no valid object found." << std::endl;
#endif
			return ObjectManagerIF::CHILD_INIT_FAILED;
		}
	}

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

	result = modeHelper.initialize();
	if (result != RETURN_OK) {
		return result;
	}
	result = actionHelper.initialize(commandQueue);
	if (result != RETURN_OK) {
		return result;
	}

	fillCommandAndReplyMap();

	return RETURN_OK;
}

void DeviceHandlerBase::readCommandQueue() {

	if (dontCheckQueue()) {
		return;
	}

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

	result = modeHelper.handleModeCommand(&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 = acceptExternalDeviceCommands();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}
	heaterCommandMap::iterator iter = HeaterCommandMap.find(actionId);
	if (iter == heaterCommandMap.end()) {
		result = COMMAND_NOT_SUPPORTED;
	} else {
		if (commandedBy == commandQueue){
			heaterCommand(*(data), *(data + 1), NO_COMMANDER);
			commandPending = true;
		}
		else {
			heaterCommand(*(data), *(data + 1), commandedBy);
			commandPending = true;
		}
		result = RETURN_OK;
	}
	return result;
}

ReturnValue_t DeviceHandlerBase::acceptExternalDeviceCommands() {
	if (mode != MODE_ON) {
		return WRONG_MODE_FOR_COMMAND;
	}
	return RETURN_OK;
}

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

	ReturnValue_t result;
	store_address_t address;
	HeaterCommand_t command;

	switch(onOff) {
	case PowerSwitchIF::SWITCH_ON:
		command(SWITCH_ON, switchNr);
	case PowerSwitchIF::SWITCH_OFF:
		command(SWITCH_OFF, switchNr);
	default:
		sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request"
				<< std::endl;
	}

	result = IPCStore->addData(&address, &command, sizeof(command));
	if (result == RETURN_OK) {
		CommandMessage message;
		ActionMessage::setCommand(&message,
				HeaterHandler::SWITCH_HEATER, address);
		/* 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::handlePendingCommand(){
	ReturnValue_t result;

	if (!commandPending) {
		return;
	}
	switch(heaterCommand.action) {
	case SET_SWITCH_ON:
		result = gpioInterface->pullHigh(heaterCommand.heaterSwitchNr);
		if (result != RETURN_OK) {
			triggerEvent()
		}
		if (heaterCommand.replyQueue != NO_COMMANDER) {
			actionHelper.finish(heaterCommand.replyQueue, heaterCommand.action,
					result);
		}
		commandPending = false;
		break;
	case SET_SWITCH_OFF:
		result = gpioInterface->pullLow(heaterCommand.heaterSwitchNr);
		if (heaterCommand.replyQueue != NO_COMMANDER) {
			actionHelper.finish(heaterCommand.replyQueue, heaterCommand.action,
					result);
		}
		commandPending = false;
		break;
	}
}