/* 
	HELP
*/

#include <fsfw/power/PowerSwitchIF.h>
#include <cstdlib>
#include <mission/Controller/ThermalController.h>
#include <OBSWConfig.h>
#include <mission/Controller/tcs_data_config.h> 

ThermalController::ThermalController(object_id_t objectId, object_id_t powerSwitcher, size_t commandQueueDepth):
		ControllerBase(objectId, 0, commandQueueDepth),
		powerSwitcherId(powerSwitcher), TempValueVec(datapool::Temperature_value, &SensorData, PoolVariableIF::VAR_READ),
        TEMPERATURE_SENSOR_CH1(objects::TCS_SENSOR_CH1, &TempValueVec[0], &TempValueVec, 0, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH1, &SensorData, NULL),
        TEMPERATURE_SENSOR_CH2(objects::TCS_SENSOR_CH2, &TempValueVec[1], &TempValueVec, 1, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH2, &SensorData, NULL),
        HEATER_1(objects::TCS_HEATER_1, objects::TCS_SWITCH_1),
        REDUNDANT_HEATER_1(objects::TCS_REDUNDANT_HEATER_1, objects::TCS_SWITCH_1R),
        HEATER_2(objects::TCS_HEATER_2, objects::TCS_SWITCH_2),
        REDUNDANT_HEATER_2(objects::TCS_REDUNDANT_HEATER_2, objects::TCS_SWITCH_2R),
		TEMPERATURE_COMPONENT_1(objects::TCS_COMPONENT_1, COMPONENT_DOMAIN_ID, datapool::Temperature_COMPONENT_1, datapool::TargetState_COMPONENT_1, datapool::CurrentState_COMPONENT_1,
				datapool::HeaterRequest_COMPONENT_1, &ComponentData, &TEMPERATURE_SENSOR_CH1, NULL, NULL, &HEATER_1, &REDUNDANT_HEATER_1, NULL, COMPONENT1_CONFIG, ThermalComponentIF::SAFE),
		TEMPERATURE_COMPONENT_2(objects::TCS_COMPONENT_2, COMPONENT_DOMAIN_ID + 1, datapool::Temperature_COMPONENT_2, datapool::TargetState_COMPONENT_2, datapool::CurrentState_COMPONENT_2,
				datapool::HeaterRequest_COMPONENT_2, &ComponentData, &TEMPERATURE_SENSOR_CH2, NULL, NULL, &HEATER_2, &REDUNDANT_HEATER_2, NULL, COMPONENT2_CONFIG, ThermalComponentIF::SAFE)
		{
        sensors.push_back(TEMPERATURE_SENSOR_CH1);
        sensors.push_back(TEMPERATURE_SENSOR_CH2);

        heaters.push_back(HEATER_1);
		heaters.push_back(HEATER_2);

		redundant_heaters.push_back(REDUNDANT_HEATER_1);
		redundant_heaters.push_back(REDUNDANT_HEATER_2);

        components.push_back(TEMPERATURE_COMPONENT_1);
        components.push_back(TEMPERATURE_COMPONENT_2);
}

ThermalController::~ThermalController() {
}

