#ifndef TURNDETECTION_H
#define TURNDETECTION_H

#include "GyroscopeData.h"
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/Matrix3.h"

#include "../../geo/Point3.h"
#include "PoseDetection.h"

//#include <eigen3/Eigen/Dense>

#include <cmath>
#include <vector>

#include "TurnDetectionPlot.h"


#include "../../Assertions.h"

class TurnDetection {

private:

	PoseDetection* pose = nullptr;

	//std::vector<GyroscopeData> gyroData;
	//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
	Vector3 prevGyro = Vector3(0,0,0);

	Timestamp lastGyroReading;

	#ifdef WITH_DEBUG_PLOT
		TurnDetectionPlot plot;
	#endif

public:

	/** ctor */
	TurnDetection(PoseDetection* pose) : pose(pose) {
		;
	}


	// does not seem to help...
//	struct DriftEstimator {

//		MovingAverageTS<Eigen::Vector3f> avg;

//		DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
//			;
//		}

//		void removeDrift(const Timestamp ts, Eigen::Vector3f& gyro) {

//			if (gyro.norm() < 0.15) {
//				avg.add(ts, gyro);
//				gyro -= avg.get();
//			}

//		}

//	} driftEst;


	float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {

		// ignore the first reading completely, just remember its timestamp
		if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}

		// time-difference between previous and current reading
		const Timestamp curDiff = ts - lastGyroReading;
		lastGyroReading = ts;

		// fast sensors might lead to delay = 0 ms. filter those values
		if (curDiff.isZero()) {return 0.0f;}

		// ignore readings until the first orientation-estimation is available
		// otherwise we would use a wrong rotation matrix which yields wrong results!
		if (!pose->isKnown()) {return 0.0f;}

		// get the  current gyro-reading as vector
		//Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
		const Vector3 vec(gyro.x, gyro.y, gyro.z);

		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
		//Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
		const Vector3 curGyro = pose->getMatrix() * vec;
		//driftEst.removeDrift(ts, curGyro);

		// area
		//const Eigen::Vector3f area =
		const Vector3 area =

				// Trapezoid rule (should be more accurate but does not always help?!)
				//(prevGyro * curDiff.sec()) +					// squared region
				//((curGyro - prevGyro) * 0.5 * curDiff.sec());	// triangle region to the next (enhances the quality)

				// just the rectangular region
				(prevGyro * curDiff.sec());		// BEST?!


		//}

		// update the old value
		prevGyro = curGyro;

		// rotation = z-axis only!
		//const float delta = area(2);
		const float delta = area.z;

		#ifdef WITH_DEBUG_PLOT
			plot.add(ts, delta, gyro, curGyro);
		#endif

		// done
		return delta;

	}

};

#endif // TURNDETECTION_H