Sensor.cpp

#include "Sensor.h"

Sensor::Sensor(int distanceToCenter, Direction orientation){
	this->distanceToCenter = distanceToCenter;
	this->orientation = orientation;

	this->vectorIndex = 0;
	this->threadSafeLock = false;

	// Fill vector with dummy measurements
	for(int i = 0; i < SENSOR_MAX_VECTOR_SIZE; i++)
		this->measurementVector.push_back(0);

	Logger::getInstance().log("Sensor object constructed");
}

Sensor::~Sensor(){
	Logger::getInstance().log("Sensor object destroyed");
}

void Sensor::update(int measurement){
	// Find index of the oldest measurement
	int indexToOverwrite;

	if(this->vectorIndex - 1 == -1)
		indexToOverwrite = SENSOR_MAX_VECTOR_SIZE - 1;
	else indexToOverwrite = this->vectorIndex - 1;

	// Overwrite oldest measurement	
	this->measurementVector[indexToOverwrite] = measurement + this->distanceToCenter;

	// Update vectorIndex to point to the latest measurement
	this->vectorIndex = indexToOverwrite;
}

float Sensor::calcAverage(){
	int index = this->vectorIndex;

	for(int i = 0; i < SENSOR_MAX_VECTOR_SIZE; i++){
		// Gets the index to use
		if(i != 0)
			index = this->nextIndex(index);

		// If last measurement is not deviating


		// TODO


	}
}

int Sensor::getLastMeasurement(){
	return this->measurementVector[vectorIndex];
}

int Sensor::getPrevMeasurement(){
	// Find index of previous index
	int indexOfPrevMeasurement;

	if(vectorIndex + 1 == SENSOR_MAX_VECTOR_SIZE)
		indexOfPrevMeasurement = 0;
	else indexOfPrevMeasurement = vectorIndex + 1;

	return this->measurementVector[indexOfPrevMeasurement];
}

Direction Sensor::getOrientation(){
	return this->orientation;
}

bool Sensor::isLastMeasurementDeviating(){
	// isDeviating when last measurement is higher/lower then the previous measurement by SENSOR_DEVIATION_THRESHOLD
	int prevMeasurement = this->getPrevMeasurement();

	if(this->getLastMeasurement() > (prevMeasurement + SENSOR_DEVIATION_THRESHOLD) || this->getLastMeasurement() < (prevMeasurement - SENSOR_DEVIATION_THRESHOLD))
		return true;

	return false;
}

CoordSet Sensor::getWallPos(const CoordSet currentPos, const Direction k9Orientation){
	CoordSet wallPos;

	// Calculate point in wall based on sensor orientation, k9 orientation and the current position 
	if(this->orientation == NORTH){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y + this->getLastMeasurement();
					break;
		case EAST:	wallPos.x = currentPos.x + this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case SOUTH:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y - this->getLastMeasurement();
					break;
		case WEST:	wallPos.x = currentPos.x - this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		}
	}	
	if(this->orientation == EAST){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	wallPos.x = currentPos.x + this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case EAST:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y - this->getLastMeasurement();
					break;
		case SOUTH:	wallPos.x = currentPos.x - this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case WEST:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y + this->getLastMeasurement();
					break;
		}
	}
	if(this->orientation == SOUTH){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y - this->getLastMeasurement();
					break;
		case EAST:	wallPos.x = currentPos.x - this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case SOUTH:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y + this->getLastMeasurement();
					break;
		case WEST:	wallPos.x = currentPos.x + this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		}	
	}
	if(this->orientation == WEST){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	wallPos.x = currentPos.x - this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case EAST:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y + this->getLastMeasurement();
					break;
		case SOUTH:	wallPos.x = currentPos.x + this->getLastMeasurement();
					wallPos.y = currentPos.y;
					break;
		case WEST:	wallPos.x = currentPos.x;
					wallPos.y = currentPos.y - this->getLastMeasurement();
					break;
		}		
	}

	return wallPos;
}

bool Sensor::getThreadSafeLock(){
	return this->threadSafeLock;
}

void Sensor::setThreadSafeLock(bool lock){
	this->threadSafeLock = lock;
}

int Sensor::nextIndex(int lastIndex){
	int nextIndex = lastIndex++;

	// Check if nextIndex is not pointing outside the array, if it does, nextIndex is 0
	if(nextIndex < SENSOR_MAX_VECTOR_SIZE)
		return nextIndex;
	else
		return 0;
}

int Sensor::prevIndex(int lastIndex){
	int prevIndex = lastIndex--;

	// Check if prevIndex is not pointing outside the array, if it does, prevIndex is SENSOR_MAX_VECTOR_SIZE - 1
	if(prevIndex > -1)
		return prevIndex;
	else
		return SENSOR_MAX_VECTOR_SIZE - 1;
}