#ifndef POSEDETECTION3_H
#define POSEDETECTION3_H

#include "AccelerometerData.h"
#include "GyroscopeData.h"

#include "../../data/Timestamp.h"
#include "../../math/Matrix3.h"
#include "../../math/Quaternion.h"

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

#include "PoseDetectionPlot.h"
#include "PoseProvider.h"


class PoseDetection3 : public PoseProvider {

private:

	struct {
		//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
		Matrix3 rotationMatrix = Matrix3::identity();
		bool isKnown = false;
		Timestamp lastEstimation;
	} orientation;

	#ifdef WITH_DEBUG_PLOT
		PoseDetectionPlot plot;
		PoseDetectionPlotAngles plotAngles;
	#endif

	Vector3 lastGyroReading;
	Timestamp lastGyroReadingTS;

public:

	/** ctor */
	PoseDetection3() {
		#ifdef WITH_DEBUG_PLOT
			plot.setName("PoseDetection3");
		#endif
	}

	/** get the smartphone's rotation matrix */
	const Matrix3& getMatrix() const override {
		return orientation.rotationMatrix;
	}

	/** is the pose known and stable? */
	bool isKnown() const override {
		return orientation.isKnown;
	}


	bool first = true;

	Quaternion qGyro = Quaternion(1, 0,0,0);
	Quaternion qGyroUpdate;
	Quaternion qAccel;
	Quaternion qFiltered;

	void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {

		const Vector3 vec(-acc.x, -acc.y, acc.z);
		const Vector3 v = vec.normalized();


		const float mag = -std::acos(v.dot(Vector3(0,0,1)));//-std::acos(v.z); << same
		const Vector3 n = Vector3(v.y, -v.x, 0).normalized();
		qAccel = Quaternion::fromAxisAngle(mag, n.x, n.y, n.z);

		//const float magY = std::atan2(acc.x, acc.z);
		//const float magX = std::atan2(acc.y, acc.z);
		//qAccel = Quaternion::fromAxisAngle(magX, 1, 0, 0) * Quaternion::fromAxisAngle(-magY, 0, 1, 0);



		//Vector3 dv = qAccel.toEuler();
		//dv.z = 0;
		//std::cout << dv.x << " " << dv.y << " " << dv.z << std::endl;

		// for plotting
		if (first) {
			qGyro = qAccel;
			first = false;
		}

		int i = 0; (void) i;

	}




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

		Vector3 vec(gyro.x, gyro.y, gyro.z);

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

		// 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;}

		// current area
		//const Vector3 curArea =	(lastGyroReading * curDiff.sec());


		// length of the rotation vector
		const float theta =  curDiff.sec() * lastGyroReading.norm();
		const Vector3 v = lastGyroReading.normalized();
		lastGyroReading = vec;

		if (theta == 0) {return;}
		qGyroUpdate = Quaternion::fromAxisAngle(theta, v.x, v.y, v.z);// qUpdate(cos(theta/2),		v.x * sin(theta/2), v.y * sin(theta/2), v.z * sin(theta/2));

		// update
		qGyro = qGyroUpdate * qGyro;



		update(ts);

	}

	Matrix3 getMatrixAcc() const {
		Vector3 r = qAccel.toEuler();
		return Matrix3::getRotationRad(r.x, r.y, r.z);
	}
	Matrix3 getMatrixGyro() const {
		Vector3 r = qGyro.toEuler();
		return Matrix3::getRotationRad(r.x, r.y, r.z);
	}

private:

	Vector3 curAngle_rad;
	int cnt = 0;

	void update(const Timestamp ts) {


		//Quaternion qFused = qGyro;
		//qFiltered = qAccel;
		//Quaternion qFused = (qGyro * alpha) + (qAccel * (1-alpha));
		qFiltered = Quaternion::lerp(qFiltered*qGyroUpdate, qAccel, 0.05);
		//qFiltered = Quaternion::fromAxisAngle(qAccel.w*0.02, qAccel.x, qAccel.y, qAccel.z) * (qFiltered * qGyroUpdate) * 0.98;

		Vector3 fused = qFiltered.toEuler();
		//std::cout << fused.x <<" " << fused.y << " " << fused.z << std::endl;

		// update
		orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, fused.z); //getMatrixFor(cur);


		orientation.isKnown = true;
		orientation.lastEstimation = ts;

		// debug-plot (if configured)
		#ifdef WITH_DEBUG_PLOT
		    plot.add(ts, Vector3(0,0,0), orientation.rotationMatrix);
			if (++cnt % 2 == 0) {
				plotAngles.addAcc(ts, qAccel.toEuler().x, qAccel.toEuler().y);
				plotAngles.addGyro(ts, qGyro.toEuler().x, qGyro.toEuler().y);
				plotAngles.addFused(ts, fused.x, fused.y);
			}
		#endif

	}


};

#endif // POSEDETECTION3_H