#ifndef INDOOR_IMU_POSEDETECTION_H
#define INDOOR_IMU_POSEDETECTION_H

#include "AccelerometerData.h"

#include "../../data/Timestamp.h"

#include "../../math/MovingAverageTS.h"
#include "../../math/MovingMedianTS.h"
#include "../../math/Matrix3.h"

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

//#include <eigen3/Eigen/Dense>

#include "PoseDetectionPlot.h"

/**
 * estimate the smartphones world-pose,
 * based on the accelerometer's data
 */
class PoseDetection {


	/** live-pose-estimation using moving average of the smartphone's accelerometer */
	struct EstMovingAverage {

		// average the accelerometer
		MovingAverageTS<AccelerometerData> avg;

		EstMovingAverage(const Timestamp window) :
			avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {

			// start approximately
			addAcc(Timestamp(), AccelerometerData(0,0,9.81));

		}

		/** add the given accelerometer reading */
		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
			avg.add(ts, acc);
		}

		AccelerometerData getBase() const {
			return avg.get();
		}

		/** get the current rotation matrix estimation */
		//Eigen::Matrix3f get() const {
		Matrix3 get() const {

			// get the current acceleromter average
			const AccelerometerData avgAcc = avg.get();
			//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
			const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);

			// rotate average-accelerometer into (0,0,1)
			//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
			const Vector3 zAxis(0,0,1);
			const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
			//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized());	// INVERSE
			//const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);

			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
			//Eigen::Vector3f aligned = (rotMat * avg).normalized();
			const Vector3 aligned = (rotMat * avg).normalized();
			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

			return rotMat;

		}

	};

//	/** live-pose-estimation using moving median of the smartphone's accelerometer */
//	struct EstMovingMedian {

//		// median the accelerometer
//		MovingMedianTS<float> medianX;
//		MovingMedianTS<float> medianY;
//		MovingMedianTS<float> medianZ;

//		EstMovingMedian(const Timestamp window) :
//			medianX(window), medianY(window), medianZ(window) {

//			// start approximately
//			addAcc(Timestamp(), AccelerometerData(0,0,9.81));

//		}

//		/** add the given accelerometer reading */
//		void addAcc(const Timestamp ts, const AccelerometerData& acc) {
//			medianX.add(ts, acc.x);
//			medianY.add(ts, acc.y);
//			medianZ.add(ts, acc.z);
//		}

//		AccelerometerData getBase() const {
//			return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
//		}

//		/** get the current rotation matrix estimation */
//		//Eigen::Matrix3f get() const {
//		Matrix3 get() const {

//			const Vector3 base(medianX.get(), medianY.get(), medianZ.get());

//			// rotate average-accelerometer into (0,0,1)
//			const Vector3 zAxis(0,0,1);
//			const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);

//			// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
//			const Vector3 aligned = (rotMat * base).normalized();
//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

//			return rotMat;

//		}

//	};



private:

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

	/** how the pose is estimated */
	//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
	EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450));
	//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));

	#ifdef WITH_DEBUG_PLOT
		PoseDetectionPlot plot;
	#endif


public:

	/** ctor */
	PoseDetection() {
		;
	}

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

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

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

		// add accelerometer data
		est.addAcc(ts, acc);

		// update (if needed)
		orientation.rotationMatrix = est.get();
		orientation.isKnown = true;
		orientation.lastEstimation = ts;

		// debug-plot (if configured)
		#ifdef WITH_DEBUG_PLOT
			plot.add(ts, est.getBase(), orientation.rotationMatrix);
		#endif

	}

public:


	/** get a matrix that rotates the vector "from" into the vector "to" */
	static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {

		// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another

		const Vector3 v = from.cross(to) / from.cross(to).norm();
		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );

		Matrix3 A({
			0.0f,	-v.z,	v.y,
			v.z,	0.0f,	-v.x,
			-v.y,	v.x,	0.0f
		});

		return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));

	}

};



#endif // INDOOR_IMU_POSEDETECTION_H