//Navigation Mesh Pathfinding with A* in C++
//Project: Navmesh + Dx11 Graphics Techdemo
//http://robert-lindner.com/img/projects/navmesh/ai_paper.pdf
//http://robert-lindner.com/blog/navmesh/
//Author: Robert Lindenr

//This sample contains code for building the navmesh neightbor structure from a mesh, A* and finding the current element in the mesh using point in triangle tests


//********************
//A star algotithm for any time of navigation structure, be it grids or meshes
//********************
const vector<NavElement*>& PathFinder::AStar(NavElement* startElement, NavElement* endElement)
{
	//Clear containers
	ClearContainers();
	if (startElement == endElement)
	{
		m_Path.push_back(startElement);
		return m_Path;
	}
	//Temp containers
	deque<NavElement*> openList;
	deque<NavElement*> closedList;

	//Add StartElement to OpenList
	NavElement* pCurentElement = nullptr;
	openList.push_back(startElement);

	//While our open list is not empty
	while (openList.size() != 0)
	{
		//1. Get the node with the lowest F from the openList
		float lowestFScore = std::numeric_limits<float>::max();
		for (auto el : openList)
		{
			if (el->m_FScore < lowestFScore)
			{
				pCurentElement = el;
				lowestFScore = el->m_FScore;
			}
		}
		//2. Pop current off the open list and push it to the closed
		openList.erase(std::remove_if(openList.begin(), openList.end(), 
			[pCurentElement](const NavElement* e) {return e == pCurentElement; }));
		closedList.push_back(pCurentElement);

		//3. Retrieve the chosen elements' adjacent elements
		auto vAdj = pCurentElement->GetWalkableNeighbors();

		//If any of the successors is the goal, call it a day (set it's parent, break the loop by clearing open and breaking
		if (std::find(vAdj.begin(), vAdj.end(), endElement) != vAdj.end())
		{
			endElement->m_pParent = pCurentElement;
			openList.clear();
			break;
		}

		//Else go over all the elements
		for (auto pAdj : vAdj)
		{
			//If node is in closed, ignore it
			if (std::find(closedList.begin(), closedList.end(), pAdj) != closedList.end())
			{
			}
			else
			{
				//if node not in open list, compute score and add it
				if (std::find(openList.begin(), openList.end(), pAdj) == openList.end())
				{
					//3.1 Set the parent to the current
					pAdj->m_pParent = pCurentElement;
					//Calculate the g, h and f
					//g = current.g + cost(dist current and this) ---> OR ues GetTileCost() if using hard value (needs extra support for diagonal movement)
					XMFLOAT2 tempV(abs(pCurentElement->GetCenter().x - pAdj->GetCenter().x),
						abs(pCurentElement->GetCenter().y - pAdj->GetCenter().y));
					float cost = Heuristic(tempV.x, tempV.y);
					pAdj->m_GScore = cost + pCurentElement->m_GScore;
					//h = dist this and goal
					tempV = XMFLOAT2(abs(pAdj->GetCenter().x - endElement->GetCenter().x),
						abs(pAdj->GetCenter().y - endElement->GetCenter().y));
					pAdj->m_HScore = Heuristic(tempV.x, tempV.y);
					//f = g + h
					pAdj->m_FScore = pAdj->m_GScore + pAdj->m_HScore;
					//Add it
					openList.push_back(pAdj);
					//Store for showcasing nodes
					m_pCheckedElements.push_back(pAdj);
				}
			}
		}
	}

	//Reconstruct path
	m_Path.push_back(endElement);
	m_Path.push_back(endElement->m_pParent);
	NavElement* pNextElement = pCurentElement->m_pParent;
	while (pNextElement != nullptr)
	{
		if (std::find(m_Path.begin(), m_Path.end(), pNextElement) == m_Path.end())
		{
			m_Path.push_back(pNextElement);
			pNextElement = pNextElement->m_pParent;
		}
		else break;
	}

	return m_Path;
}

