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

#include "MagnetometerData.h"
#include "PoseProvider.h"
#include "TurnProvider.h"

#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingStdDevTS.h"
#include "../../geo/Point3.h"
#include "../../Assertions.h"
#include "../../geo/Angle.h"

#include "CompassDetectionPlot.h"

#include <cmath>
#include <vector>
#include "../../math/dsp/iir/BiQuad.h"


/**
 * custom compass implementation, similar to turn-detection
 */
class CompassDetection {

private:

	#ifdef WITH_DEBUG_PLOT
		CompassDetectionPlot plot;
	#endif

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

	MovingAverageTS<MagnetometerData> avgIn;

	//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
	MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);



	const PoseProvider* pose = nullptr;
	const TurnProvider* turn = nullptr;

	int numMagReadings = 0;

	bool stable;

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

public:

	/** ctor */
	CompassDetection(const PoseProvider* pose, const TurnProvider* turn, const Timestamp avgFrame = Timestamp::fromMS(500)) :
	    pose(pose), turn(turn), avgIn(avgFrame, MagnetometerData(0,0,0)) {
		;
	}

	/** get the current uncertainty estimation */
	float getSigma() {
		return curSigma + pose->getSigma() + turn->getSigma();
	}

	/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
	float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {

		++numMagReadings;

		// get the current magnetometer-reading as vector
		//Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z;
		const Vector3 vec(mag.x, mag.y, mag.z);

		// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
		//Eigen::Vector3f _curMag = pose->getMatrix() * vec;
		const Vector3 _curMag = pose->getMatrix() * vec;

		//avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2)));
		avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z));
		const MagnetometerData curMag = avgIn.get();
		//const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z);

		// calculate standard-deviation
		//stdDev.add(ts, curMag);
		//const float dev = std::max(stdDev.get().x, stdDev.get().y);


		// calculate angle
		// https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
		const float mx = (curMag.x);
		const float my = (curMag.y);
		const float tmp = std::atan2(my, mx);
		//const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my));

		// http://www.magnetic-declination.com/
		// http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html
		const float declination = 3.0f / 180.0f * M_PI;		// GERMANY!
		//const float declination = 0;		// GERMANY!
		float curHeading = Angle::makeSafe_2PI(- tmp + declination);

		float resHeading;
		stable = true;

		// calculate standard-deviation within a certain timeframe
		const float curDiff = Angle::getSignedDiffRAD_2PI(curHeading, lastHeading);
		stdDev.add(ts, curDiff);
		stdDevForSigma.add(ts, curDiff);
		curSigma = (5.0f/180.0f*(float)M_PI) + stdDevForSigma.get()*10;
		lastHeading = curHeading;



		// if the standard-deviation is too high,
		// the compass is considered unstable
//		if (numMagReadings < 250 || stdDev.get() > 0.30) {
//			resHeading = NAN;
//			stable = false;
//		} else {
			resHeading = curHeading;
			stable = true;
//		}

		#ifdef WITH_DEBUG_PLOT
			plot.add(ts, curHeading, stable, mag, curMag);
		#endif

		// done
		return resHeading;

	}


};

#endif // INDOOR_IMU_COMPASSDETECTION_H