#ifndef DEBUGSHORTESTPATH_H
#define DEBUGSHORTESTPATH_H

#include <Indoor/grid/walk/GridWalkShortestPathControl.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 "../Helper.h"
#include "../Vis.h"

template <typename T> class DebugShortestPath : public GridWalkShortestPathControl<T> {

private:

	Vis vis;


public:

	/** ctor */
	template <typename Access> DebugShortestPath(Grid<T>& grid, const Access& acc, const T& target, Helper::FHWSFloors& floors) : GridWalkShortestPathControl<T>(grid, acc, target) {

		vis.particles.setColorHex("#0000ff");
		vis.particles.setPointSize(1.5);

		vis.addFloor(floors.f0, floors.h0);
		vis.addFloor(floors.f1, floors.h1);
		vis.addFloor(floors.f2, floors.h2);
		vis.addFloor(floors.f3, floors.h3);



	}

	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {

		GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);

		if (this->recalc == 0){
			vis.estPath.clear();
			vis.particles.clear();
			vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z));
			const int advance = this->stdDevDist / grid.getGridSize_cm();
			const T& d = *this->path->getFromStart(advance).element;
			vis.particles.add(K::GnuplotPoint3(d.x_cm, d.y_cm, d.z_cm));
			for (int i = 0; i < (int)this->path->size()-1; ++i) {
				const DijkstraNode<T>& dn1 = (*this->path)[i+0];
				const DijkstraNode<T>& dn2 = (*this->path)[i+1];
				K::GnuplotPoint3 p1 (dn1.element->x_cm, dn1.element->y_cm, dn1.element->z_cm);
				K::GnuplotPoint3 p2 (dn2.element->x_cm, dn2.element->y_cm, dn2.element->z_cm);
				vis.estPath.addSegment(p1, p2);
			}
			vis.show();

		}


		return s;

	}

};

#endif // DEBUGSHORTESTPATH_H