#ifndef PLOTTI_H
#define PLOTTI_H

#include "Settings.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 <Indoor/smc/filtering/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::GnuplotSplotElementPoints pNormal1;
    K::GnuplotSplotElementPoints pNormal2;
    K::GnuplotSplotElementColorPoints pDistributation1;
    K::GnuplotSplotElementColorPoints pDistributation2;
	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.getColor().setHexStr("#888888");
		splot.add(&pAPs); pAPs.setPointSize(0.7);
		splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
        splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
        splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
        splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f);
        splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f);
        splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f);
		splot.add(&pFloor);
        splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
        splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000");
        splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000");
        splot.add(&gtPath); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000");
        splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00");
        splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#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, const Point2 cen, const std::string& str) {

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

        gp << "set label "<<idx<<" at screen " << cen.x << "," << cen.y << " '" << str << "'"<< "\n";
		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 << "," << std::round(pos.z * 10) / 10  << " to " << pos.x << "," << pos.y << "," << (std::round(pos.z * 10) / 10)+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 setTimeInMinute(const int minutes, const int seconds) {
        gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
    }

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

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

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

    void debugDistribution1(std::vector<SMC::Particle<MyState>> samples){

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

        pDistributation1.clear();
        for (int i = 0; i < samples.size(); ++i) {
            //if (i % 10 != 0) {continue;}

            double prob = samples[i].weight;
            if (prob < min) {min = prob;}
            if (prob > max) {max = prob;}

            K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
            pDistributation1.add(pos, prob);
        }

        if (min == max) {min -= 1;}
        gp << "set cbrange [" << min << ":" << max << "]\n";
    }

    void debugDistribution2(std::vector<SMC::Particle<MyState>> samples){

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

        pDistributation2.clear();
        for (int i = 0; i < samples.size(); ++i) {
            if (i % 25 != 0) {continue;}

            double prob = samples[i].weight;
            if (prob < min) {min = prob;}
            if (prob > max) {max = prob;}

            K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
            pDistributation2.add(pos, prob);
        }

        if (min == max) {min -= 1;}
        gp << "set cbrange [" << min << ":" << max << "]\n";
    }

    void drawNormalN1(Distribution::NormalDistributionN normParticle){

        pNormal1.clear();
        for (int i = 0; i < 100000; ++i) {
            if (++i % 25 != 0) {continue;}
            Eigen::VectorXd vec = normParticle.draw();
            K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
            pNormal1.add(pos);
        }

    }

    void drawNormalN2(Distribution::NormalDistributionN normParticle){

        pNormal2.clear();
        for (int i = 0; i < 100000; ++i) {
            if (++i % 25 != 0) {continue;}
            Eigen::VectorXd vec = normParticle.draw();
            K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
            pNormal2.add(pos);
        }
    }

    void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) {

        WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
        const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan);

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

        const float step = 2.0f;
        for (float x = 0; x < 80; x += step) {
            for (float y = 0; y < 55; y += step) {
                Point3 pt(x,y,z);
                double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs);

                if (prob < min) {min = prob;}
                if (prob > max) {max = prob;}
                pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
            }
        }

        if (min == max) {min -= 1;}
        gp << "set cbrange [" << min << ":" << max << "]\n";

    }

	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<SMC::Particle<State>>& particles) {
		pParticles.clear();
		int i = 0;
		for (const SMC::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