#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H
#define INDOOR_IMU_COMPASSDETECTIONPLOT_H

#ifdef WITH_DEBUG_PLOT

	#include <KLib/misc/gnuplot/Gnuplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
	#include <KLib/misc/gnuplot/GnuplotPlot.h>
	#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
	#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>

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

	class CompassDetectionPlot {

		Timestamp plotRef;
		Timestamp lastPlot;

		K::Gnuplot gp1;
		K::Gnuplot gp2;
		//K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);

		K::GnuplotPlot plotMagRaw;
		K::GnuplotPlotElementLines lineMagRawX;
		K::GnuplotPlotElementLines lineMagRawY;
		K::GnuplotPlotElementLines lineMagRawZ;
		K::GnuplotPlotElementLines lineMagRawMagnitude;

		K::GnuplotPlot plotMagFix;
		K::GnuplotPlotElementLines lineMagFixX;
		K::GnuplotPlotElementLines lineMagFixY;
		K::GnuplotPlotElementLines lineMagFixZ;

		K::GnuplotPlot plotMagScatter;
		K::GnuplotPlotElementPoints scatterPoints;



	public:

		CompassDetectionPlot() {

			gp1 << "set autoscale xfix\n";
			gp2 << "set size ratio -1\n";

			//multiplot.add(&plotMagRaw);
			//multiplot.add(&plotMagFix);

			plotMagRaw.setTitle("Magnetometer (raw)");
			plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x");
			plotMagRaw.add(&lineMagRawY); lineMagRawY.getStroke().getColor().setHexStr("#00ff00"); lineMagRawY.setTitle("y");
			plotMagRaw.add(&lineMagRawZ); lineMagRawZ.getStroke().getColor().setHexStr("#0000ff"); lineMagRawZ.setTitle("z");
			plotMagRaw.add(&lineMagRawMagnitude); lineMagRawMagnitude.getStroke().getColor().setHexStr("#000000"); lineMagRawMagnitude.setTitle("magnitude");

			plotMagFix.setTitle("Magnetometer (re-aligned)");
			plotMagFix.add(&lineMagFixX); lineMagFixX.getStroke().getColor().setHexStr("#ff0000"); lineMagFixX.setTitle("x");
			plotMagFix.add(&lineMagFixY); lineMagFixY.getStroke().getColor().setHexStr("#00ff00"); lineMagFixY.setTitle("y");
			plotMagFix.add(&lineMagFixZ); lineMagFixZ.getStroke().getColor().setHexStr("#0000ff"); lineMagFixZ.setTitle("z");

			plotMagScatter.add(&scatterPoints);


		}

		void add(Timestamp ts, float curHeading, bool stable, const MagnetometerData& mag, const MagnetometerData& curMag) {

			if (plotRef.isZero()) {plotRef = ts;}
			const Timestamp tsPlot = (ts-plotRef);
			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);

			// raw gyro
			lineMagRawX.add( K::GnuplotPoint2(tsPlot.ms(), mag.x) );
			lineMagRawY.add( K::GnuplotPoint2(tsPlot.ms(), mag.y) );
			lineMagRawZ.add( K::GnuplotPoint2(tsPlot.ms(), mag.z) );
			lineMagRawMagnitude.add( K::GnuplotPoint2(tsPlot.ms(), mag.magnitude()) );

			// adjusted mag
			lineMagFixX.add( K::GnuplotPoint2(tsPlot.ms(), curMag.x) );
			lineMagFixY.add( K::GnuplotPoint2(tsPlot.ms(), curMag.y) );
			lineMagFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curMag.z) );

			const float len = 1;//curMag.magnitude();// std::sqrt(curMag.x*curMag.x + curMag.y*curMag.y);
			scatterPoints.add(K::GnuplotPoint2(curMag.x/len, curMag.y/len));

			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {

				lastPlot = tsPlot;

				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
				lineMagRawX.removeIf(remove);
				lineMagRawY.removeIf(remove);
				lineMagRawZ.removeIf(remove);
				lineMagRawMagnitude.removeIf(remove);
				lineMagFixX.removeIf(remove);
				lineMagFixY.removeIf(remove);
				lineMagFixZ.removeIf(remove);


				const float s = stable ? 0.1 : 0.03;
				const float ax = 0.85 + std::cos(curHeading)*s;
				const float ay = 0.85 + std::sin(curHeading)*s;
				gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
				gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";

				gp1.draw(plotMagFix);
				gp1.flush();

				gp2.draw(plotMagScatter);
				gp2.flush();

			}

		}

	};

#endif

#endif