#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H
#define INDOOR_IMU_TURNDETECTIONPLOT_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/GnuplotSplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>

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

	class TurnDetectionPlot {

		Timestamp plotRef;
		Timestamp lastPlot;

		K::Gnuplot gp1;

		K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);

		K::GnuplotPlot plotGyroRaw;
		K::GnuplotPlotElementLines lineGyroRawX;
		K::GnuplotPlotElementLines lineGyroRawY;
		K::GnuplotPlotElementLines lineGyroRawZ;

		K::GnuplotPlot plotGyroFix;
		K::GnuplotPlotElementLines lineGyroFixX;
		K::GnuplotPlotElementLines lineGyroFixY;
		K::GnuplotPlotElementLines lineGyroFixZ;

		K::GnuplotSplot plotPose;
		K::GnuplotSplotElementLines linePose;

		float plotCurHead = 0;

	public:

		TurnDetectionPlot() {

			gp1 << "set autoscale xfix\n";
			gp1 << "set view equal xyz\n";

			multiplot.add(&plotGyroRaw);
			multiplot.add(&plotGyroFix);

			plotGyroRaw.setTitle("Gyroscope (raw)");
			plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
			plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
			plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");

			plotGyroFix.setTitle("Gyroscope (fixed)");
			plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
			plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
			plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");

			plotPose.setTitle("Pose");
			plotPose.getView().setEnabled(false);
			plotPose.add(&linePose);
			plotPose.getAxisX().setRange(-5,+5);
			plotPose.getAxisY().setRange(-5,+5);
			plotPose.getAxisZ().setRange(-5,+5);

		}

		void addRelative(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {

			plotCurHead += delta;

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

			// raw gyro
			lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
			lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
			lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );

			// adjusted gyro
			lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) );
			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );

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

				lastPlot = tsPlot;

				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
				lineGyroRawX.removeIf(remove);
				lineGyroRawY.removeIf(remove);
				lineGyroRawZ.removeIf(remove);
				lineGyroFixX.removeIf(remove);
				lineGyroFixY.removeIf(remove);
				lineGyroFixZ.removeIf(remove);

				const float ax = 0.85 + std::cos(plotCurHead)*0.1;
				const float ay = 0.85 + std::sin(plotCurHead)*0.1;
				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(plotGyroRaw);	// raw only
				gp1.draw(plotGyroFix);	// fixed only
				//gp1.draw(multiplot);	// both

				gp1.flush();

			}

		}

		void addAbsolute(Timestamp ts, const float curHead, const GyroscopeData& gyro, const Vector3& gyroFixed) {

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

			// raw gyro
			lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
			lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
			lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );

			// adjusted gyro
			lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) );
			lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
			lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );

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

				lastPlot = tsPlot;

				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
				lineGyroRawX.removeIf(remove);
				lineGyroRawY.removeIf(remove);
				lineGyroRawZ.removeIf(remove);
				lineGyroFixX.removeIf(remove);
				lineGyroFixY.removeIf(remove);
				lineGyroFixZ.removeIf(remove);

				const float ax = 0.85 + std::cos(curHead)*0.1;
				const float ay = 0.85 + std::sin(curHead)*0.1;
				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(plotGyroRaw);	// raw only
				gp1.draw(plotGyroFix);	// fixed only
				//gp1.draw(multiplot);	// both

				gp1.flush();

			}

		}

	};

#endif

#endif // INDOOR_IMU_TURNDETECTIONPLOT_H