#include "SensorHandlerOfAgent.h"
#include <stdio.h>
#include "printError.h"

#include <algorithm> 

#define MAXNUMOF_MOUNTEDSENSORS 100

using namespace std;

SensorHandlerOfAgent :: SensorHandlerOfAgent() {
	init_sensorHandler();
}

void SensorHandlerOfAgent :: init_sensorHandler() {
	maxNumOf_mountedSensors = MAXNUMOF_MOUNTEDSENSORS;
}

//TODO: if(vMountedSensors.size() < maxNumOf_mountedSensors)  als aller erste Abfrage machen ... noch bevor "SensorSlotOfAgent* sensorSlotOfAgent = new SensorSlotOfAgent();"
//TODO: delete object if it cannot added to vector
bool SensorHandlerOfAgent :: mount_sensorIntoSensorSlot(Channel* inputPort) {
	SensorSlotOfAgent* sensorSlotOfAgent = new SensorSlotOfAgent();
	if(sensorSlotOfAgent != NULL) {
		if(sensorSlotOfAgent->set_comPort(inputPort)) {
			try {
				if(vMountedSensors.size() < maxNumOf_mountedSensors) {
					vMountedSensors.push_back(sensorSlotOfAgent);
				}
				else {
					printError("Max number of mounted sensors is already reached!");

					return false;
				}
			}
			catch(bad_alloc& error) {
				printError("bad_alloc caught: ", error.what());
				return false;
			}
			return true;
		}
		else {
			printError("Input port is no set!");
			vMountedSensors.pop_back(); //TODO: check if it is right?!?!

			return false;
		}
	}
	else {
		printError("Couldn't create SensorSlot!");

		return false;
	}
}

//TODO: what to do when 2 sensorSlots have the same inputPort??!!
SensorSlotOfAgent* SensorHandlerOfAgent :: get_sensorSlotAddress(Channel* inputPort) {
	for(auto &sensorSlot : vMountedSensors) {
		if(sensorSlot->get_comPort() == inputPort) {
			return sensorSlot;
		}
	}
	return NULL;
}

//TODO: what to do when 2 slaveAgentSlots have the same inputPort??!!
//TODO: case if slot with comPort is not in this vector
unsigned int  SensorHandlerOfAgent :: get_sensorSlotNumber(Channel* inputPort) {
	unsigned int slotNumber = 0;
	for(auto &sensorSlot : vMountedSensors) {
		if(sensorSlot->get_comPort() == inputPort) {
			return slotNumber;
		}
		slotNumber++;
	}
	return 0;
}

//TODO: what to do when 2 sensorSlots have the same historyModule??!!
SensorSlotOfAgent* SensorHandlerOfAgent :: get_sensorSlotAddress(HistoryModule* historyModule) {
	for(auto &sensorSlot : vMountedSensors) {
		if(sensorSlot->get_historyModule() == historyModule) {
			return sensorSlot;
		}
	}
	return NULL;
}

//TODO: what to do when 2 sensorSlots have the same confidenceModule??!!
SensorSlotOfAgent* SensorHandlerOfAgent :: get_sensorSlotAddress(ConfidenceModule* confidenceModule) {
	for(auto &sensorSlot : vMountedSensors) {
		if(sensorSlot->get_confidenceModule() == confidenceModule) {
			return sensorSlot;
		}
	}
	return NULL;
}

bool SensorHandlerOfAgent :: demount_sensor(Channel* inputPort) {
	vMountedSensors.erase(vMountedSensors.begin() + get_sensorSlotNumber(inputPort));
	return false;
}

//TODO: do it also for integer variables
bool SensorHandlerOfAgent :: read_sensorValue(SensorSlotOfAgent* sensorSlotOfAgent) {
	if(sensorSlotOfAgent != NULL) {
		Channel* channel = sensorSlotOfAgent->get_comPort();
		if(channel != NULL) {
			float inputValue;
			if(channel->get_MsgUp(&inputValue)) {
				sensorSlotOfAgent->set_sensorValue(inputValue);
				return true;
			}
		}
	}
	return false;
}

bool SensorHandlerOfAgent :: read_allSensorValues() {
	bool flag_readSensor = false;
	for(auto &sensorSlot : vMountedSensors) {
		if(read_sensorValue(sensorSlot)) {
			flag_readSensor = true;
		}
	}
	return flag_readSensor;
}

bool SensorHandlerOfAgent :: attach_historyModule(Channel* inputPort, HistoryModule* historyModule) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	if(sensorSlotOfAgent != NULL) {
		return sensorSlotOfAgent->set_historyModule(historyModule);
	}
	return false;
}

bool SensorHandlerOfAgent :: detach_historyModule(SensorSlotOfAgent* sensorSlotOfAgent) {
	if(sensorSlotOfAgent != NULL) {
		return sensorSlotOfAgent->del_historyModule();
	}
	return false;
}

bool SensorHandlerOfAgent :: detach_historyModule(Channel* inputPort) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	return detach_historyModule(sensorSlotOfAgent);
}

bool SensorHandlerOfAgent :: detach_historyModule(HistoryModule* historyModule) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(historyModule);
	return detach_historyModule(sensorSlotOfAgent);
}

HistoryModule* SensorHandlerOfAgent :: get_historyModuleOfSensorSlot(Channel* inputPort) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	return sensorSlotOfAgent->get_historyModule();
}

bool SensorHandlerOfAgent :: attach_confidenceModule(Channel* inputPort, ConfidenceModule* confidenceModule) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	if(sensorSlotOfAgent != NULL) {
		return sensorSlotOfAgent->set_confidenceModule(confidenceModule);
	}
	return false;
}

bool SensorHandlerOfAgent :: detach_confidenceModule(SensorSlotOfAgent* sensorSlotOfAgent) {
	if(sensorSlotOfAgent != NULL) {
		return sensorSlotOfAgent->del_confidenceModule();
	}
	return false;
}

bool SensorHandlerOfAgent :: detach_confidenceModule(Channel* inputPort) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	return detach_confidenceModule(sensorSlotOfAgent);
}

bool SensorHandlerOfAgent :: detach_confidenceModule(ConfidenceModule* confidenceModule) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(confidenceModule);
	return detach_confidenceModule(sensorSlotOfAgent);
}

ConfidenceModule* SensorHandlerOfAgent :: get_confidenceModuleOfSensorSlot(Channel* inputPort) {
	SensorSlotOfAgent* sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
	return sensorSlotOfAgent->get_confidenceModule();
}

vector<SensorSlotOfAgent*>* SensorHandlerOfAgent :: get_vMountedSensors() {
	return &vMountedSensors;
}