#ifndef POSEDETECTION2_H
#define POSEDETECTION2_H


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

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

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

#include "../../math/filter/Complementary.h"

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

//#include "../../math/FixedFrequencyInterpolator.h"

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

/**
 * estimate the smartphones world-pose,
 * based on the accelerometer's data
 *
 * https://robotics.stackexchange.com/questions/6953/how-to-calculate-euler-angles-from-gyroscope-output
 */
class PoseDetection2 : public PoseProvider {

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;
		PoseDetectionPlotAngles plotAngles;
	#endif

	Vector3 lastGyroReading;
	Timestamp lastGyroReadingTS;

	//FixedFrequencyInterpolator<Vector3> interpolAcc;
	//FixedFrequencyInterpolator<Vector3> interpolGyro;

	Filter::Complementary<Vector3> filter;

	//std::vector<Vector3> accBuf;
	//std::vector<Vector3> gyroBuf;



	Vector3 curAccel;

	bool firstAccel = true;
	Vector3 curAccel_rad;
	Vector3 curGyroSum_rad;

	//Matrix3 curGyroMatrix = Matrix3::identity();

	static constexpr float splitFreq = 2.5;
	static constexpr float sRate = 50;
	static constexpr int sRateTS = 1000/sRate;


public:

	/** ctor */
	//PoseDetection2() : interpolAcc(Timestamp::fromMS(sRateTS)), interpolGyro(Timestamp::fromMS(sRateTS)), filter(splitFreq, sRate) {
	PoseDetection2() : filter(1, 2.5) {
        #ifdef WITH_DEBUG_PLOT
		    plot.setName("PoseDetection2");
        #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;
	}


	float angleX_old_rad = 0;
	float angleY_old_rad = 0;


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

		// https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/#docs

		float angleX_rad = std::atan2( acc.y, acc.z);
		float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
		float angleZ_rad = 0;

		if (std::abs(angleX_rad-angleX_old_rad) > M_PI) {angleX_rad += 2*M_PI;}
		if (std::abs(angleY_rad-angleY_old_rad) > M_PI) {angleY_rad += 2*M_PI;}

		angleX_old_rad = angleX_rad;
		angleY_old_rad = angleY_rad;

//		float angleX_rad = std::atan2(acc.y, std::sqrt(acc.x*acc.x + acc.z*acc.z));
//		float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
//		float angleZ_rad = 0;

//		Point2 ref(0,1);
//		Point2 xz( acc.x, acc.z);
//		Point2 yz(-acc.y, acc.z);
//		float angleY_rad = std::atan2( determinant(ref,xz), dot(ref,xz) );
//		float angleX_rad = std::atan2( determinant(ref,yz), dot(ref,yz) );
//		float angleZ_rad = 0.0;

//		float angleX_rad =  std::atan2(acc.y, acc.z);
//		float angleY_rad = -std::atan2(acc.x, acc.z);
//		float angleZ_rad = 0;//-std::atan2(acc.y, acc.x);


		//		float angleX2_rad = (angleX1_rad > 0) ? (angleX1_rad - 2*M_PI) : (angleX1_rad + 2*M_PI);
//		float angleY2_rad = (angleY1_rad > 0) ? (angleY1_rad - 2*M_PI) : (angleY1_rad + 2*M_PI);
//		float angleX_rad = (std::abs(curAngle_rad.x-angleX1_rad) < std::abs(curAccel_rad.x-angleX2_rad)) ? (angleX1_rad) : (angleX2_rad);
//		float angleY_rad = (std::abs(curAngle_rad.y-angleY1_rad) < std::abs(curAccel_rad.y-angleY2_rad)) ? (angleY1_rad) : (angleY2_rad);

		curAccel = Vector3(acc.x, acc.y, acc.z);
		curAccel_rad = Vector3(angleX_rad, angleY_rad, angleZ_rad);

		//curAccel_rad = Matrix3::getRotationRad(0,0,-angleZ_rad) * Vector3(angleX_rad, angleY_rad, angleZ_rad);


		if (firstAccel) {
			curGyroSum_rad = curAccel_rad;
			firstAccel = false;
		}



	}




	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());

//		// update sum
//		curAngle_rad += curArea;
//		curGyroSum_rad += curArea;

//		// PAPER
		const float dx = 1 * lastGyroReading.x +	std::sin(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.y +	std::cos(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
		const float dy =							std::cos(curGyroSum_rad.x)*lastGyroReading.y +															-std::sin(curGyroSum_rad.x)*lastGyroReading.z;
		const float dz =							std::sin(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.y +								std::cos(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
		curGyroSum_rad.x += dx*curDiff.sec();
		curGyroSum_rad.y += dy*curDiff.sec();
		curGyroSum_rad.z += dz*curDiff.sec();


//		// PAPER
//		const Vector3 n = lastGyroReading / lastGyroReading.norm();
//		const float mag = lastGyroReading.norm() * curDiff.sec();
//		if (mag != 0) {
//			curGyroMatrix = Matrix3::getRotationVec(n.x, n.y, n.z, mag).transposed() * curGyroMatrix;
//		}

		// DEBUG PLOT


		// update old reading
		lastGyroReading = vec;

		update(ts);



	}

	Matrix3 getMatrixGyro() const {
		return Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
	}

	Matrix3 getMatrixAcc() const {
		return Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z);
	}

private:

	Vector3 curAngle_rad;
	int cnt = 0;

	void update(const Timestamp ts) {


		// complementary filter		x = alpha * x + (1-alpha) * y;
		const float alpha = 0.98f;
		//curAngle_rad = curAngle_rad*alpha + curAccel_rad*(1-alpha);
		//curAngle_rad = curAccel_rad;
		//curAngle_rad = curGyroSum_rad;

		//std::cout << curGyroSum_rad.x <<" " << curGyroSum_rad.y << " " << curGyroSum_rad.z << std::endl;

		//curAngle_rad = filter.get();

		filter.addSlow(ts, curAccel_rad);
		filter.addFast(ts, curGyroSum_rad);

		static Vector3 fused;
		static Vector3 pref;
		Vector3 diff = curGyroSum_rad - pref;
		fused = (fused+diff) * alpha + curAccel_rad * (1-alpha);
		pref = curGyroSum_rad;

		// update
		//orientation.rotationMatrix = Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z); //getMatrixFor(cur);
		//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); //getMatrixFor(cur);
		//orientation.rotationMatrix = curGyroMatrix;
		//orientation.rotationMatrix = Matrix3::getRotationRadZ(curGyroSum_rad.z) * Matrix3::getRotationRadX(curGyroSum_rad.x) * Matrix3::getRotationRadY(curGyroSum_rad.y);
		//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
		orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, 0); //getMatrixFor(cur);


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

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

	}

//	/** get the current rotation matrix estimation */
//	Matrix3 getMatrixFor(const Vector3 cur) const {

//		// rotate average-accelerometer into (0,0,1)
//		//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
//		const Vector3 zAxis(0,0,1);
//		const Matrix3 rotMat = getRotationMatrix(cur.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 * cur).normalized();
//		Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");

//		return rotMat;

//	}

//	/** 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 // POSEDETECTION2_H