/* 
	HELP
*/

#include <mission/Controller/TCS_ThermalComponent.h>

TCS_ThermalComponent::TCS_ThermalComponent(object_id_t reportingObjectId,
		uint8_t domainId, uint32_t temperaturePoolId,
		uint32_t targetStatePoolId, uint32_t currentStatePoolId,
		uint32_t requestPoolId, DataSet* dataSet,
		ArduinoTCSTemperatureSensor* sensor,
		ArduinoTCSTemperatureSensor* firstRedundantSensor,
		ArduinoTCSTemperatureSensor* secondRedundantSensor,
		TCS_Heater *Heater, TCS_Heater *RedundantHeater,
		ThermalModuleIF* thermalModule, Parameters parameters,
		Priority priority) :
		CoreComponent(reportingObjectId, domainId, temperaturePoolId,
				targetStatePoolId, currentStatePoolId, requestPoolId, dataSet,
				sensor, firstRedundantSensor, secondRedundantSensor,
				thermalModule,
				{ parameters.lowerOpLimit, parameters.upperOpLimit,
						parameters.heaterOn, parameters.hysteresis,
						parameters.heaterSwitchoff }, priority,
				ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL),
				Heater(Heater), RedundantHeater(RedundantHeater),
				nopParameters({ parameters.lowerNopLimit, parameters.upperNopLimit }) {

		//Set thermal state once, then leave to operator.
		/*DataSet mySet;
		PoolVariable<int8_t> writableTargetState(targetStatePoolId, &mySet, PoolVariableIF::VAR_WRITE);
		writableTargetState = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
		mySet.commit(PoolVariableIF::VALID);*/

}

TCS_ThermalComponent::~TCS_ThermalComponent() {
}

ThermalComponentIF::HeaterRequest TCS_ThermalComponent::performOperation(uint8_t opCode, bool redundancy, bool dual) {

	sif::debug<<"DEBUG_COMPONENT: 1"<<std::endl;
	// Heater define the Internal State
	if (Heater != NULL) {
		Heater->performOperation(0);
	}
	if (redundancy){
		if (RedundantHeater != NULL) {
			RedundantHeater->performOperation(0);
		}
	}
	sif::debug<<"DEBUG_COMPONENT: 2"<<std::endl;

	HeaterRequest request = HEATER_DONT_CARE;
	request = CoreComponent::performOperation(0);
	sif::debug<<"DEBUG_COMPONENT: 3"<<std::endl;

	/*if (Heater == NULL) {
		informComponentsAboutHeaterState(false, NONE, priority, request);
		//return;
	}*/

	//bool heating = calculateModuleHeaterRequestAndSetModuleStatus(strategy);
	bool heating = false;

	if (request != ThermalComponentIF::HEATER_DONT_CARE) {
		//Components overwrite the module request.
		heating = ((request == ThermalComponentIF::HEATER_REQUEST_ON) ||
				(request == ThermalComponentIF::HEATER_REQUEST_EMERGENCY_ON));
		sif::debug<<"DEBUG_COMPONENT: 4"<<std::endl;
	}


	// strategy NOT-PASSIVE as default
	if (heating) {
		ReturnValue_t result = Heater->set();
		if (redundancy){
			if (result != HasReturnvaluesIF::RETURN_OK || dual) {
				sif::debug<<"DEBUG_COMPONENT: 5"<<std::endl;
				RedundantHeater->set();
				sif::debug<<"DEBUG_COMPONENT: 6"<<std::endl;
			} else {
				sif::debug<<"DEBUG_COMPONENT: 7"<<std::endl;
				RedundantHeater->clear(false);
				sif::debug<<"DEBUG_COMPONENT: 8"<<std::endl;
			}
		} else {
			sif::debug<<"DEBUG_COMPONENT: 9"<<std::endl;
			if (result != HasReturnvaluesIF::RETURN_OK) {
				sif::debug<<"DEBUG_COMPONENT: 10"<<std::endl;
				Heater->clear(false);
				sif::debug<<"DEBUG_COMPONENT: 11"<<std::endl;
			}
		}
	} else {
		sif::debug<<"DEBUG_COMPONENT: 12"<<std::endl;
		Heater->clear(false);
		RedundantHeater->clear(false);
		sif::debug<<"DEBUG_COMPONENT: 13"<<std::endl;
	}
	sif::debug<<"DEBUG_COMPONENT: 14"<<std::endl;

	/*if (strategy == PASSIVE) {
		informComponentsAboutHeaterState(false, NONE, priority, request);
		if (oldStrategy != PASSIVE) {
			heater->set(false, false, true);
		}
	} else {
		if (safeOnly) {
			informComponentsAboutHeaterState(heating, SAFE, priority, request);
		} else {
			informComponentsAboutHeaterState(heating, ALL, priority, request);
		}
		heater->set(heating, dual);
	}
	oldStrategy = strategy;*/
	return request;
}


