PathFinder

//PathFinder by Fleynaro for Industrial RPG

#if defined _PathFinder_included
	#endinput
#endif
#define _PathFinder_included

#define PF:: PF_

//Settings
#define PF_PATH_SIZE 512
#define PF_SIDE_SIZE_X 128
#define PF_SIDE_SIZE_Y 128
#define PF_CELL_SIZE_DEFAULT 1.5
#define PF_WALL_UP_MAX 1.0
#define PF_WALL_DOWN_MAX 7.0

//System
#define PF_NULL 0
#define PF_SetParent(%0,%1,%2,%3) PF_CoordPlane[%0][%1] = PF_CoordPlane[%0][%1] & ~0xFFFF | (%2 | (%3 << 8))
#define PF_GetParentX(%0,%1) (PF_CoordPlane[%0][%1] & 0xFF)
#define PF_GetParentY(%0,%1) (PF_CoordPlane[%0][%1] >> 8 & 0xFF)
#define PF_GetSavedZ(%0,%1) (float(PF_CoordPlaneZ[%0][%1]) / 1000.0)
#define PF_SaveZ(%0,%1,%2) (PF_CoordPlaneZ[%0][%1] = floatround(%2 * 1000))
#define PF_SetG(%0,%1,%2) PF_CoordPlane[%0][%1] = PF_CoordPlane[%0][%1] & ~(0b11111111111 << 16) | (%2 << 16)
#define PF_GetG(%0,%1) (PF_CoordPlane[%0][%1] >> 16 & 0b11111111111)
#define PF_SetRecheck(%0,%1,%2) PF_CoordPlane[%0][%1] = PF_CoordPlane[%0][%1] & ~(0b1 << 30) | (%2 << 30)
#define PF_IsRecheck(%0,%1) (PF_CoordPlane[%0][%1] >> 30 & 0b1)
#define PF_SetClosed(%0,%1) PF_CoordPlane[%0][%1] = PF_CoordPlane[%0][%1] & ~(0b1 << 31) | (1 << 31)
#define PF_IsClosed(%0,%1) (PF_CoordPlane[%0][%1] >> 31 & 0b1)
#define PF_IsOpen(%0,%1) (PF_CoordPlane[%0][%1] != PF_NULL)
#define PF_IsValid(%0,%1) (0 <= %0 < PF_SIDE_SIZE_X && 0 <= %1 < PF_SIDE_SIZE_Y)

#define PF_ABS(%0) ((%0 < 0) ? -(%0) : (%0))

#define PF_NOT_COORD 0xFFFFFFFF
#define PF_SetCoord(%0,%1,%2) %0 = (%1 + 1) | ((%2 + 1) << 8)
#define PF_GetCoordX(%0) ((%0 & 0xFF) - 1)
#define PF_GetCoordY(%0) ((%0 >> 8 & 0xFF) - 1)
#define PF_IsCoord(%0) (%0 != 0)

//Arrays
new PF_CoordPlane[PF_SIDE_SIZE_X][PF_SIDE_SIZE_Y],
	PF_CoordPlaneZ[PF_SIDE_SIZE_X][PF_SIDE_SIZE_Y],
	Float: PF_Path[PF_PATH_SIZE][3],
	PF_PathSize,
	PF_BeginX, PF_BeginY,
	PF_FinalX, PF_FinalY;

new Float: PF_CellSize,
	PF_EndType;
#define PF_END_TYPE_POINT 1
#define PF_END_TYPE_LINE 2
#define PF_END_TYPE_CIRCLE 3
#define PF_END_TYPE_SQUARE 4

new Float: PF_StartPos[3],
	Float: PF_EndPos[4];
#define PF_SetStart(%0,%1,%2) (PF_StartPos[0] = %0, PF_StartPos[1] = %1, PF_StartPos[2] = %2)
#define PF_SetEndAsPoint(%0,%1,%2) (PF_EndType = PF_END_TYPE_POINT, PF_EndPos[0] = %0, PF_EndPos[1] = %1, PF_EndPos[2] = %2)
#define PF_SetEndAsLine(%0,%1,%2,%3) (PF_EndType = PF_END_TYPE_LINE, PF_EndPos[0] = %0, PF_EndPos[1] = %1, PF_EndPos[2] = %2, PF_EndPos[3] = %3)
#define PF_SetEndAsCircle(%0,%1,%2) (PF_EndType = PF_END_TYPE_CIRCLE, PF_EndPos[0] = %0, PF_EndPos[1] = %1, PF_EndPos[2] = %2)
#define PF_SetEndAsSquare(%0,%1,%2,%3) (PF_EndType = PF_END_TYPE_SQUARE, PF_EndPos[0] = %0, PF_EndPos[1] = %1, PF_EndPos[2] = %2, PF_EndPos[3] = %3)
#define PF_foreach() for ( new pointI = PF_PathSize - 1; pointI != -1; pointI -- )

