#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 {

//	struct LongTermTriggerAverage {

//		Eigen::Vector3f sum;
//		int cnt;

//		XYZ() {
//			reset();
//		}

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

//			// did NOT improve the result for every smartphone (only some)
//			//const float deltaMag = std::abs(acc.magnitude() - 9.81);
//			//if (deltaMag > 5.0) {return;}

//			// adjust sum and count (for average calculation)
//			Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
//			sum += vec;
//			++cnt;


//		}

//		AccelerometerData getAvg() const {
//			return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
//		}

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

//			// get the current acceleromter average
//			const Eigen::Vector3f avg = sum / cnt;

//			// rotate average accelerometer into (0,0,1)
//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
//			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();
//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

//			return rotMat;

//		}

//		/** reset the current sum etc. */
//		void reset() {
//			cnt = 0;
//			sum = Eigen::Vector3f::Zero();
//		}


//	};


	/** 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 */
//	Eigen::Matrix3f getMatrix() const {
//		return orientation.rotationMatrix;
//	}

	/** 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 Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {

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

//		const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
//		const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );

//		Eigen::Matrix3f A; A <<
//			0,		-x(2),	x(1),
//			x(2),	0,		-x(0),
//			-x(1),	x(0),	0;

//		return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));

//	}

	/** 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)));

	}





//    /** get a rotation matrix for the given x,y,z rotation (in radians) */
//    static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
//        const float g = x; const float b = y; const float a = z;
//        const float a11 = std::cos(a)*std::cos(b);
//        const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
//        const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
//        const float a21 = std::sin(a)*std::cos(b);
//        const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
//        const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
//        const float a31 = -std::sin(b);
//        const float a32 = std::cos(b)*std::sin(g);
//        const float a33 = std::cos(b)*std::cos(g);
//        Eigen::Matrix3f m;
//        m <<
//            a11,	a12,	a13,
//            a21,	a22,	a23,
//            a31,	a32,	a33;
//        ;
//        return m;
//    }

//	/** estimate the smartphones current holding position */
//	void estimateHolding2() {


//		// z-axis points through the device and is the axis we are interested in
//		// http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics

//		avgAcc = Eigen::Vector3f::Zero();

//		for (const AccelerometerData& acc : accData) {
//		//for (const GyroscopeData& acc : gyroData) {
//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
//		//	Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
//			avgAcc += vec;
//		}

//		//avgAcc /= accData.size();
//		avgAcc = avgAcc.normalized();

//		Eigen::Vector3f rev; rev << 0,0,1;

//		rotMat = getRotationMatrix(avgAcc, rev);


//		//Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
//		//Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
////		Eigen::Vector3f re = rotMat * avgAcc;
////		Eigen::Vector3f diff = rev-re;
////		Assert::isTrue(diff.norm() < 0.001, "rotation error");

//	}

//	struct RotationMatrixEstimationUsingAccAngle {

//		Eigen::Vector3f lastAvg;
//		Eigen::Vector3f avg;
//		int cnt;

//		RotationMatrixEstimationUsingAccAngle() {
//			reset();
//		}

//		void add(const float x, const float y, const float z) {

//			Eigen::Vector3f vec; vec << x,y,z;
//			avg += vec;
//			++cnt;

//		}

//		void reset() {
//			cnt = 0;
//			avg = Eigen::Vector3f::Zero();
//		}

//		Eigen::Matrix3f get() {

//			// http://www.hobbytronics.co.uk/accelerometer-info

//			avg /= cnt;
//			lastAvg = avg;

//			//const float mag = avg.norm();

//			const float baseX = 0;
//			const float baseY = 0;
//			const float baseZ = 0;	// mag?

//			const float x = avg(0) - baseX;
//			const float y = avg(1) - baseY;
//			const float z = avg(2) - baseZ;

//			const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
//			const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );

//			const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0);		// TODO -ax or +ax?

//			// sanity check
//			Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
//			Eigen::Vector3f aligned = (rotMat * avg).normalized();
//			Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
//			//			int i = 0; (void) i;

//			reset();
//			return rotMat;

//		}

//	} est;

//	struct PCA {

//		Eigen::Vector3f avg;
//		Eigen::Vector3f lastAvg;
//		Eigen::Matrix3f covar;
//		int cnt = 0;

//		PCA() {
//			reset();
//		}

//		void add(const float x, const float y, const float z) {

//			Eigen::Vector3f vec; vec << x,y,z;
//			avg += vec;
//			covar += vec*vec.transpose();
//			++cnt;

//		}

//		Eigen::Matrix3f get() {

//			avg /= cnt;
//			covar /= cnt;
//			lastAvg = avg;

//			std::cout << avg << std::endl;

//			Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
//			for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}

//			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
//			solver.eigenvalues();
//			solver.eigenvectors();

//			const auto eval = solver.eigenvalues();
//			const auto evec = solver.eigenvectors();
//			Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");

//			Eigen::Matrix3f rotMat;
//			rotMat.col(0) = evec.col(0);
//			rotMat.col(1) = evec.col(1);
//			rotMat.col(2) = evec.col(2);		// 0,0,1 (z-axis) belongs to the strongest eigenvalue
//			rotMat.transposeInPlace();

//			//Eigen::Vector3f gy; gy << 0, 30, 30;
//			Eigen::Vector3f avg1 = rotMat * avg;
//			int i = 0; (void) i;

//			reset();

//			return rotMat;

//		}

//		void reset() {
//			cnt = 0;
//			avg = Eigen::Vector3f::Zero();
//			covar = Eigen::Matrix3f::Zero();
//		}


//	} pca1;




//	/** estimate the smartphones current holding position */
//	void estimateHolding() {

//		Eigen::Vector3f avg = Eigen::Vector3f::Zero();
//		Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();

//		for (const AccelerometerData& acc : accData) {
////		for (const GyroscopeData& acc : gyroData) {
//			Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
////			Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
//			avg += vec;
//			covar += vec * vec.transpose();
//		}

//		avg /= accData.size();		// TODO
//		covar /= accData.size();	//TODO

//		avgAcc = avg.normalized();


};



#endif // INDOOR_IMU_POSEDETECTION_H