#include <mission/assemblies/TestAssembly.h>
#include <common/config/commonSystemObjects.h>

#include <fsfw/objectmanager/ObjectManagerIF.h>


TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId):
        AssemblyBase(objectId, parentId) {
	ModeListEntry newModeListEntry;
	newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_0);
	newModeListEntry.setMode(MODE_OFF);
	newModeListEntry.setSubmode(SUBMODE_NONE);

	commandTable.insert(newModeListEntry);

	newModeListEntry.setObject(objects::TEST_DEVICE_HANDLER_1);
	newModeListEntry.setMode(MODE_OFF);
	newModeListEntry.setSubmode(SUBMODE_NONE);

	commandTable.insert(newModeListEntry);

}

TestAssembly::~TestAssembly() {
}

ReturnValue_t TestAssembly::commandChildren(Mode_t mode,
		Submode_t submode) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::info << "TestAssembly: Received command to go to mode " << mode <<
            " submode " << (int) submode << std::endl;
#else
    sif::printInfo("TestAssembly: Received command to go to mode %d submode %d\n", mode, submode);
#endif
	ReturnValue_t result = RETURN_OK;
	if(mode == MODE_OFF){
		commandTable[0].setMode(MODE_OFF);
		commandTable[0].setSubmode(SUBMODE_NONE);
		commandTable[1].setMode(MODE_OFF);
		commandTable[1].setSubmode(SUBMODE_NONE);
	}
	else if(mode == DeviceHandlerIF::MODE_NORMAL) {
		if(submode == submodes::SINGLE){
			commandTable[0].setMode(MODE_OFF);
			commandTable[0].setSubmode(SUBMODE_NONE);
			commandTable[1].setMode(MODE_OFF);
			commandTable[1].setSubmode(SUBMODE_NONE);
			// We try to prefer 0 here but we try to switch to 1 even if it might fail
			if(isDeviceAvailable(objects::TEST_DEVICE_HANDLER_0)) {
				if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON) {
					commandTable[0].setMode(mode);
					commandTable[0].setSubmode(SUBMODE_NONE);
				}
				else {
					commandTable[0].setMode(MODE_ON);
					commandTable[0].setSubmode(SUBMODE_NONE);
				    result = NEED_SECOND_STEP;
				}
			}
			else {
				if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON) {
					commandTable[1].setMode(mode);
					commandTable[1].setSubmode(SUBMODE_NONE);
				}
				else{
					commandTable[1].setMode(MODE_ON);
					commandTable[1].setSubmode(SUBMODE_NONE);
				    result = NEED_SECOND_STEP;
				}
			}
		}
		else{
			// Dual Mode Normal
			if (childrenMap[objects::TEST_DEVICE_HANDLER_0].mode == MODE_ON) {
				commandTable[0].setMode(mode);
				commandTable[0].setSubmode(SUBMODE_NONE);
			}
			else{
				commandTable[0].setMode(MODE_ON);
				commandTable[0].setSubmode(SUBMODE_NONE);
			    result = NEED_SECOND_STEP;
			}
			if (childrenMap[objects::TEST_DEVICE_HANDLER_1].mode == MODE_ON) {
				commandTable[1].setMode(mode);
				commandTable[1].setSubmode(SUBMODE_NONE);
			}
			else{
				commandTable[1].setMode(MODE_ON);
				commandTable[1].setSubmode(SUBMODE_NONE);
			    result = NEED_SECOND_STEP;
			}
		}
	}
	else{
		//Mode ON
		if(submode == submodes::SINGLE){
			commandTable[0].setMode(MODE_OFF);
			commandTable[0].setSubmode(SUBMODE_NONE);
			commandTable[1].setMode(MODE_OFF);
			commandTable[1].setSubmode(SUBMODE_NONE);
			// We try to prefer 0 here but we try to switch to 1 even if it might fail
			if(isDeviceAvailable(objects::TEST_DEVICE_HANDLER_0)){
				commandTable[0].setMode(MODE_ON);
				commandTable[0].setSubmode(SUBMODE_NONE);
			}
			else{
				commandTable[1].setMode(MODE_ON);
				commandTable[1].setSubmode(SUBMODE_NONE);
			}
		}
		else{
			commandTable[0].setMode(MODE_ON);
			commandTable[0].setSubmode(SUBMODE_NONE);
			commandTable[1].setMode(MODE_ON);
			commandTable[1].setSubmode(SUBMODE_NONE);
		}
	}



	HybridIterator<ModeListEntry> iter(commandTable.begin(),
			commandTable.end());
	executeTable(iter);
	return result;
}

ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode,
		Submode_t submode) {
	switch (mode) {
	case MODE_OFF:
		if (submode == SUBMODE_NONE) {
			return RETURN_OK;
		} else {
			return INVALID_SUBMODE;
		}
	case DeviceHandlerIF::MODE_NORMAL:
	case MODE_ON:
		if (submode < 3) {
			return RETURN_OK;
		} else {
			return INVALID_SUBMODE;
		}
	}
	return INVALID_MODE;
}

ReturnValue_t TestAssembly::initialize() {
	ReturnValue_t result = AssemblyBase::initialize();
	if(result != RETURN_OK){
		return result;
	}
	handler0 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_0);
	handler1 = objectManager->get<TestDevice>(objects::TEST_DEVICE_HANDLER_1);
	if((handler0 == nullptr) or (handler1 == nullptr)){
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	handler0->setParentQueue(this->getCommandQueue());
	handler1->setParentQueue(this->getCommandQueue());


	result = registerChild(objects::TEST_DEVICE_HANDLER_0);
		if (result != HasReturnvaluesIF::RETURN_OK) {
			return result;
		}
	result = registerChild(objects::TEST_DEVICE_HANDLER_1);
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}
	return result;
}

ReturnValue_t TestAssembly::checkChildrenStateOn(
		Mode_t wantedMode, Submode_t wantedSubmode) {
	if(submode == submodes::DUAL){
		for(const auto& info:childrenMap) {
			if(info.second.mode != wantedMode or info.second.mode != wantedSubmode){
				return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
			}
		}
		return RETURN_OK;
	}
	else if(submode == submodes::SINGLE) {
		for(const auto& info:childrenMap) {
			if(info.second.mode == wantedMode and info.second.mode != wantedSubmode){
				return RETURN_OK;
			}
		}
	}
	return INVALID_SUBMODE;
}

bool TestAssembly::isDeviceAvailable(object_id_t object) {
	if(healthHelper.healthTable->getHealth(object) == HasHealthIF::HEALTHY){
		return true;
	}
	else{
		return false;
	}
}