#include-once

; #INDEX# =======================================================================================================================
; Title .........: NavMesh Navigation UDF
; AutoIt Version : 3.3.14.2
; Date ..........: 17.05.2017
; Description ...: Provides NavMesh functions to calculate routes between waypoints using the A*-Algorithm.
;                  This UDF works with directed graphs so missing nodes in the mesh file will have critical impact
;                  on the result and might even let the calculation fail.
; Author(s) .....: alpines
; ===============================================================================================================================

; #CURRENT# =====================================================================================================================
; _NavMesh_GetPathDistance
; _NavMesh_GetPath
; _NavMesh_ListWaypointsByWaypoint
; _NavMesh_GetCoordinatesByWaypoint
; _NavMesh_GetWaypointByCoordinates
; _NavMesh_GetLowestInList
; _NavMesh_GetNearestWaypoint
; _NavMesh_GetDistance
; _NavMesh_LoadMeshFromFile
; ===============================================================================================================================

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetPathDistance
; Description ...: Calculates the total travel distance on a given path
; Syntax.........: _NavMesh_GetPathDistance($aPath, $aMesh)
; Parameters ....: $aPath		- The path of whose distance should be calculated
;                  $aMesh		- The mesh array containing the waypoints of the path
; Return values .: Success		- Total distance on the path
;                  Failure		- -1 if invalid path array was passed
; Author ........: alpines
; Remarks .......: Return value 0 is not indicating an error but -1 is
; ===============================================================================================================================
Func _NavMesh_GetPathDistance($aPath, $aMesh)
	If Not UBound($aPath) Then Return -1

	Local $aPos, $aPos_, $iTravelDistance = 0

	For $i = 0 To UBound($aPath) - 2
		$aPos  = _NavMesh_GetCoordinatesByWaypoint($aPath[$i]    , $aMesh)
		$aPos_ = _NavMesh_GetCoordinatesByWaypoint($aPath[$i + 1], $aMesh)

		$iTravelDistance += _NavMesh_GetDistance($aPos[0], $aPos[1], $aPos_[0], $aPos_[1])
	Next

	Return $iTravelDistance
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetPath
; Description ...: Calculates the shortest path between two waypoints
; Syntax.........: _NavMesh_GetPath($iStartWaypoint, $iEndWaypoint, $aMesh[, $bReturnWaypoints = True])
; Parameters ....: $iStartWaypoint		- The waypoint where the path starts
;                  $iEndWaypoint		- The waypoint where the path ends
;                  $aMesh				- The mesh array containing the waypoints
;                  $bReturnWaypoints	- If true then the waypoint indices are returned, coordinates if false
; Return values .: Success		- Array containing waypoints or coordinates (depending on the param $bReturnWaypoints)
;                  Failure		- Zero, non-array
; Author ........: alpines
; Remarks .......: The start waypoint and the end waypoint have to be on the mesh, otherwise this function will fail.
; ===============================================================================================================================
Func _NavMesh_GetPath($iStartWaypoint, $iEndWaypoint, $aMesh, $bReturnWaypoints = True)
	If $iStartWaypoint < 0 or $iStartWaypoint > UBound($aMesh) - 1 or $iEndWaypoint < 0 or $iEndWaypoint > UBound($aMesh) - 1 Then Return 0

	If $iStartWaypoint = $iEndWaypoint Then
		If $bReturnWaypoints Then
			Local $aReturn[1] = [$iStartWaypoint]
			Return $aReturn
		Else
			Local $aReturn = _NavMesh_GetCoordinatesByWaypoint($iStartWaypoint, $aMesh)
			Return $aReturn
		EndIf
	EndIf

	Local $aStart       = _NavMesh_GetCoordinatesByWaypoint($iStartWaypoint, $aMesh)
	Local $aDestination = _NavMesh_GetCoordinatesByWaypoint($iEndWaypoint, $aMesh)

	Local $aReturn[0][2], $aClosedList[0], _
		  $aOpenList[1][4] = [[0, _NavMesh_GetDistance($aStart[0], $aStart[1], $aDestination[0], $aDestination[1]), _
							   $iStartWaypoint, $iStartWaypoint]]

	Do
		Local $iIndex = _NavMesh_GetLowestInList($aOpenList)
		Local $aWaypoints = _NavMesh_ListWaypointsByWaypoint($aOpenList[$iIndex][3], $aMesh)

		If UBound($aWaypoints) Then
			Local $iWaypoint = 0

			Do
				For $i = 0 To UBound($aClosedList) - 1
					If $aWaypoints[$iWaypoint] = $aClosedList[$i] Then
						__AD($aWaypoints, $iWaypoint)
						$iWaypoint -= 1
						ExitLoop
					EndIf
				Next

				$iWaypoint += 1
			Until $iWaypoint >= UBound($aWaypoints)
		EndIf

		ReDim $aOpenList[UBound($aOpenList) + UBound($aWaypoints)][4]

		For $i = 0 To UBound($aWaypoints) - 1
			Local $iOpenListSize = UBound($aOpenList) - UBound($aWaypoints)

			$aTmp = _NavMesh_GetCoordinatesByWaypoint($aOpenList[$iIndex][3], $aMesh)
			$aTmp2 = _NavMesh_GetCoordinatesByWaypoint($aWaypoints[$i], $aMesh)
			$aOpenList[$iOpenListSize + $i][0] = $aOpenList[$iIndex][0] + _NavMesh_GetDistance($aTmp[0], $aTmp[1], $aTmp2[0], $aTmp2[1])
			$aOpenList[$iOpenListSize + $i][1] = $aOpenList[$iOpenListSize + $i][0] + _NavMesh_GetDistance($aTmp2[0], $aTmp2[1], $aDestination[0], $aDestination[1])
			$aOpenList[$iOpenListSize + $i][2] = $aOpenList[$iIndex][2] & ";" & $aWaypoints[$i]
			$aOpenList[$iOpenListSize + $i][3] = $aWaypoints[$i]
		Next

		ReDim $aClosedList[UBound($aClosedList) + 1]
		$aClosedList[UBound($aClosedList) - 1] = $aOpenList[$iIndex][3]
		__A2DD($aOpenList, $iIndex)

		If Not UBound($aOpenList) Then Return 0
	Until $aOpenList[_NavMesh_GetLowestInList($aOpenList)][3] = $iEndWaypoint

	Local $aSplit = StringSplit($aOpenList[_NavMesh_GetLowestInList($aOpenList)][2], ";", 3)

	If $bReturnWaypoints Then
		Return $aSplit
	Else
		ReDim $aReturn[UBound(StringSplit($aOpenList[_NavMesh_GetLowestInList($aOpenList)][2], ";", 3))][2]

		For $i = 0 To UBound($aReturn) - 1
			$aTmp = _NavMesh_GetCoordinatesByWaypoint($aSplit[$i], $aMesh)
			$aReturn[$i][0] = $aTmp[0]
			$aReturn[$i][1] = $aTmp[1]
		Next

		Return $aReturn
	EndIf
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_ListWaypointsByWaypoint
; Description ...: List all connected waypoints for a specific waypoint
; Syntax.........: _NavMesh_ListWaypointsByWaypoint($iWaypoint, $aMesh)
; Parameters ....: $iWaypoint	- The waypoint whose neighbors should be listed
;                  $aMesh		- The mesh array containing the waypoints
; Return values .: Success		- Array containing all neighbor waypoints of a specific waypoint
; Author ........: alpines
; Remarks .......: This method can not fail.
; ===============================================================================================================================
Func _NavMesh_ListWaypointsByWaypoint($iWaypoint, $aMesh)
	Local $aTmp[0]

	If UBound($aMesh, 2) < 2 Then Return $aTmp

	For $i = 2 To UBound($aMesh, 2) - 1
		If $aMesh[$iWaypoint][$i] <> -1 Then
			ReDim $aTmp[UBound($aTmp) + 1]
			$aTmp[UBound($aTmp) - 1] = $aMesh[$iWaypoint][$i]
		EndIf
	Next

	Return $aTmp
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetCoordinatesByWaypoint
; Description ...: Gets the coordinates of a waypoint
; Syntax.........: _NavMesh_GetCoordinatesByWaypoint($iWaypoint, $aMesh)
; Parameters ....: $iWaypoint	- The waypoint whose coordinates should be returned
;                  $aMesh		- The mesh array containing the waypoints
; Return values .: Success		- Array containing the coordinates of the waypoint on the mesh
; Author ........: alpines
; Remarks .......: This method can not fail.
; ===============================================================================================================================
Func _NavMesh_GetCoordinatesByWaypoint($iWaypoint, $aMesh)
	Local $aReturn[2] = [$aMesh[$iWaypoint][0], $aMesh[$iWaypoint][1]]
	Return $aReturn
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetWaypointByCoordinates
; Description ...: Gets the waypoint index of the coordinates
; Syntax.........: _NavMesh_GetWaypointByCoordinates($iPositinoX, $iPositionY, $aMesh)
; Parameters ....: $iPositionX	- The x-position of the waypoint
;                  $iPositionY	- The y-position of the waypoint
;                  $aMesh		- The mesh array containing the waypoints
; Return values .: Success		- The waypoint index corresponding to the coordinates
;                  Failure		- Returns -1 if no waypoint was found
; Author ........: alpines
; Remarks .......: The coordinates have to match the ones on the mesh.
;                  You can use _NavMesh_GetNearestWaypoint to get closest waypoint to your coordinates.
; ===============================================================================================================================
Func _NavMesh_GetWaypointByCoordinates($iPositionX, $iPositionY, $aMesh)
	For $i = 0 To UBound($aMesh) - 1
		If $iPositionX = $aMesh[$i][0] and $iPositionY = $aMesh[$i][1] Then Return $i
	Next

	Return -1
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetLowestInList
; Description ...: Returns the waypoint with the highest priority (shortest estimated distance) to the end waypoint
; Syntax.........: _NavMesh_GetLowestInList($aList)
; Parameters ....: $aList		- OpenList Array [traveled so far, estimated distance, way so far, current waypoint]
; Return values .: Success		- The index of the waypoint in the array with the highest priority
;                  Failure		- Returns -1 if list is empty
; Author ........: alpines
; Remarks .......: This function is designed to be used internally but can be used publicly aswell.
; ===============================================================================================================================
Func _NavMesh_GetLowestInList($aList)
	Local $iLowest, $iIndex = -1, $bFirst = True

	For $i = 0 To UBound($aList) - 1
		If $aList[$i][1] < $iLowest or $bFirst Then
			$iLowest = $aList[$i][1]
			$iIndex = $i
			$bFirst = False
		EndIf
	Next

	Return $iIndex
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetNearestWaypoint
; Description ...: Returns the waypoint closest to given coordinates
; Syntax.........: _NavMesh_GetNearestWaypoint($iX1, $iY1, $aMesh)
; Parameters ....: $iX1		- x-position of the waypoint
;                  $iY1		- y-position of the waypoint
;                  $aMesh	- The mesh array containing the waypoints
; Return values .: Success	- The index of the waypoint in the array with the highest priority
;                  Failure	- Returns -1 if no waypoint was found
; Author ........: alpines
; ===============================================================================================================================
Func _NavMesh_GetNearestWaypoint($iX1, $iY1, $aMesh)
	If Not UBound($aMesh) Then Return -1

	Local $iLowest = _NavMesh_GetDistance($iX1, $iY1, $aMesh[0][0], $aMesh[0][1]), _
		  $aReturn[2] = [$aMesh[0][0], $aMesh[0][1]]

	If Not IsArray($aMesh) or Not UBound($aMesh, 2) Then SetError(1, 0, -1)

	For $i = 0 To UBound($aMesh) - 1
		If _NavMesh_GetDistance($iX1, $iY1, $aMesh[$i][0], $aMesh[$i][1]) < $iLowest Then
			$iLowest = _NavMesh_GetDistance($iX1, $iY1, $aMesh[$i][0], $aMesh[$i][1])
			$aReturn[0] = $aMesh[$i][0]
			$aReturn[1] = $aMesh[$i][1]
		EndIf
	Next

	Return $aReturn
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_GetDistance
; Description ...: Returns the distance between two given coordinates
; Syntax.........: _NavMesh_GetDistance($iX1, $iY1, $iX2, $iY2)
; Parameters ....: $iX1		- x-position of the first coordinate
;                  $iY1		- y-position of the first coordinate
;                  $iX2		- x-position of the second coordinate
;                  $iY2		- y-position of the second coordinate
; Return values .: Success	- The distance between two coordinates using pythagoras
; Author ........: alpines
; Remarks .......: This method can not fail.
; ===============================================================================================================================
Func _NavMesh_GetDistance($iX1, $iY1, $iX2, $iY2)
	Return Sqrt(Abs($iX1 - $iX2)^2 + Abs($iY1 - $iY2)^2)
