/*
	OneLoneCoder.com - Path Finding #2 - Wave Propagation & Potential Fields
	"...never get lost again, so long as you know where you are" - @Javidx9


	Background
	~~~~~~~~~~
	A nice follow up alternative to the A* Algorithm. Wave propagation is less
	applicable to multiple objects with multiple destinations, but fantatsic
	for multiple objects all reaching the same destination.

	WARNING! This code is NOT OPTIMAL!! It is however very robust. There
	are many ways to optimise this further.

	License (OLC-3)
	~~~~~~~~~~~~~~~

	Copyright 2018 OneLoneCoder.com

	Redistribution and use in source and binary forms, with or without
	modification, are permitted provided that the following conditions
	are met:

	1. Redistributions or derivations of source code must retain the above
	copyright notice, this list of conditions and the following disclaimer.

	2. Redistributions or derivative works in binary form must reproduce
	the above copyright notice. This list of conditions and the following
	disclaimer must be reproduced in the documentation and/or other
	materials provided with the distribution.

	3. Neither the name of the copyright holder nor the names of its
	contributors may be used to endorse or promote products derived
	from this software without specific prior written permission.

	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
	"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
	LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
	A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
	HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
	SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
	LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
	DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
	THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
	(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
	OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

	Links
	~~~~~
	YouTube:	https://www.youtube.com/javidx9
	Discord:	https://discord.gg/WhwHUMV
	Twitter:	https://www.twitter.com/javidx9
	Twitch:		https://www.twitch.tv/javidx9
	GitHub:		https://www.github.com/onelonecoder
	Patreon:	https://www.patreon/javidx9
	Homepage:	https://www.onelonecoder.com

	Relevant Videos
	~~~~~~~~~~~~~~~
	Part #1 https://youtu.be/icZj67PTFhc
	Part #2 https://youtu.be/0ihciMKlcP8

	Author
	~~~~~~
	David Barr, aka javidx9, ŠOneLoneCoder 2018
*/
#define OLC_PGE_APPLICATION
#include "olcPixelGameEngine.h"

#include <vector>
#include <list>
#include <algorithm>
#include <utility>


// Override base class with your custom functionality
class PathFinding_FlowFields : public olc::PixelGameEngine
{
public:
	PathFinding_FlowFields()
	{
		sAppName = "PathFinding - Flow Fields";
	}

private:
	int nMapWidth;
	int nMapHeight;
	int nCellSize;
	int nBorderWidth;

	bool *bObstacleMap;

	int *nFlowFieldZ;
	float *fFlowFieldY;
	float *fFlowFieldX;

	int nStartX;
	int nStartY;
	int nEndX;
	int nEndY;

	int nWave = 1;

public:
	bool OnUserCreate() override
	{
		nBorderWidth = 4;
		nCellSize = 32;
		nMapWidth = ScreenWidth() / nCellSize;
		nMapHeight = ScreenHeight() / nCellSize;
		bObstacleMap = new bool[nMapWidth * nMapHeight]{ false };
		nFlowFieldZ = new int[nMapWidth * nMapHeight]{ 0 };
		fFlowFieldX = new float[nMapWidth * nMapHeight]{ 0 };
		fFlowFieldY = new float[nMapWidth * nMapHeight]{ 0 };

		nStartX = 3;
		nStartY = 7;
		nEndX = 12;
		nEndY = 7;
		return true;
	}