#define PF_RESULT_SUCCESS 1
#define PF_RESULT_SUCCESS_NOT_FULL 2
#define PF_RESULT_FAIL 3
new PF_RESULT;

//Stocks
stock PF::CoordPlaneInit()
{
	for ( new x; x < PF::SIDE_SIZE_X; x ++ ) {
		for ( new y; y < PF::SIDE_SIZE_Y; y ++ ) {
			PF::CoordPlane[x][y] = PF::NULL;
			PF::CoordPlaneZ[x][y] = 0;
		}
	}
	PF::PathSize = 0;
	//PF::EndType = PF::END_TYPE_POINT;
	//PF::CellSize = PF::CELL_SIZE_DEFAULT;
	PF::RESULT = 0;
	return 1;
}

//Stocks
stock PF::SetBeginPointDefault()
{
	PF::BeginX = PF::SIDE_SIZE_X / 2,
	PF::BeginY = PF::SIDE_SIZE_Y / 2;
	return 1;
}

stock PF::SetBeginPointRelOf(Float: x, Float: y, width = 10, height = 10)
{
	PF::SetBeginPointDefault();
	if ( x - PF::StartPos[0] > PF::SIDE_SIZE_X * PF::CellSize ) {
		PF::BeginX = width;
	} else if ( PF::StartPos[0] - x > PF::SIDE_SIZE_X * PF::CellSize ) {
		PF::BeginX = PF::SIDE_SIZE_X - width;
	}
	if ( y - PF::StartPos[1] > PF::SIDE_SIZE_Y * PF::CellSize ) {
		PF::BeginY = height;
	} else if ( PF::StartPos[1] - y > PF::SIDE_SIZE_Y * PF::CellSize ) {
		PF::BeginY = PF::SIDE_SIZE_Y - height;
	}
	return 1;
}


