Sensor.h

#pragma once

#include "Direction.h"
#include "CoordSet.h"
#include "Logger.h"
#include <vector>

// Ping sensor pins
#define PIN_PING_SENSOR_LEFT 6
#define PIN_PING_SENSOR_RIGHT 7
#define PIN_PING_SENSOR_FRONT 8

// When should a measurement be seen as a deviation? latest measurement + or - the threshold compared to the previous measurement
#define SENSOR_DEVIATION_THRESHOLD 2

// The amount of measurements each sensor should keep
#define SENSOR_MAX_VECTOR_SIZE 6

class Sensor{
public:
	Sensor(int distanceToCenter, Direction orientation);
	~Sensor();

	void update(int measurement);

	float calcAverage(); // Calculates average without deviations

	int getLastMeasurement();
	int getPrevMeasurement();

	Direction getOrientation();

	bool isLastMeasurementDeviating();

	CoordSet getWallPos(const CoordSet currentPos, const Direction k9Orientation);

	bool getThreadSafeLock();
	void setThreadSafeLock(bool lock);
private:
	int vectorIndex;

	int distanceToCenter;

	std::vector<int> measurementVector;

	Direction orientation;

	bool threadSafeLock; // To make sure the main thread can process a sensor deviation before the sensorthread overwrites it with a new measurement
protected:
	int nextIndex(int lastIndex);
	int prevIndex(int lastIndex);
};