	bool OnUserUpdate(float fElapsedTime) override
	{
		// Little convenience lambda 2D -> 1D
		auto p = [&](int x, int y) { return y * nMapWidth + x;  };

		// User Input
		int nSelectedCellX = GetMouseX() / nCellSize;
		int nSelectedCellY = GetMouseY() / nCellSize;

		if (GetMouse(0).bReleased)
		{
			// Toggle Obstacle if mouse left clicked
			bObstacleMap[p(nSelectedCellX, nSelectedCellY)] =
				!bObstacleMap[p(nSelectedCellX, nSelectedCellY)];
		} 

		if (GetMouse(1).bReleased)
		{
			nStartX = nSelectedCellX;
			nStartY = nSelectedCellY;
		}

		if (GetKey(olc::Key::Q).bReleased)
		{
			nWave++;
		}

		if (GetKey(olc::Key::A).bReleased)
		{
			nWave--;
			if (nWave == 0)
				nWave = 1;
		}



		// 1) Prepare flow field, add a boundary, and add obstacles
		//    by setting the flow Field Height (Z) to -1
		for (int x = 0; x < nMapWidth; x++)
		{
			for (int y = 0; y < nMapHeight; y++)
			{
				// Set border or obstacles
				if (x == 0 || y == 0 || x == (nMapWidth - 1) || y == (nMapHeight - 1) || bObstacleMap[p(x, y)])
				{
					nFlowFieldZ[p(x, y)] = -1;
				}
				else
				{
					nFlowFieldZ[p(x, y)] = 0;
				}
			}
		}

		// 2) Propagate a wave (i.e. flood-fill) from target location. Here I use
		//    a tuple, of {x, y, distance} - though you could use a struct or 
		//    similar.
		std::list<std::tuple<int, int, int>> nodes;

		// Add the first discovered node - the target location, with a distance of 1
		nodes.push_back({ nEndX, nEndY, 1 });

		while (!nodes.empty())
		{
			// Each iteration through the discovered nodes may create newly discovered
			// nodes, so I maintain a second list. It's important not to contaminate
			// the list being iterated through.
			std::list<std::tuple<int, int, int>> new_nodes;

			// Now iterate through each discovered node. If it has neighbouring nodes
			// that are empty space and undiscovered, add those locations to the
			// new nodes list
			for (auto &n : nodes)
			{
				int x = std::get<0>(n); // Map X-Coordinate
				int y = std::get<1>(n); // Map Y-Coordinate
				int d = std::get<2>(n); // Distance From Target Location

				// Set distance count for this node. NOte that when we add nodes we add 1
				// to this distance. This emulates propagating a wave across the map, where
				// the front of that wave increments each iteration. In this way, we can 
				// propagate distance information 'away from target location'
				nFlowFieldZ[p(x, y)] = d;

				// Add neigbour nodes if unmarked, i.e their "height" is 0. Any discovered
				// node or obstacle will be non-zero

				// Check East
				if ((x + 1) < nMapWidth && nFlowFieldZ[p(x + 1, y)] == 0)
					new_nodes.push_back({ x + 1, y, d + 1 });

				// Check West
				if ((x - 1) >= 0 && nFlowFieldZ[p(x - 1, y)] == 0)
					new_nodes.push_back({ x - 1, y, d + 1 });

				// Check South
				if ((y + 1) < nMapHeight && nFlowFieldZ[p(x, y + 1)] == 0)
					new_nodes.push_back({ x, y + 1, d + 1 });

				// Check North
				if ((y - 1) >= 0 && nFlowFieldZ[p(x, y - 1)] == 0)
					new_nodes.push_back({ x, y - 1, d + 1 });
			}

			// We will now have potentially multiple nodes for a single location. This means our
			// algorithm will never complete! So we must remove duplicates form our new node list.
			// Im doing this with some clever code - but it is not performant(!) - it is merely
			// convenient. I'd suggest doing away with overhead structures like linked lists and sorts
			// if you are aiming for fastest path finding.

			// Sort the nodes - This will stack up nodes that are similar: A, B, B, B, B, C, D, D, E, F, F
			new_nodes.sort([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2)
			{
				// In this instance I dont care how the values are sorted, so long as nodes that
				// represent the same location are adjacent in the list. I can use the p() lambda
				// to generate a unique 1D value for a 2D coordinate, so I'll sort by that.
				return p(std::get<0>(n1), std::get<1>(n1)) < p(std::get<0>(n2), std::get<1>(n2));
			});

			// Use "unique" function to remove adjacent duplicates       : A, B, -, -, -, C, D, -, E, F -
			// and also erase them                                       : A, B, C, D, E, F
			new_nodes.unique([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2)
			{
				return  p(std::get<0>(n1), std::get<1>(n1)) == p(std::get<0>(n2), std::get<1>(n2));
			});

			// We've now processed all the discoverd nodes, so clear the list, and add the newly
			// discovered nodes for processing on the next iteration
			nodes.clear();
			nodes.insert(nodes.begin(), new_nodes.begin(), new_nodes.end());

			// When there are no more newly discovered nodes, we have "flood filled" the entire
			// map. The propagation phase of the algorithm is complete
		}


		// 3) Create Path. Starting a start location, create a path of nodes until you reach target
		//    location. At each node find the neighbour with the lowest "distance" score.
		std::list<std::pair<int, int>> path;
		path.push_back({ nStartX, nStartY });
		int nLocX = nStartX;
		int nLocY = nStartY;
		bool bNoPath = false;

		while (!(nLocX == nEndX && nLocY == nEndY) && !bNoPath)
		{
			std::list<std::tuple<int, int, int>> listNeighbours;

			// 4-Way Connectivity
			if ((nLocY - 1) >= 0 && nFlowFieldZ[p(nLocX, nLocY - 1)] > 0)
				listNeighbours.push_back({ nLocX, nLocY - 1, nFlowFieldZ[p(nLocX, nLocY - 1)] });

			if ((nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY)] > 0)
				listNeighbours.push_back({ nLocX + 1, nLocY, nFlowFieldZ[p(nLocX + 1, nLocY)] });

			if ((nLocY + 1) < nMapHeight && nFlowFieldZ[p(nLocX, nLocY + 1)] > 0)
				listNeighbours.push_back({ nLocX, nLocY + 1, nFlowFieldZ[p(nLocX, nLocY + 1)] });

			if ((nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY)] > 0)
				listNeighbours.push_back({ nLocX - 1, nLocY, nFlowFieldZ[p(nLocX - 1, nLocY)] });