void NavMesh::Initialize(const GameContext& gameContext)
{
	//[...]
	auto mesh = ContentManager::Load<MeshFilter>(m_AssetFile);
	for (size_t i = 0; i < mesh->m_Indices.size(); i += 3)
	{
		DWORD indices[3] = {
			mesh->m_Indices[i + 0],
			mesh->m_Indices[i + 1],
			mesh->m_Indices[i + 2] };
		XMFLOAT3 positions[3] = {
			mesh->m_Positions[indices[0]],
			mesh->m_Positions[indices[1]],
			mesh->m_Positions[indices[2]] };
		auto e = new NavMeshElement(positions, indices, m_MatID, m_pMat);
		AddChild(e);
		m_Elements.push_back(e);
	}
	//Build neightbor structure
	for (auto el : m_Elements)
	{
		for (auto n : m_Elements)if (!(el == n))el->PotentialNeighbor(n);
	}
	Logger::LogInfo(L"fknbreak");
}
vector<pair<DWORD, DWORD> > NavMeshElement::GetEdges()
{
	vector<pair<DWORD, DWORD> > ret;
	ret.push_back(pair<DWORD, DWORD>(m_Indices[0], m_Indices[1]));
	ret.push_back(pair<DWORD, DWORD>(m_Indices[1], m_Indices[2]));
	ret.push_back(pair<DWORD, DWORD>(m_Indices[0], m_Indices[2]));
	return ret;
}
vector<pair<XMFLOAT3, XMFLOAT3> > NavMeshElement::GetEdgePositions()
{
	vector<pair<XMFLOAT3, XMFLOAT3> > ret;
	ret.push_back(pair<XMFLOAT3, XMFLOAT3>(m_Vertices[0], m_Vertices[1]));
	ret.push_back(pair<XMFLOAT3, XMFLOAT3>(m_Vertices[1], m_Vertices[2]));
	ret.push_back(pair<XMFLOAT3, XMFLOAT3>(m_Vertices[0], m_Vertices[2]));
	return ret;
}
void NavMeshElement::PotentialNeighbor(NavMeshElement* el)
{
	{
		auto edges = GetEdges();
		auto nEdges = el->GetEdges();
		for (auto a : edges)
		{
			for (auto b : nEdges)
			{
				if ((a.first == b.first && a.second == b.second)
					|| (a.first == b.second && a.second == b.first))
				{
					m_Neighbors.push_back(el);
					return;
				}
			}
		}
	}
	//Also check with positions in case of split edges, 
	//might fail though due to floating point accuracy
	{
		auto edges = GetEdgePositions();
		auto nEdges = el->GetEdgePositions();
		for (auto a : edges)
		{
			for (auto b : nEdges)
			{
				if ((XMFloat3Equals(a.first, b.first) && XMFloat3Equals(a.second, b.second))
					|| (XMFloat3Equals(a.first, b.second) && XMFloat3Equals(a.second, b.first)))
				{
					m_Neighbors.push_back(el);
					return;
				}
			}
		}
	}
}

//*************************
//Determines the element an object is likeley to be standing on to calculate the path from
//*************************
NavElement* NavMesh::GetElement(XMFLOAT3 pos)
{
	//sort elements by proximity first
	//Should be more efficient if the search position is inside a triangle because the 
	//		check would only be performed a few times, possibly cutting time in half
	//if search position has no matching triangle its worse because it was uselessly sorted
	//		, but it helps returning the closest triangle in that case
	for (auto e : m_Elements) 
	{
		e->StoreProximity(pos);
	}
	sort(m_Elements.begin(), m_Elements.end(),
		[](const NavMeshElement* a, const NavMeshElement* b) -> bool
	{
		return a->m_Proximity < b->m_Proximity;
	});

	//Using barycentric coordinate test 
	//Its the same method used for ray triangle intersection tests in my pathtracer lightwhat
	XMVECTOR P = XMLoadFloat3(&pos);
	for (auto e : m_Elements)
	{
		//Get triangle positions
		XMVECTOR A = XMLoadFloat3(&e->m_Vertices[0]);
		XMVECTOR B = XMLoadFloat3(&e->m_Vertices[1]);
		XMVECTOR C = XMLoadFloat3(&e->m_Vertices[2]);
		//Some data could be precalculated at the cost of memory bloating (marked with //p)
		//thats a total of 13 floats
		//Calculate triangle vectors
		XMVECTOR v0 = C - A;//p
		XMVECTOR v1 = B - A;//p
		XMVECTOR v2 = P - A;
		//Calculate dot products
		XMVECTOR dotVec;
		float dot00, dot01, dot02, dot11, dot12;
		dotVec = XMVector3Dot(v0, v0); XMStoreFloat(&dot00, dotVec);//p
		dotVec = XMVector3Dot(v0, v1); XMStoreFloat(&dot01, dotVec);//p
		dotVec = XMVector3Dot(v0, v2); XMStoreFloat(&dot02, dotVec);
		dotVec = XMVector3Dot(v1, v1); XMStoreFloat(&dot11, dotVec);//p
		dotVec = XMVector3Dot(v1, v2); XMStoreFloat(&dot12, dotVec);
		//Calculate barycentric coordinates
		float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);//p
		float u = (dot11*dot02 - dot01*dot12)*invDenom;
		if (u >= 0)
		{
			float v = (dot00*dot12 - dot01*dot02)*invDenom;
			if ((v >= 0) && (u + v < 1))
			{
				//Point is in triangle
				return e;
			}
		}
	}

	//If there is no triangle intersection (efficiency worstcase)
	//return closest triangle
#if defined(DEBUG) | defined(_DEBUG)
	Logger::LogInfo(L"NavMesh>GetElement no matching triangle found, returning closest");
#endif
	return m_Elements[0];
}
void NavMeshElement::StoreProximity(XMFLOAT3 pos)
{
	auto p = XMLoadFloat3(&pos);
	auto c = XMLoadFloat3(&GetCenter3());
	auto lengthV = XMVector3Length(p - c);
	XMStoreFloat(&m_Proximity, lengthV);
}