#ifndef PLOTTY_H
#define PLOTTY_H

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

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

struct Color {

	uint8_t r;
	uint8_t g;
	uint8_t b;

	Color() : r(0), g(0), b(0) {
		;
	}

	static Color fromRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
		Color c; c.setRGB(r,g,b);
		return c;
	}

	static Color fromHSV(const uint8_t h, const uint8_t s, const uint8_t v) {
		Color c; c.setHSV(h,s,v);
		return c;
	}

	void setRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
		this->r = r;
		this->g = g;
		this->b = b;
	}

	void setHSV(const uint8_t h, const uint8_t s, const uint8_t v) {

		uint8_t region, remainder, p, q, t;

		region = h / 43;
		remainder = (h - (region * 43)) * 6;

		p = (v * (255 - s)) >> 8;
		q = (v * (255 - ((s * remainder) >> 8))) >> 8;
		t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;

		switch (region) {
			case 0:
				r = v; g = t; b = p;
				break;
			case 1:
				r = q; g = v; b = p;
				break;
			case 2:
				r = p; g = v; b = t;
				break;
			case 3:
				r = p; g = q; b = v;
				break;
			case 4:
				r = t; g = p; b = v;
				break;
			default:
				r = v; g = p; b = q;
				break;
		}

	}

	std::string toHEX() const {
		char buf[8];
		sprintf(buf, "#%02x%02x%02x", r, g, b);
		std::string color(buf);
		return color;
	}

};


class Plotty {

public:

	const Floorplan::IndoorMap* map;
	K::Gnuplot gp;
	K::GnuplotSplot splot;
	K::GnuplotSplotElementPoints points;
	K::GnuplotSplotElementColorPoints cpoints;

	K::GnuplotSplotElementLines pathReal;
	K::GnuplotSplotElementLines pathEst;
	K::GnuplotSplotElementPoints particles;

	K::GnuplotSplotElementLines mapOutlineGlass;
	K::GnuplotSplotElementLines mapOutlineDrywall;
	K::GnuplotSplotElementLines mapOutlineConcrete;

	std::string codeFile;

	struct Settings {
		std::vector<int> floors = {};
		bool stairs = true;
		bool obstacles = true;
		bool outline = true;
		float minZ = -9999;
		float maxZ = +9999;
	} settings;

public:

	Plotty(const Floorplan::IndoorMap* map) : map(map) {

		//gp << "set view equal xy\n";

		gp << "set palette model RGB\n";
		//gp << "set palette defined (0 '#0000ff', 1 '#ff0000')\n";
		gp << "r(x) = (x < 0) ? 0 : (x/2)\n";
		gp << "g(x) = 0\n";
		gp << "b(x) = (x > 0) ? 0 : (-x/2)\n";
		gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
		gp << "set ticslevel 0\n";

		// how to draw the floorplan
		mapOutlineConcrete.setColorHex("#888888");	mapOutlineConcrete.setLineWidth(2);
		mapOutlineDrywall.setColorHex("#888888");
		mapOutlineGlass.setColorHex("#888888");		mapOutlineGlass.setDashType(2);
		splot.add(&mapOutlineConcrete);
		splot.add(&mapOutlineDrywall);
		splot.add(&mapOutlineGlass);

		splot.add(&particles); particles.setPointSize(0.20); particles.setColorHex("#777777");

		splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#000000");
		splot.add(&pathEst); pathEst.setLineWidth(2); pathEst.setColorHex("#0000ff");

		splot.add(&points);
		points.setPointType(7);
		points.setPointSize(0.5);

		splot.add(&cpoints);
		cpoints.setPointSize(2);
		cpoints.setPointType(7);

	}

	void setGroundTruth(const Point3 pos_m) {
		gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
	}
	void setCurEst(const Point3 pos_m) {
		gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
	}


	void setPaletteRedBlue() {

		float max = -9999;
		float min = +9999;
		for (const auto& e : cpoints.get()) {
			if (e.color > max) {max = e.color;}
			if (e.color < min) {min = e.color;}
		}
		setPaletteRedBlue(min, max);

	}

	void setPaletteRedBlue(const float blueVal, const float redVal) {

		// we need to map the range from [blueVal:redVal] to [0:1]
		const float min = blueVal;
		const float max = redVal;
		const float range = (max - min);
		const float center01 = (0-min)/range;

		// values above 0 dB = red
		// values below 0 dB = blue
		gp << "set palette model RGB\n";
		gp << "cen01 = " << center01 << "\n";
		gp << "r(x) = (x < cen01) ? 0 : ((x-cen01) / (1-cen01))\n";
		gp << "g(x) = 0\n";
		gp << "b(x) = (x > cen01) ? 0 : (1 - (x/cen01))\n";
		gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
	}

	void addLabel(const std::string& txt, const Point3 pos) {
		gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n";
	}