//http://qiao.github.io/PathFinding.js/visual/
stock PF::FindPath()
{
	PF::CoordPlaneInit();
	new X = PF::BeginX,
		Y = PF::BeginY,
		Float: Z = PF::StartPos[2];

	/*new Float: x2, Float: y2;
	PF::GetGlobalCoord(0, 0, x2, y2);
	SetPlayerMapIcon(0, 0, x2, y2, 100.0, 0, 0xFF0000FF, MAPICON_GLOBAL);
	PF::GetGlobalCoord(0, 255, x2, y2);
	SetPlayerMapIcon(0, 1, x2, y2, 100.0, 0, 0xFF0000FF, MAPICON_GLOBAL);
	PF::GetGlobalCoord(255, 0, x2, y2);
	SetPlayerMapIcon(0, 2, x2, y2, 100.0, 0, 0xFF0000FF, MAPICON_GLOBAL);
	PF::GetGlobalCoord(255, 255, x2, y2);
	SetPlayerMapIcon(0, 3, x2, y2, 100.0, 0, 0xFF0000FF, MAPICON_GLOBAL);*/

	//printf("PF::StartPos[0] = %f, PF::StartPos[1] = %f", PF::StartPos[0], PF::StartPos[1]);
	//printf("PF::EndPos[0] = %f, PF::EndPos[1] = %f, PF::CellSize = %f", PF::EndPos[0], PF::EndPos[1], PF::CellSize);
	/*new x22 = PF::BeginX + floatround((PF::EndPos[0] - PF::StartPos[0]) / PF::CellSize),
		y22 = PF::BeginY + floatround((PF::EndPos[1] - PF::StartPos[1]) / PF::CellSize);
	printf("%i <=> %i (%f)", x22, y22, (PF::EndPos[0] - PF::StartPos[0]) / PF::CellSize);*/
	PF::FinalX = PF::BeginX + floatround((PF::EndPos[0] - PF::StartPos[0]) / PF::CellSize),
	PF::FinalY = PF::BeginY + floatround((PF::EndPos[1] - PF::StartPos[1]) / PF::CellSize);
	//printf("PF::FinalX = %i, PF::FinalY = %i", PF::FinalX, PF::FinalY);

	//new restCount = 0;
	while ( !PF::IsFinish(X, Y/*, Z*/) )
	{
		//PF::GetGlobalCoord(X, Y, x2, y2);
		//CreateDynamicPickup(1241, 23, x2, y2, Z);
		/*new str[27];
		format(str, 27, "%i) %i,%i (%i) | %i,%i", restCount, X, Y, PF::GetF(X, Y), PF::GetParentX(X, Y), PF::GetParentY(X, Y));
		CreateDynamic3DTextLabel(str, 0xFFFFFFFF, x2, y2, Z, 30.0);*/
		PF::SetClosed(X, Y);
		PF::SaveZ(X, Y, Z);
		//printf("%i) X, Y, Z = %i,%i,%f", restCount, X, Y, PF::GetSavedZ(X, Y));
		new ceils[8],
			bool: found = false;
		ceils = PF::OpenCeils(X, Y);

		for ( new i; i < 8 && PF::IsCoord(ceils[i]); i ++ ) {
			new Float: curZ,
				curX = PF::GetCoordX(ceils[i]),
				curY = PF::GetCoordY(ceils[i]);
			if ( PF::CheckWall(X, Y, Z, curX, curY, curZ) ) {
				if ( PF::IsRecheck(curX, curY) ) {
					new px = PF::GetParentX(curX, curY),
						py = PF::GetParentY(curX, curY),
						Float: zz;
					if ( !PF::CheckWall(px, py, PF::GetSavedZ(px,py), curX, curY, zz) ) {
						PF::SetParent(curX, curY, X, Y);
						PF::SetG(curX, curY, PF::GetG(X, Y) + 1);
					}
					//printf("check!");
				}
				X = curX,
				Y = curY,
				Z = curZ;
				found = true;
				break;
			}
		}
		if ( !found ) {
			if ( X == PF::BeginX && Y == PF::BeginY ) {
				PF::RESULT = PF::RESULT_FAIL;
				return 0;
			}
			new px = PF::GetParentX(X, Y);
			Y = PF::GetParentY(X, Y);
			X = px;
			Z = PF::GetSavedZ(X, Y);
			//printf("===> Go to parent X, Y, Z = %i,%i,%f", X, Y, Z);
		}
		//if ( restCount == 120 ) return 0;
		//restCount ++;
	}
	//printf("restCount = %i", restCount);

	PF::SaveZ(X, Y, Z);
	return PF::CreatePath(X, Y);
}

//return: make path
stock PF::CreatePath(curX, curY)
{
	while ( PF::PathSize < PF::PATH_SIZE ) {
		if ( curX == PF::BeginX && curY == PF::BeginY ) {
			if ( !PF::RESULT ) {
				PF::RESULT = PF::RESULT_SUCCESS;
			}
			return 1;
		}
		PF::GetGlobalCoord(curX, curY, PF::Path[PF::PathSize][0], PF::Path[PF::PathSize][1]);
		PF::Path[PF::PathSize ++][2] = PF::GetSavedZ(curX, curY);
		//printf(">>>>>> X = %i, Y = %i ||||||>>>>> Z = %i, %f", curX, curY, PF::CoordPlaneZ[curX][curY], PF::GetSavedZ(curX, curY));

		new tempX = curX;
		curX = PF::GetParentX(curX, curY),
		curY = PF::GetParentY(tempX, curY);
	}
	PF::RESULT = PF::RESULT_FAIL;
	return 0;
}

//return: boolean
stock PF::IsFinish(x, y/*, Float: z*/)
{
	if ( x == PF::SIDE_SIZE_X - 1 || y == PF::SIDE_SIZE_Y - 1 ) {
		PF::RESULT = PF::RESULT_SUCCESS_NOT_FULL;
		return 1;
	}
	new Float: globalX, Float: globalY;
	PF::GetGlobalCoord(x, y, globalX, globalY);

	switch ( PF::EndType )
	{
		case PF::END_TYPE_POINT: {
			if ( x == PF::FinalX && y == PF::FinalY ) {
				return 1;
			}
		}
		case PF::END_TYPE_CIRCLE: {
			if ( floatpower(globalX - PF::EndPos[0], 2) + floatpower(globalY - PF::EndPos[1], 2) <= floatpower(PF::EndPos[2], 2) ) {
				return 1;
			}
		}
	}
	return 0;
}