ReturnValue_t TCS_ThermalComponent::setTargetState(int8_t newState) {
	DataSet mySet;
	PoolVariable<int8_t> writableTargetState(targetState.getDataPoolId(),
			&mySet, PoolVariableIF::VAR_READ_WRITE);
	mySet.read();
	if ((writableTargetState == STATE_REQUEST_OPERATIONAL)
			&& (newState != STATE_REQUEST_IGNORE)) {
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	switch (newState) {
	case STATE_REQUEST_HEATING:
	case STATE_REQUEST_IGNORE:
	case STATE_REQUEST_OPERATIONAL:
		writableTargetState = newState;
		break;
	case STATE_REQUEST_NON_OPERATIONAL:
		writableTargetState = newState;
		break;
	default:
		return INVALID_TARGET_STATE;
	}
	mySet.commit(PoolVariableIF::VALID);
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t TCS_ThermalComponent::setLimits(const uint8_t* data, uint32_t size) {
	if (size != 4 * sizeof(parameters.lowerOpLimit)) {
		return MonitoringIF::INVALID_SIZE;
	}
	size_t readSize = size;
	SerializeAdapter::deSerialize(&nopParameters.lowerNopLimit, &data,
			&readSize, SerializeIF::Endianness::BIG);
	SerializeAdapter::deSerialize(&parameters.lowerOpLimit, &data,
			&readSize, SerializeIF::Endianness::BIG);
	SerializeAdapter::deSerialize(&parameters.upperOpLimit, &data,
			&readSize, SerializeIF::Endianness::BIG);
	SerializeAdapter::deSerialize(&nopParameters.upperNopLimit, &data,
			&readSize, SerializeIF::Endianness::BIG);
	return HasReturnvaluesIF::RETURN_OK;
}

ThermalComponentIF::State TCS_ThermalComponent::getState(float temperature,
		CoreComponent::Parameters parameters, int8_t targetState) {
	if (temperature < nopParameters.lowerNopLimit) {
		return OUT_OF_RANGE_LOW;
	} else {
		State state = CoreComponent::getState(temperature, parameters,
				targetState);
		if (state != NON_OPERATIONAL_HIGH
				&& state != NON_OPERATIONAL_HIGH_IGNORED) {
			return state;
		}
		if (temperature > nopParameters.upperNopLimit) {
			state = OUT_OF_RANGE_HIGH;
		}
		if (targetState == STATE_REQUEST_IGNORE) {
			state = getIgnoredState(state);
		}
		return state;
	}
}

void TCS_ThermalComponent::checkLimits(ThermalComponentIF::State state) {
	if (targetState == STATE_REQUEST_OPERATIONAL || targetState == STATE_REQUEST_IGNORE) {
		CoreComponent::checkLimits(state);
		return;
	}
	//If component is not operational, it checks the NOP limits.
	temperatureMonitor.translateState(state, temperature.value,
			nopParameters.lowerNopLimit, nopParameters.upperNopLimit, false);
}

ThermalComponentIF::HeaterRequest TCS_ThermalComponent::getHeaterRequest(
		int8_t targetState, float temperature,
		CoreComponent::Parameters parameters) {
	if (targetState == STATE_REQUEST_IGNORE) {
		isHeating = false;
		return HEATER_DONT_CARE;
	}
	
	if (temperature
			> nopParameters.upperNopLimit - parameters.heaterSwitchoff) {
		isHeating = false;
		return HEATER_REQUEST_EMERGENCY_OFF;
	}

	float nopHeaterLimit = nopParameters.lowerNopLimit + parameters.heaterOn;
	float opHeaterLimit = parameters.lowerOpLimit + parameters.heaterOn;

	if (isHeating) {
		nopHeaterLimit += parameters.hysteresis;
		opHeaterLimit += parameters.hysteresis;
	}

	if (temperature < nopHeaterLimit) {
		isHeating = true;
		return HEATER_REQUEST_EMERGENCY_ON;
	}

	if ((targetState == STATE_REQUEST_OPERATIONAL)
			|| (targetState == STATE_REQUEST_HEATING)) {
		if (temperature < opHeaterLimit) {
			isHeating = true;
			return HEATER_REQUEST_ON;
		}
		if (temperature
				> parameters.upperOpLimit - parameters.heaterSwitchoff) {
			isHeating = false;
			return HEATER_REQUEST_OFF;
		}
	}

	isHeating = false;
	return HEATER_DONT_CARE;
}

ThermalComponentIF::State TCS_ThermalComponent::getIgnoredState(int8_t state) {
	switch (state) {
	case OUT_OF_RANGE_LOW:
		return OUT_OF_RANGE_LOW_IGNORED;
	case OUT_OF_RANGE_HIGH:
		return OUT_OF_RANGE_HIGH_IGNORED;
	case OUT_OF_RANGE_LOW_IGNORED:
		return OUT_OF_RANGE_LOW_IGNORED;
	case OUT_OF_RANGE_HIGH_IGNORED:
		return OUT_OF_RANGE_HIGH_IGNORED;
	default:
		return CoreComponent::getIgnoredState(state);
	}
}

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

/*void TCS_ThermalComponent::informComponentsAboutHeaterState(bool heaterIsOn,
		Informee whomToInform, Priority priority, HeaterRequest request) {

		switch (whomToInform) {
		case ALL:
			break;
		case SAFE:
			if (!(priority == ThermalComponentIF::SAFE)) {
				markStateIgnored();
				continue;
			}
			break;
		case NONE:
			markStateIgnored();
			continue;
		}

		if (heaterIsOn) {
			if ((request == ThermalComponentIF::HEATER_REQUEST_EMERGENCY_OFF)
					|| (request == ThermalComponentIF::HEATER_REQUEST_OFF)) {
				markStateIgnored();
			}
		} else {
			if ((request == ThermalComponentIF::HEATER_REQUEST_EMERGENCY_ON)
					|| (request == ThermalComponentIF::HEATER_REQUEST_ON)) {
				markStateIgnored();
			}
		}
}*/