/*
 * © 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 TURNDETECTION_H
#define TURNDETECTION_H

#include "GyroscopeData.h"
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingStdDevTS.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"
#include "TurnProvider.h"

class TurnDetection : public TurnProvider {

private:

	PoseProvider* pose = nullptr;

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

	Timestamp lastGyroReading;

	float curSigma = 0;

	MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);

	#ifdef WITH_DEBUG_PLOT
		TurnDetectionPlot plot;
	#endif

public:

	/** ctor */
	TurnDetection(PoseProvider* 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;


	/** get the current uncertainty estimation */
	float getSigma() const override {
		return curSigma;
	}

	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)

		    // average (is the same as above)
		    //((curGyro+prevGyro)/2 * curDiff.sec());

		    // 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.addRelative(ts, delta, gyro, curGyro);
		#endif

		//stdDevForSigma.add(ts, prevGyro.z);	// ignore delta T. directly us radians-per-sec as sigma
		//curSigma = stdDevForSigma.get();
		const float radPerSec = 1.0f / 180.0f * M_PI;
		curSigma = radPerSec + std::abs(prevGyro.z * 0.05);	// constant of 1deg/sec + 5% of current turn rate

		// done
		return delta;

	}

};

#endif // TURNDETECTION_H