EndFunc

; #FUNCTION# ====================================================================================================================
; Name...........: _NavMesh_LoadMeshFromFile
; Description ...: Loads the mesh file into a mesh array
; Syntax.........: _NavMesh_LoadMeshFromFile($sFilePath)
; Parameters ....: $sFilePath	- The path to a mesh-file
; Return values .: Success	- The array containing the mesh file
;                  Failure	- Zero, non-array
; Author ........: alpines
; Remarks .......: The mesh file has to follow this structure:
;                  x;y;wp1;wp2;...
;                  x2;y2;wp2;wp4;...
; ===============================================================================================================================
Func _NavMesh_LoadMeshFromFile($sFilePath)
	Local $aMesh[0][0], $sFile, $aLines, $aTmp, $i, $i_, $iOffset

	If Not FileExists($sFilePath) Then Return 0
	$sFile = FileRead($sFilePath)
	$aLines = StringSplit($sFile, @CRLF, 3)

	For $i = 0 To UBound($aLines) - 1
		If Not StringLen($aLines[$i]) Then
			$iOffset += 1
			ContinueLoop
		EndIf

		$aTmp = StringSplit($aLines[$i], ";", 2)
		ReDim $aMesh[UBound($aMesh) + 1][((UBound($aTmp) > UBound($aMesh, 2)) ? UBound($aTmp) : UBound($aMesh, 2))]
		For $i_ = 0 To UBound($aTmp) - 1
			$aMesh[$i - $iOffset][$i_] = $i_ < 2 ? Number($aTmp[$i_]) : Number($aTmp[$i_] - 1)
		Next
	Next

	For $i = 0 To UBound($aMesh) - 1
		If UBound($aMesh, 2) <= 2 Then ExitLoop

		For $j = 2 To UBound($aMesh, 2) - 1
			If StringLen($aMesh[$i][$j]) < 1 Then $aMesh[$i][$j] = "-1"
		Next
	Next

	If Not UBound($aMesh) Then Return 0
	Return $aMesh