ReturnValue_t ThermalController::initialize() {

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

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

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

	/* *****POWER SWITCHER***** */
	/*PowerSwitchIF *powerSwitcher = objectManager->get<PowerSwitchIF>(
			powerSwitcherId);
	if (powerSwitcher == NULL) {
		return HasReturnvaluesIF::RETURN_FAILED;
	}*/
	/* ************************ */

	mode = MODE_NORMAL;
	submode = HEATER_REDUNDANCY;

	sif::debug<<"\nDEBUG_TCS: Start initialization"<<std::endl;

	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t ThermalController::handleCommandMessage(CommandMessage *message) {
	/*ReturnValue_t result = actionHelper.handleActionMessage(message);
	if (result == HasReturnvaluesIF::RETURN_OK) {
		return HasReturnvaluesIF::RETURN_OK;
	}

	result = parameterHelper.handleParameterMessage(message);
	if (result == HasReturnvaluesIF::RETURN_OK) {
		return HasReturnvaluesIF::RETURN_OK;
	}
	return result;*/
	
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t ThermalController::performOperation(uint8_t opCode) {
	
	sif::debug<<"DEBUG_TCS: Start of controller operations"<<std::endl;

	for (std::list<ArduinoTCSTemperatureSensor>::iterator iter = sensors.begin(); iter != sensors.end(); iter++) {
		iter->performHealthOp();
	}
	sif::debug<<"DEBUG_TCS: Health ops"<<std::endl;

	ControllerBase::performOperation(0);
	sif::debug<<"DEBUG_TCS: Controller ops"<<std::endl;

	if (mode == MODE_OFF) {
		return HasReturnvaluesIF::RETURN_OK;
	}

	SensorData.read();
	printf("\nTemperature: %f", TempValueVec[0]);
	printf("\nTemperature: %f\n", TempValueVec[1]);
	for (std::list<ArduinoTCSTemperatureSensor>::iterator iter = sensors.begin(); iter != sensors.end(); iter++) {
		iter->performOperation(0);
	}
	SensorData.commit();
	sif::debug<<"DEBUG_TCS: Sensor ops"<<std::endl;
	
	ComponentData.read();
	//calculateStrategy(true, true);
	ThermalComponentIF::HeaterRequest request;
	for (std::list<TCS_ThermalComponent>::iterator iter = components.begin(); iter != components.end(); iter++) {
		request = iter->performOperation(0, true, false); // request returnvalue to check, overridden
		//request = iter->performOperation(0, ThermalComponentIF::SAFE, true, false);
	}
	ComponentData.commit();
	sif::debug<<"DEBUG_TCS: Component ops"<<std::endl;//*/

	return HasReturnvaluesIF::RETURN_OK;	
}

void ThermalController::performControlOperation() {
	//Done by overwritten performOperation!
}

ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode,
        uint32_t *msToReachTheMode) {
        
	msToReachTheMode = 0;
	switch (mode) {
	case MODE_OFF: 
		startTransition(mode, 0);
		break;
	case MODE_NORMAL:
		if (submode == NO_REDUNDANCY) {
			return HasReturnvaluesIF::RETURN_OK;
		}
		else if (submode == HEATER_REDUNDANCY) {
			return HasReturnvaluesIF::RETURN_OK;
		}
		break;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void ThermalController::startTransition(Mode_t mode, Submode_t submode){
    switch (mode) {
    case MODE_OFF: 
    	mode = MODE_NORMAL;
    	break;
    case MODE_NORMAL:
    	mode = MODE_OFF;
    	break;
    default:
		mode = MODE_OFF;
    	break;
    }    
    switch (submode) {
    case NO_REDUNDANCY:
    	submode = HEATER_REDUNDANCY;
    	break;
    case HEATER_REDUNDANCY:
    	submode = NO_REDUNDANCY;
        break;
    case 0:
        break;
    default:
    	submode = NO_REDUNDANCY;
        break;
    }
}

//********************************************************************************************************************************************************************

/*void ThermalController::calculateStrategy(bool announce, bool redundancy) {
 *
 * NOT NEEDED NOW
 *
	bool changed = false;
	if (mode == MODE_ON) {
		if (submode == NO_REDUNDANCY) {
			if (redundancy){
				startTransition(NULL, submode);
				changed = true;
				strategy = ThermalModuleIF::ACTIVE_DUAL;
				break;
			}
			else{
				changed = true;
				strategy = ThermalModuleIF::ACTIVE_SINGLE;
				break;
			}
		}
		else if (submode == HEATER_REDUNDANCY){
			if (!redundancy){
				startTransition(NULL, submode);
				changed = true;
				strategy = ThermalModuleIF::ACTIVE_SINGLE;
				break;
			}
			else{
				changed = true;
				strategy = ThermalModuleIF::ACTIVE_DUAL;
				break;
			}
		}
	}
	else {
		startTransition(mode, NULL);
	}
}*/

/*ReturnValue_t ThermalController::executeAction(ActionId_t actionId,
		MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) {
	switch (actionId) {
	case SET_PREHEATING_COMPONENT: {
		if (size != 1 + sizeof(object_id_t)) {
			return INVALID_PARAMETERS;
		}
		object_id_t objectId;
		int32_t readSize = size;
		const uint8_t *pointer = data;
		SerializeAdapter<object_id_t>::deSerialize(&objectId, &pointer,
				&readSize, true);
		ThermalComponentIF *component = findComponentByObjectId(objectId);
		if (component != NULL) {
			if (component->setTargetState(*pointer) == RETURN_OK) {
				return EXECUTION_FINISHED;
			} else {
				return NOT_ALLOWED;
			}
		}
	}
		return INVALID_PARAMETERS;
	case SET_PREHEATING_MODULE:
		if (size != 2) {
			return INVALID_PARAMETERS;
		}
		switch (*data) {
		case 0:
			coreModule.setHeating(data[1] != 0);
			break;
		case 1:
			serviceModule.setHeating(data[1] != 0);
			break;
		case 2:
			payloadModule.setHeating(data[1] != 0);
			break;
		default:
			return INVALID_PARAMETERS;
		}
		return EXECUTION_FINISHED;
	default:
		return INVALID_ACTION_ID;
	}
}*/

/*ThermalComponentIF* ThermalController::findComponentByObjectId(object_id_t id) {
	//This method was generated with a regular expression from the ctor code.
	//More space, but faster than iterating a linked list to find the right id.
	switch (id) {
	case objects::TCS_COMPONENT_1:
		return &TEMPERATURE_COMPONENT_1;
	case objects::TCS_COMPONENT_2:
		return &TEMPERATURE_COMPONENT_2;
	default:
		return NULL;
	}
}*/

/*void ThermalController::announceMode(bool recursive) {
	triggerEvent(TCS_CONTROL_STRATEGY, strategy, 0);
	ControllerBase::announceMode(recursive);
}*/