#ifndef PLOTTI_H
#define PLOTTI_H

#include "filter/Structs.h"


#include <functional>

#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>

#include <Indoor/floorplan/v2/Floorplan.h>

#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>


#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>

#include <KLib/math/filter/particles/ParticleFilter.h>

struct Plotti {

	K::Gnuplot gp;
	K::GnuplotSplot splot;
	K::GnuplotSplotElementPoints pGrid;
	K::GnuplotSplotElementLines pFloor;
	K::GnuplotSplotElementLines pOutline;
	K::GnuplotSplotElementLines pStairs;
	K::GnuplotSplotElementPoints pAPs;
	K::GnuplotSplotElementPoints pInterest;
	K::GnuplotSplotElementPoints pParticles;
	K::GnuplotSplotElementColorPoints pColorPoints;
	K::GnuplotSplotElementLines gtPath;
    K::GnuplotSplotElementLines estPath;
    K::GnuplotSplotElementLines estPathSmoothed;

	Plotti() {
		gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
		splot.add(&pGrid);		pGrid.setPointSize(0.25);	pGrid.setColorHex("#888888");
		splot.add(&pAPs); pAPs.setPointSize(0.7);
		splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
		splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f);
		splot.add(&pFloor);
		splot.add(&pOutline); pOutline.setColorHex("#999999");
		splot.add(&pStairs); pStairs.setColorHex("#000000");
		splot.add(&pInterest); pInterest.setPointSize(2); pInterest.setColorHex("#ff0000");
		splot.add(&gtPath); gtPath.setLineWidth(2); gtPath.setColorHex("#000000");
        splot.add(&estPath); estPath.setLineWidth(2); estPath.setColorHex("#00ff00");
        splot.add(&estPathSmoothed); estPathSmoothed.setLineWidth(2); estPathSmoothed.setColorHex("#0000ff");
	}

	void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
		gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
	}

	void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
		gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
	}


	void showAngle(const int idx, const float rad) {

		Point2 cen(0.9, 0.9);
		Point2 rot(0, 1);
		Point2 pos = cen + rot.rotated(rad) * 0.05;

		gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";

	}

	void setEst(const Point3 pos) {
		gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+1 << " nohead lw 1 front \n";
	}

	void setGT(const Point3 pos) {
		gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
		gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
	}

	void addGroundTruthNode(const Point3 pos) {
		K::GnuplotPoint3 gp(pos.x, pos.y, pos.z);
		gtPath.add(gp);
	}

    // estimated path
    void addEstimationNode(const Point3 pos){
        K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
        estPath.add(est);
    }

    // estimated path
    void addEstimationNodeSmoothed(const Point3 pos){
        K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
        estPathSmoothed.add(est);
    }

	template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {

		pColorPoints.clear();

//		const float step = 2.0;
//		float z = 0;
//		for (float x = -20; x < 90; x += step) {
//			for (float y = -10; y < 60; y += step) {
//				const Point3 pos_m(x,y,z);
//				const double prob = func(obs, pos_m);
//				pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
//			}
//		}

		std::minstd_rand gen;
		std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);

		float min = +9999;
		float max = -9999;

		for (int i = 0; i < 10000; ++i) {
			int idx = dist(gen);
			Node& n = grid[idx];
			const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
			const Point3 pos_m = pos_cm / 100.0f;
			const double prob = func(obs, pos_m);
			if (prob < min) {min = prob;}
			if (prob > max) {max = prob;}
			pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
		}

		if (min == max) {min -= 1;}

		gp << "set cbrange [" << min << ":" << max << "]\n";

	}

	void addStairs(Floorplan::IndoorMap* map) {

		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::Stair* stair : f->stairs) {
				std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
					for (const Floorplan::Quad3& quad : quads) {
					for (int i = 0; i < 4; ++i) {
						int idx1 = i;
						int idx2 = (i+1) % 4;
						pStairs.addSegment(
							K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
							K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
						);
					}
				}
			}
		}

	}

	void addFloors(Floorplan::IndoorMap* map) {

		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::FloorObstacle* obs : f->obstacles) {
				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
				if (line) {
					K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
					K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
					pFloor.addSegment(p1, p2);
				}
			}
		}

	}

	void addOutline(Floorplan::IndoorMap* map) {

		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
				const int cnt = poly->poly.points.size();
				for (int i = 0; i < cnt; ++i) {
					Point2 p1 = poly->poly.points[(i+0)];
					Point2 p2 = poly->poly.points[(i+1)%cnt];
					K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
					K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
					pOutline.addSegment(gp1, gp2);
				}
			}
		}

	}

	template <typename Node> void addGrid(Grid<Node>& grid) {
		pGrid.clear();
		for (const Node& n : grid) {
			K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
			pGrid.add(p/100.0f);
		}
	}

	template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) {
		pParticles.clear();
		int i = 0;
		for (const K::Particle<State>& p : particles) {
			if (++i % 25 != 0) {continue;}
			K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
			pParticles.add(pos / 100.0f);
		}
	}

	void show() {
		gp.draw(splot);
        gp.flush();
	}

    void saveToFile(std::ofstream& stream){
        gp.draw(splot);
        stream << "set terminal x11 size 2000,1500\n";
        stream << gp.getBuffer();
        stream << "pause -1\n";
        gp.flush();
    }

    void printSingleFloor(const std::string& path, const int floorNum) {
        gp << "set terminal png size 1280,720\n";
        gp << "set output '" << path << "_" << floorNum <<".png'\n";
        gp << "set view 0,0\n";
        gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n";
        gp << "set autoscale xy\n";
    }

    void printSideView(const std::string& path, const int degree) {
        gp << "set terminal png size 1280,720\n";
        gp << "set output '" << path << "_deg" << degree <<".png'\n";
        gp << "set view 90,"<< degree << "\n";
        gp << "set autoscale xy\n";
        gp << "set autoscale z\n";
    }

    void printOverview(const std::string& path) {
        gp << "set terminal png size 1280,720\n";
        gp << "set output '" << path << "_overview" << ".png'\n";
        gp << "set view 75,60\n";
        gp << "set autoscale xy\n";
        gp << "set autoscale z\n";
    }


};

#endif // PLOTTI_H