#pragma once

#include "../GGE/Utils/Point2D.h"
#include "../GGE/Utils/Bounds2D.h"
#include <vector>
#include <map>
#include <algorithm>

namespace gaming { namespace ai {

	namespace pathfinding {

		//if the returned vector has size of 0, this means that path is not found. 
		//The returned vector will contain to but not from
		//This method utilizes A*
		template <class T_>
		std::vector<gge::utils::Point> Simple4Way(const T_ &map, gge::utils::Point from, gge::utils::Point to) {
			using namespace gge::utils;

			class node {
			public:
				node(float G, const Point &from, const Point &point) : G(G), from(from), point(point)
				{ }

				node() {}

				float G;

				Point from;
				Point point;
			};

			struct pointsorter {
				bool operator ()(const Point p1, const Point p2) const {
					if(p1.y==p2.y)
						return p1.x<p2.x;
					else
						return p1.y<p2.y;
				}
			};

			auto H=[&](Point p) -> float {
				return float(abs(p.x-to.x)+abs(p.y-to.y));
			};

			std::vector<Point> points;

			if(from==to) {
				points.push_back(from);
				return points;
			}

			typedef std::pair<float, node> np;
			std::multimap<float, node> open;
			std::map<Point, node, pointsorter> opened;
			std::map<Point, bool, pointsorter> closed;

			open.insert(np(H(from),node(0,from,from)));

			Point p;
			while(1) {
				if(open.begin()==open.end()) //not found
					return std::vector<Point>();

				auto c=open.begin();
				float cG=open.begin()->second.G;
				p=open.begin()->second.point;
				Point p2;


				p2=p+Point(-1,0);
				if(p.x>0 && map(p2).AccessibleFrom(map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(+1,0);
				if(p.x<map.GetWidth()-1 && map(p2).AccessibleFrom(map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(0,-1);
				if(p.y>0 && map(p2).AccessibleFrom(map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(0,1);
				if(p.y<map.GetHeight()-1 && map(p2).AccessibleFrom(map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}

				open.erase(c);
				closed[p]=true;
			}

			points.push_back(to);
			while(p!=from) {

				points.push_back(p);
				p=opened[p].from;
			}

			std::reverse(points.begin(),points.end());
			return points;
		}

		//if the returned vector has size of 0, this means that path is not found. 
		//The returned vector will contain to but not from
		//This method utilizes A*
		template <class T_, class O_>
		std::vector<gge::utils::Point> Extended4Way(const T_ &map, gge::utils::Point from, gge::utils::Point to, O_ &object, gge::utils::Bounds limits, bool reversed) {
			using namespace gge::utils;

			limits.Left=gge::utils::Limit(limits.Left, 0, map.GetWidth());
			limits.Right=gge::utils::Limit(limits.Right, 0, map.GetWidth());
			limits.Top=gge::utils::Limit(limits.Top, 0, map.GetHeight());
			limits.Bottom=gge::utils::Limit(limits.Bottom, 0, map.GetHeight());

			class node {
			public:
				node(float G, const Point &from, const Point &point) : G(G), from(from), point(point)
				{ }

				node() {}

				float G;

				Point from;
				Point point;
			};

			struct pointsorter {
				bool operator ()(const Point p1, const Point p2) const {
					if(p1.y==p2.y)
						return p1.x<p2.x;
					else
						return p1.y<p2.y;
				}
			};

			auto H=[&](Point p) -> float {
				return float(abs(p.x-to.x)+abs(p.y-to.y));
			};

			std::vector<Point> points;

			if(from==to) {
				points.push_back(from);
				return points;
			}

			typedef std::pair<float, node> np;
			std::multimap<float, node> open;
			std::map<Point, node, pointsorter> opened;
			std::map<Point, bool, pointsorter> closed;

			open.insert(np(H(from),node(0,from,from)));

			Point p;
			while(1) {
				if(open.begin()==open.end()) //not found
					return std::vector<Point>();

				auto c=open.begin();
				float cG=open.begin()->second.G;
				p=open.begin()->second.point;
				Point p2;


				p2=p+Point(-1,0);
				if(p.x>limits.Left && map(p2).AccessibleByFrom(object, map, p, map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(+1,0);
				if(p.x<limits.Right-1 && map(p2).AccessibleByFrom(object, map, p, map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(0,-1);
				if(p.y>limits.Top && map(p2).AccessibleByFrom(object, map, p, map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}
				p2=p+Point(0,1);
				if(p.y<limits.Bottom-1 && map(p2).AccessibleByFrom(object, map, p, map(p)) && !closed[p2]) {
					if(!opened.count(p2) || opened[p2].G>cG+1) {
						node n(cG+1.f, p, p2);
						open.insert(np(cG+1.f+H(p2),n));
						opened[p2]=n;

						if(p2==to) break;
					}
				}

				open.erase(c);
				closed[p]=true;
			}

			points.push_back(to);
			while(p!=from) {

				points.push_back(p);
				p=opened[p].from;
			}

			if(!reversed)
				std::reverse(points.begin(),points.end());

			return points;
		}

	}
} }
