#ifndef TURNSENSOR_H
#define TURNSENSOR_H

#include <Indoor/sensors/imu/PoseDetection.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include "AccelerometerSensor.h"
#include "GyroscopeSensor.h"

struct TurnData {
	float radSinceLastEvent;
	float radSinceStart;
	TurnData() : radSinceLastEvent(0), radSinceStart(0) {;}
};

class TurnSensor :
		public SensorListener<AccelerometerData>,		// << takes
		public SensorListener<GyroscopeData>,			// << takes
		public Sensor<TurnData> {						// << provides

private:

	PoseDetection pose;
	TurnDetection turn;
	TurnData data;

	AccelerometerSensor& acc;
	GyroscopeSensor& gyro;

public:

	/** ctor */
	TurnSensor(AccelerometerSensor& acc, GyroscopeSensor& gyro) : acc(acc), gyro(gyro), turn(&pose) {
		acc.addListener(this);
		gyro.addListener(this);
	}

	void start() override {
		//
	}

	void stop() override {
		//
	}

	bool isRunning() const override {
		return acc.isRunning() && gyro.isRunning();
	}

	virtual void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
		(void) sensor;
		//turn.addAccelerometer(ts, data);
		pose.addAccelerometer(ts, data);
	}

	virtual void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
		(void) sensor;
		const float rad = turn.addGyroscope(ts, data);
		this->data.radSinceLastEvent = rad;
		this->data.radSinceStart += rad;
		informListeners(ts, this->data);
	}

};


#endif // TURNSENSOR_H