			// 8-Way Connectivity
			if ((nLocY - 1) >= 0 && (nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY - 1)] > 0)
				listNeighbours.push_back({ nLocX - 1, nLocY - 1, nFlowFieldZ[p(nLocX - 1, nLocY - 1)] });

			if ((nLocY - 1) >= 0 && (nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY - 1)] > 0)
				listNeighbours.push_back({ nLocX + 1, nLocY - 1, nFlowFieldZ[p(nLocX + 1, nLocY - 1)] });

			if ((nLocY + 1) < nMapHeight && (nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY + 1)] > 0)
				listNeighbours.push_back({ nLocX - 1, nLocY + 1, nFlowFieldZ[p(nLocX - 1, nLocY + 1)] });

			if ((nLocY + 1) < nMapHeight && (nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY + 1)] > 0)
				listNeighbours.push_back({ nLocX + 1, nLocY + 1, nFlowFieldZ[p(nLocX + 1, nLocY + 1)] });

			// Sprt neigbours based on height, so lowest neighbour is at front
			// of list
			listNeighbours.sort([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2)
			{
				return std::get<2>(n1) < std::get<2>(n2); // Compare distances
			});

			if (listNeighbours.empty()) // Neighbour is invalid or no possible path
				bNoPath = true;
			else
			{
				nLocX = std::get<0>(listNeighbours.front());
				nLocY = std::get<1>(listNeighbours.front());
				path.push_back({ nLocX, nLocY });
			}
		}


		// 4) Create Flow "Field"
		for (int x = 1; x < nMapWidth - 1; x++)
		{
			for (int y = 1; y < nMapHeight - 1; y++)
			{
				float vx = 0.0f;
				float vy = 0.0f;

				vy -= (float)((nFlowFieldZ[p(x, y + 1)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x, y + 1)]) - nFlowFieldZ[p(x, y)]);
				vx -= (float)((nFlowFieldZ[p(x + 1, y)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x + 1, y)]) - nFlowFieldZ[p(x, y)]);
				vy += (float)((nFlowFieldZ[p(x, y - 1)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x, y - 1)]) - nFlowFieldZ[p(x, y)]);
				vx += (float)((nFlowFieldZ[p(x - 1, y)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x - 1, y)]) - nFlowFieldZ[p(x, y)]);

				float r = 1.0f / sqrtf(vx*vx + vy * vy);
				fFlowFieldX[p(x, y)] = vx * r;
				fFlowFieldY[p(x, y)] = vy * r;
			}
		}




		// Draw Map
		Clear(olc::BLACK);

		for (int x = 0; x < nMapWidth; x++)
		{
			for (int y = 0; y < nMapHeight; y++)
			{
				olc::Pixel colour = olc::BLUE;

				if (bObstacleMap[p(x, y)])
					colour = olc::GREY;

				if (nWave == nFlowFieldZ[p(x, y)])
					colour = olc::DARK_CYAN;

				if (x == nStartX && y == nStartY)
					colour = olc::GREEN;

				if (x == nEndX && y == nEndY)
					colour = olc::RED;

				// Draw Base
				FillRect(x * nCellSize, y * nCellSize, nCellSize - nBorderWidth, nCellSize - nBorderWidth, colour);

				// Draw "potential" or "distance" or "height" :D
				//DrawString(x * nCellSize, y * nCellSize, std::to_string(nFlowFieldZ[p(x, y)]), olc::WHITE);

				if (nFlowFieldZ[p(x, y)] > 0)
				{
					float ax[4], ay[4];
					float fAngle = atan2f(fFlowFieldY[p(x, y)], fFlowFieldX[p(x, y)]);
					float fRadius = (float)(nCellSize - nBorderWidth) / 2.0f;
					int fOffsetX = x * nCellSize + ((nCellSize - nBorderWidth) / 2);
					int fOffsetY = y * nCellSize + ((nCellSize - nBorderWidth) / 2);
					ax[0] = cosf(fAngle) * fRadius + fOffsetX;
					ay[0] = sinf(fAngle) * fRadius + fOffsetY;
					ax[1] = cosf(fAngle) * -fRadius + fOffsetX;
					ay[1] = sinf(fAngle) * -fRadius + fOffsetY;
					ax[2] = cosf(fAngle + 0.1f) * fRadius * 0.7f + fOffsetX;
					ay[2] = sinf(fAngle + 0.1f) * fRadius * 0.7f + fOffsetY;
					ax[3] = cosf(fAngle - 0.1f) * fRadius * 0.7f + fOffsetX;
					ay[3] = sinf(fAngle - 0.1f) * fRadius * 0.7f + fOffsetY;

					DrawLine(ax[0], ay[0], ax[1], ay[1], olc::CYAN);
					DrawLine(ax[0], ay[0], ax[2], ay[2], olc::CYAN);
					DrawLine(ax[0], ay[0], ax[3], ay[3], olc::CYAN);

				}
			}
		}


		bool bFirstPoint = true;
		int ox, oy;
		for (auto &a : path)
		{
			if (bFirstPoint)
			{
				ox = a.first;
				oy = a.second;
				bFirstPoint = false;
			}
			else
			{
				DrawLine(
					ox * nCellSize + ((nCellSize - nBorderWidth) / 2),
					oy  * nCellSize + ((nCellSize - nBorderWidth) / 2),
					a.first * nCellSize + ((nCellSize - nBorderWidth) / 2),
					a.second * nCellSize + ((nCellSize - nBorderWidth) / 2), olc::YELLOW);

				ox = a.first;
				oy = a.second;

				FillCircle(ox * nCellSize + ((nCellSize - nBorderWidth) / 2), oy * nCellSize + ((nCellSize - nBorderWidth) / 2), 10, olc::YELLOW);
			}
		}

		
		return true;
	}
};


int main()
{
	PathFinding_FlowFields demo;
	if (demo.Construct(512, 480, 2, 2))
		demo.Start();
	return 0;
}