//return: boolean
stock PF::CheckWall(parentX, parentY, Float: parentZ, ceilX, ceilY, &Float: ceilZ)
{
	new Float: StartX = parentX,
		Float: StartY = parentY,
		Float: EndX,
		Float: EndY,
		Float: x, Float: y, Float: z;
	PF::GetGlobalCoord(parentX, parentY, StartX, StartY);
	PF::GetGlobalCoord(ceilX, ceilY, EndX, EndY);
	return ( CA_RayCastLine_(EndX, EndY, parentZ + PF::WALL_UP_MAX, EndX, EndY, parentZ - PF::WALL_DOWN_MAX, x, y, ceilZ) != 0 && (-PF::WALL_DOWN_MAX <= (ceilZ - parentZ) <= PF::WALL_UP_MAX) && CA_RayCastLine_(StartX, StartY, parentZ + 0.5, EndX, EndY, ceilZ + 0.5, x, y, z) == 0 );
}

//return: 1
stock PF::GetGlobalCoord(x, y, &Float: resultX, &Float: resultY)
{
	resultX = PF::StartPos[0] + (x - PF::BeginX) * PF::CellSize;
	resultY = PF::StartPos[1] + (y - PF::BeginY) * PF::CellSize;
	return 1;
}

//return: ceil coord
stock PF::OpenCeils(curX, curY)
{
	new ceils[8],
		ceilsCount;

	for ( new rX = -1; rX <= 1; rX ++ ) {
		for ( new rY = -1; rY <= 1; rY ++ ) {
			if ( rX == 0 && rY == 0 ) continue;
			new x = curX + rX,
				y = curY + rY;

			if ( PF::IsValid(x, y) && !PF::IsClosed(x, y) ) {
				new G = PF::GetG(curX, curY) + 1,
					Float: z;
				if ( !PF::IsOpen(x, y) || (G <= PF::GetG(x, y) && PF::CheckWall(PF::GetParentX(x, y), PF::GetParentY(x, y), PF::GetSavedZ(PF::GetParentX(x, y),PF::GetParentY(x, y)), x, y, z)) ) {
					PF::SetParent(x, y, curX, curY);
					PF::SetG(x, y, G);
					PF::SetRecheck(x, y, 0);
					//printf("child = %i,%i with G = %i, F = %i", x, y, G, PF::GetF(x, y));
				} else {
					if ( G > PF::GetG(x, y) ) {
						PF::SetRecheck(x, y, 1);
					}
				}
				ceils = PF::AddCeilToSortArray(x, y, ceils, ceilsCount ++);
			}
		}
	}

	return ceils;
}

//return: add to ceil array and sort
stock PF::AddCeilToSortArray(x, y, array[8], ceilsCount)
{
	new F = PF::GetF(x, y);
	for ( new i; i < sizeof(array) && PF::IsCoord(array[i]); i ++ ) {
		if ( F < PF::GetF(PF::GetCoordX(array[i]), PF::GetCoordY(array[i])) ) {
			for ( new j = ceilsCount - 1; j != i - 1; j -- ) {
				array[j + 1] = array[j];
			}
			PF::SetCoord(array[i], x, y);

			//для рандома
			if ( i > 0 && PF::GetF(PF::GetCoordX(array[i - 1]), PF::GetCoordY(array[i - 1])) == F ) {
				if ( random(2) == 1 ) {
					new temp = array[i - 1];
					array[i - 1] = array[i];
					array[i] = temp;
				}
			}

			return array;
		}
	}
	PF::SetCoord(array[ceilsCount], x, y);
	return array;
}

//return: F of a ceil
stock PF::GetF(x, y)
{
	new H = 0;
	if ( PF::EndType != PF::END_TYPE_LINE ) {
		H = PF::ABS(PF::FinalX - x) + PF::ABS(PF::FinalY - y);
	}
	return (PF::GetG(x, y) + H);
}

//return: needed axis (0 - x, 1 - y)
stock PF::NeededAxis(x, y)
{
	return PF::ABS(PF::FinalX - x) > PF::ABS(PF::FinalY - y);
}