#include <framework/modes/HasModesIF.h>
#include <framework/modes/ModeHelper.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>

ModeHelper::ModeHelper(HasModesIF *owner) :
		commandedMode(HasModesIF::MODE_OFF),
		commandedSubmode(HasModesIF::SUBMODE_NONE),
		owner(owner), forced(false) {}

ModeHelper::~ModeHelper() {

}

ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
	MessageQueueMessage message;
	CommandMessage reply(&message);
	Mode_t mode;
	Submode_t submode;
	switch (command->getCommand()) {
	case ModeMessage::CMD_MODE_COMMAND_FORCED:
		forced = true;
		/* NO BREAK falls through*/
	case ModeMessage::CMD_MODE_COMMAND: {
		mode = ModeMessage::getMode(command);
		submode = ModeMessage::getSubmode(command);
		uint32_t timeout;
		ReturnValue_t result = owner->checkModeCommand(mode, submode, &timeout);
		if (result != HasReturnvaluesIF::RETURN_OK) {
			ModeMessage::cantReachMode(&reply, result);
			MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
			        MessageQueueMessage::MAX_MESSAGE_SIZE,
			        owner->getCommandQueue());
			break;
		}
		//Free to start transition
		theOneWhoCommandedAMode = command->getSender();
		commandedMode = mode;
		commandedSubmode = submode;

		if ((parentQueueId != MessageQueueMessageIF::NO_QUEUE)
				&& (theOneWhoCommandedAMode != parentQueueId)) {
			owner->setToExternalControl();
		}

		countdown.setTimeout(timeout);
		owner->startTransition(mode, submode);
	}
		break;
	case ModeMessage::CMD_MODE_READ: {
		owner->getMode(&mode, &submode);
		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, mode,
				submode);
		MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
		        MessageQueueMessage::MAX_MESSAGE_SIZE,
		       owner->getCommandQueue());
	}
		break;
	case ModeMessage::CMD_MODE_ANNOUNCE:
		owner->announceMode(false);
		break;
	case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
		owner->announceMode(true);
		break;
	default:
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t ModeHelper::initialize(MessageQueueId_t parentQueueId) {
	setParentQueue(parentQueueId);
	return initialize();
}

void ModeHelper::modeChanged(Mode_t ownerMode, Submode_t ownerSubmode) {
	forced = false;
	sendModeReplyMessage(ownerMode, ownerSubmode);
	sendModeInfoMessage(ownerMode, ownerSubmode);
	theOneWhoCommandedAMode = MessageQueueMessageIF::NO_QUEUE;
}

void ModeHelper::sendModeReplyMessage(Mode_t ownerMode,
		Submode_t ownerSubmode) {
	MessageQueueMessage message;
	CommandMessage reply(&message);
	if (theOneWhoCommandedAMode != MessageQueueMessageIF::NO_QUEUE)
	{
		if (ownerMode != commandedMode or ownerSubmode != commandedSubmode)
		{
			ModeMessage::setModeMessage(&reply,
					ModeMessage::REPLY_WRONG_MODE_REPLY, ownerMode,
					ownerSubmode);
		}
		else
		{
			ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY,
					ownerMode, ownerSubmode);
		}
		MessageQueueSenderIF::sendMessage(theOneWhoCommandedAMode, &reply,
		        MessageQueueMessage::MAX_MESSAGE_SIZE, owner->getCommandQueue());
	}
}

void ModeHelper::sendModeInfoMessage(Mode_t ownerMode,
		Submode_t ownerSubmode) {
	MessageQueueMessage message;
	CommandMessage reply(&message);
	if (theOneWhoCommandedAMode != parentQueueId
			and parentQueueId != MessageQueueMessageIF::NO_QUEUE)
	{
		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO,
				ownerMode, ownerSubmode);
		MessageQueueSenderIF::sendMessage(parentQueueId, &reply,
		        MessageQueueMessage::MAX_MESSAGE_SIZE,
		        owner->getCommandQueue());
	}
}

void ModeHelper::startTimer(uint32_t timeoutMs) {
	countdown.setTimeout(timeoutMs);
}

void ModeHelper::setParentQueue(MessageQueueId_t parentQueueId) {
	this->parentQueueId = parentQueueId;
}

ReturnValue_t ModeHelper::initialize(void) {
	return HasReturnvaluesIF::RETURN_OK;
}

bool ModeHelper::isTimedOut() {
	return countdown.hasTimedOut();
}

bool ModeHelper::isForced() {
	return forced;
}

void ModeHelper::setForced(bool forced) {
	this->forced = forced;
}