	void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) {
		std::vector<Point3> points = {
			Point3(p1.x, p1.y, p1.z),
			Point3(p2.x, p1.y, p1.z),
			Point3(p2.x, p2.y, p1.z),
			Point3(p1.x, p2.y, p1.z),
			Point3(p1.x, p1.y, p1.z),
		};
		addPolygon(points, c.toHEX(), front, fill);
	}

	void addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true) {

		for (const Point3 p : points) {
			if (p.z < settings.minZ) {return;}
			if (p.z > settings.maxZ) {return;}
		}

		gp << "set object polygon from ";
		for (size_t i = 0; i < points.size(); ++i) {
			const Point3 p = points[i];
			if (i > 0) {gp << " to ";}
			gp << p.x << "," << p.y << "," << p.z << " ";
		}
		gp << (front ? "front" : "");
		if (fill) {gp << " fs solid ";} else {gp << " fs transparent ";}
		gp << " fc rgb " << "'" << color << "'";
		gp << "\n";

	}

	void setZRange(const float min, const float max) {
		gp << "set zrange [" << min << ":" << max << "]\n";
	}


	void addFloorRect(const Point3 pos_m, const float size, Color c) {

		const Point3 p1 = pos_m + Point3(-size, -size, 0);
		const Point3 p2 = pos_m + Point3(+size, -size, 0);
		const Point3 p3 = pos_m + Point3(+size, +size, 0);
		const Point3 p4 = pos_m + Point3(-size, +size, 0);

		std::vector<Point3> points = {p1,p2,p3,p4,p1};

		addPolygon(points, c.toHEX());

//		gp << "set object polygon from ";
//		for (size_t i = 0; i < points.size(); ++i) {
//			const Point3 p = points[i];
//			if (i > 0) {gp << " to ";}
//			gp << p.x << "," << p.y << "," << p.z << " ";
//		}
//		gp << "front fs solid fc rgb " << "'" << c.toHEX() << "'";
//		gp << "\n";

	}

	void setTitle(const std::string& title) {
		gp << "set title '" << title << "'\n";
	}

	void setGroundTruth(const std::vector<int> indices) {
		const std::vector<Point3> path = FloorplanHelper::getGroundTruth(map, indices);
		pathReal.clear();
		for (const Point3& p : path) {
			pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z));
		}
	}

	void equalXY() {
		gp << "set view equal xy\n";
	}

	void setView(const float degX, const float degY) {
		gp << "set view " << degX << "," << degY << "\n";
	}

	void setScale(const float x, const float y) {
		gp << "set multiplot layout 1,1 scale " << x << "," << y << "\n";
	}

	void writeCodeTo(const std::string& file) {
		this->codeFile = file;
	}

	void noFrame() {
		gp << "unset border\n";
		gp << "unset xtics\n";
		gp << "unset ytics\n";
		gp << "unset ztics\n";
	}

	void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
		gp.setTerminal("epslatex", size);
		gp.setOutput(file);
	}

	void plot() {
		gp.draw(splot);
		gp << "unset multiplot\n";	// scaling
		if (codeFile != "") {
			std::ofstream out(codeFile);
			out << gp.getBuffer();
			out.close();
		}
		gp.flush();
	}


	void buildFloorplan() {

		std::vector<Floorplan::Floor*> floors;

		// only some floors??
		if (settings.floors.empty()) {
			floors = map->floors;
		} else {
			for (int i : settings.floors) {
				floors.push_back(map->floors[i]);
			}
		}


		for (Floorplan::Floor* floor : floors) {

			// plot obstacles
			if (settings.obstacles) {
				for (Floorplan::FloorObstacle* obs : floor->obstacles) {
					Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
					if (line) {
						const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
						const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
						switch(line->material) {
							case Floorplan::Material::CONCRETE:		mapOutlineConcrete.addSegment(p1, p2); break;
							case Floorplan::Material::GLASS:		mapOutlineGlass.addSegment(p1, p2); break;
							case Floorplan::Material::UNKNOWN:
							case Floorplan::Material::DRYWALL:		mapOutlineDrywall.addSegment(p1, p2); break;
						}


					}
				}
			}

			// plot the floor's outline
			if (settings.outline) {
				for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
					gp << "set object polygon from ";
					for (size_t i = 0; i < poly->poly.points.size() + 1; ++i) {
						if (i > 0) {gp << " to ";}
						const Point2 pt = poly->poly.points[i % poly->poly.points.size()];		// ensures closing the polygon
						gp << pt.x << "," << pt.y << "," << floor->atHeight << " ";
					}
					gp << " fs solid fc rgb " << ( poly->outdoor ? "'#bbeebb'" : "'#dddddd'");
					gp << "\n";
				}
			}

			// plot the stairs as polygon
			if (settings.stairs) {
				for (Floorplan::Stair* s : floor->stairs) {
					std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), floor);
					for (const Floorplan::Quad3& q : quads) {
						addPolygon({q.p1, q.p2, q.p3, q.p4, q.p1}, "#c0c0c0");
					}
				}
			}

		}

	}

};

#endif // PLOTTY_H