EndFunc

Func __AD(ByRef $avArray, $iElement)
	Local $iUBound = UBound($avArray, 1) - 1

	If $iElement < 0 Then $iElement = 0
	If $iElement > $iUBound Then $iElement = $iUBound

	Switch UBound($avArray, 0)
		Case 1
			For $i = $iElement To $iUBound - 1
				$avArray[$i] = $avArray[$i + 1]
			Next

			ReDim $avArray[$iUBound]

		Case 2
			Local $iSubMax = UBound($avArray, 2) - 1
			For $i = $iElement To $iUBound - 1
				For $j = 0 To $iSubMax
					$avArray[$i][$j] = $avArray[$i + 1][$j]
				Next
			Next

			ReDim $avArray[$iUBound][$iSubMax + 1]
	EndSwitch

	Return $iUBound
EndFunc

Func __A2DD(ByRef $aArray, $iDEL)
    Local $UBound2nd = UBound($aArray, 2)

    If @error = 2 Then
        Local $arTmp[UBound($aArray) - 1]
        $k = 0
        For $i = 0 To UBound($aArray) - 1
            If $i <> $iDEL Then
                $arTmp[$k] = $aArray[$i]
                $k += 1
            EndIf
        Next
    Else
        Local $arTmp[UBound($aArray) - 1][$UBound2nd]
        $k = 0
        For $i = 0 To UBound($aArray) - 1
            If $i <> $iDEL Then
                For $l = 0 To $UBound2nd-1
                    $arTmp[$k][$l] = $aArray[$i][$l]
                Next
                $k += 1
            EndIf
        Next
    EndIf

    $aArray = $arTmp
EndFunc