/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef TURNDETECTION2_H
#define TURNDETECTION2_H


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

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

//#include <eigen3/Eigen/Dense>

#include <cmath>
#include <vector>

#include "TurnDetectionPlot.h"


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

/** THIS WILL NOT WORK AS EXPECTED! */
class TurnDetection2 {

private:

	PoseProvider* pose = nullptr;

	Vector3 sumSinceStart = Vector3(0,0,0);

	Timestamp lastGyroReadingTS;
	Vector3 lastGyroReading = Vector3(0,0,0);

	bool first = true;
	Matrix3 poseMat = Matrix3::identity();

	float curRes = 0;

	#ifdef WITH_DEBUG_PLOT
		TurnDetectionPlot plot;
	#endif

public:

	/** ctor */
	TurnDetection2(PoseProvider* pose) : pose(pose) {
		;
	}


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



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

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

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

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

		        // 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
		        (lastGyroReading * curDiff.sec());		// BEST?!

		//}

		// adjust sum
		sumSinceStart = sumSinceStart + curArea;

		// update the previous value
		lastGyroReading = Vector3(gyro.x, gyro.y, gyro.z);


		// TESTING
		const float sign = (curArea.z < 0) ? (-1) : (+1);
		const float mag = curArea.norm();
		return (mag * sign);


//		// 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 curRes;}
//		if (first) {poseMat = pose->getMatrix(); first = false;}

//		// rotate the sum since start into our desired coordinate system, where the smartphone lies flat on the ground
//		const Vector3 angles = pose->getMatrix() * sumSinceStart;
//		//const Vector3 angles = poseMat * sumSinceStart;

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

//		#ifdef WITH_DEBUG_PLOT
//		    plot.addAbsolute(ts, curRes, gyro, sumSinceStart);
//		#endif

//		// done
//		return curRes;

	}

};

#endif // TURNDETECTION2_H