// path finding app by Michael Morton 
//
// GPL
//
// feel free to clean up the code and send me back a copy : )
#include "resource.h"
#include "math.h"
#include <vector>
#include <map>
using namespace std;

#include "d3dx.h"
#pragma comment(lib,"d3dx.lib")

// Global Variables:
HINSTANCE hInst;								// current instance
TCHAR szTitle[]="title";								// The title bar text
TCHAR szWindowClass[]="classname";								// The title bar tex



/**********************************************************
*
*  GLOBAL FUNCTIONS
*
***********************************************************/


/* GetTriPoint
*	
*  returns a point in the direction a tri faces.
*
*     * new point
*      
*     p1
*     /\
*	 /  \
*   p2   p3
*/

D3DXVECTOR3 GetTriPoint(const D3DXVECTOR3 &p1,const D3DXVECTOR3 &p2,const D3DXVECTOR3 &p3)
{
	D3DXVECTOR3 p2p1,p3p1;

	D3DXVECTOR3 sum;

	//vector from p2 to p1
	p2p1 = p1-p2;
	D3DXVec3Normalize(&sum,&p2p1);
	p2p1 = sum;

	//vector from p3 to p1
	p3p1 = p1-p3;
	D3DXVec3Normalize(&sum,&p3p1);
	p3p1 = sum;


	//add the vectors together to produce new vector
	D3DXVECTOR3 temp;
	temp=p2p1+p3p1;

	//normalize the vector to reduce the length to 1 unit
	D3DXVec3Normalize(&sum,&temp);
	
	// scale the vector... no good if its < 1. : )
	sum*=5; 

	//add the new vector to p1 (offset it)
	sum+=p1;
	return sum;
}


/**************************************************************************************************
Desc: Taken from comp.graphics.algorithms FAQ
**************************************************************************************************/
bool IntersectionOfTwoLines(const D3DXVECTOR3 &a,const D3DXVECTOR3 &b,const D3DXVECTOR3 &c,
								   const D3DXVECTOR3 &d, D3DXVECTOR3 &result)
{
	double r,s;	
	
	double denominator=(b.x-a.x)*(d.y-c.y)-(b.y-a.y)*(d.x-c.x);

	// If the denominator in above is zero, AB & CD are colinear
	if (denominator==0)
		return FALSE;

    double numeratorR=(a.y-c.y)*(d.x-c.x)-(a.x-c.x)*(d.y-c.y);
	//	If the numerator above is also zero, AB & CD are collinear.
	//	If they are collinear, then the segments may be projected to the x- 
	//	or y-axis, and overlap of the projected intervals checked.

	r=numeratorR/denominator;

	double numeratorS=(a.y-c.y)*(b.x-a.x)-(a.x-c.x)*(b.y-a.y);

	s=numeratorS/denominator;

	//	If 0<=r<=1 & 0<=s<=1, intersection exists
	//	r<0 or r>1 or s<0 or s>1 line segments do not intersect
	if (r<0 || r>1 || s<0 || s>1)
		return FALSE;

/*
	Note:
    If the intersection point of the 2 lines are needed (lines in this
    context mean infinite lines) regardless whether the two line
    segments intersect, then

        If r>1, P is located on extension of AB
        If r<0, P is located on extension of BA
        If s>1, P is located on extension of CD
        If s<0, P is located on extension of DC
*/

	// Find intersection point
	result.x=a.x+(r*(b.x-a.x));
	result.y=a.y+(r*(b.y-a.y));

	return TRUE;
}

//get red wingdi pen
HANDLE GetRedPen()
{
	static HANDLE redPen=NULL;
	if(redPen==NULL)
	{
		LOGPEN lp;
		POINT p;
		p.x=1;
		p.y=1;
		lp.lopnColor=RGB(255,0,0);
		lp.lopnStyle=0;
		lp.lopnWidth=p;
		redPen = CreatePenIndirect(&lp);
	}
	return redPen;

}

//get black wingdi pen
HANDLE GetBlackPen()
{
	return GetStockObject(BLACK_PEN);
}


//returns distance between 2 points
double GetDistance(D3DXVECTOR3 a,D3DXVECTOR3 b)
{
	D3DXVECTOR3 temp;
	return sqrt(pow(b.x-a.x,2)+pow(b.y-a.y,2));
}


/*********************************************************
*
*   CLASSES
*
**********************************************************/
class  vObstacle
{
public:
	vector<D3DXVECTOR3 > Point;
	
	const vObstacle& operator =(const vObstacle &)
	{
		return *this;
	}
};

class vObstacles
{
public:
	vector<vObstacle > Obstacles;

	void ClearList(){
		while(Obstacles.size())
		{
			while(Obstacles[Obstacles.size()-1].Point.size())
				Obstacles[Obstacles.size()-1].Point.pop_back();
			Obstacles.pop_back();
		}
		
	}

	const vObstacles& operator =(const vObstacles &)
	{
		return *this;
	}
};

/*********************************************
*
*  GLOBAL VARIABLES
*
**********************************************/
vObstacles olist;
int CreatingObstacles=0;
int CreatingNewObstacle=0;
int SettingStart=0;
int SettingEnd=0;
int RunPath=0;

D3DXVECTOR3 startPoint(50,50,0);
D3DXVECTOR3 endPoint(150,150,0);



class CBeenTo
{
public:
	int BeenToObject;
	std::vector<int> BeenToVertex;
};

class pathFinder{
public:
	D3DXVECTOR3 direction;
	D3DXVECTOR3 location;
	float speed;

	BOOL foundPath;

	int nextPoint;
	D3DXVECTOR3 vNextPoint;

	std::vector<CBeenTo> BeenToList;
	std::vector<D3DXVECTOR3> path;
	
	int useOptimizedPath;
	int closestObject;
	int closestVertex;
	int foundClosest;
	float dist;

	pathFinder()
	{
		useOptimizedPath = 1;

		closestObject=0;
		closestVertex=0;
		foundClosest=0;
		dist=0.0f;

		foundPath=FALSE;
		nextPoint=0;
		speed=10.f;
		SetStartPoint();
	}

    BOOL haveBeenToObjPoint(int objIndex,int vertexIndex)
    {

        int SkipVertex=0;
        //make shure we havent already tried this point
		if(BeenToList.size())
		{
			for(int k=0;k<BeenToList.size();k++)
			{
				if(BeenToList[k].BeenToObject == objIndex)
					for(int l=0;l<BeenToList[k].BeenToVertex.size(); l++)
						if(BeenToList[k].BeenToVertex[l]==vertexIndex)
						{
							SkipVertex=1;
							break;
						}
				if(SkipVertex==1)
					break;
			}
		}
        return SkipVertex?TRUE:FALSE;
    }
    
	int GetClosestVisiblePointToGoal(const D3DXVECTOR3 &fromPoint)
	{
		double fClosest=100E100;
		int SkipVertex=0;
		int didfind=0;
		
		for(int i=0;i<olist.Obstacles.size();i++)
		{
            
			if(olist.Obstacles[i].Point.size()>2)
			for(int j=0;j<olist.Obstacles[i].Point.size();j++)
			{
                if(haveBeenToObjPoint(i,j)==TRUE)
                {
                    continue;
                }

				D3DXVECTOR3 p=olist.Obstacles[i].Point[j];

				// get prev point
				D3DXVECTOR3 temp = olist.Obstacles[i].Point[j==0?olist.Obstacles[i].Point.size()-1:j-1];
				//get next point
				D3DXVECTOR3 temp2 = olist.Obstacles[i].Point[j==olist.Obstacles[i].Point.size()-1?0:j+1];

				D3DXVECTOR3 out = GetTriPoint(p,temp,temp2);

				if(CheckObstacles(fromPoint, out)==false)
				{	
					
					dist = GetDistance(p,endPoint);
					
					if( dist < fClosest)
					{
						nextPoint = 1;
						closestObject=i;
						closestVertex=j;
						fClosest=dist;
						didfind=1;

						if(CheckObstacles(out,endPoint)==FALSE)
						{
							vNextPoint = olist.Obstacles[closestObject].Point[closestVertex];
							return 0;
						}
					}
				}
			}
		}
		if(didfind)
		{
			vNextPoint = olist.Obstacles[closestObject].Point[closestVertex];
			BeenToList.push_back(CBeenTo());
			BeenToList[BeenToList.size()-1].BeenToObject=closestObject;
			BeenToList[BeenToList.size()-1].BeenToVertex.push_back(closestVertex);		
		}
		return didfind;
	}

	void ClearPath()
	{
		foundPath=FALSE;
		while(path.size())
			path.pop_back();

		foundClosest=0;
		while(BeenToList.size())
			BeenToList.pop_back();
	}

	void DrawPath(HDC hdc)
	{
		POINT lastPoint;
		if(path.size())
		for(int i=0;i<path.size()-1;i++)
		{
			SelectObject(hdc,GetRedPen());

			MoveToEx(hdc,path[i].x,path[i].y,&lastPoint);
			LineTo(hdc,path[i+1].x,path[i+1].y);
		}
	}


	void GetPath()
	{
		if(foundPath==FALSE)
		{
			ClearPath();
			if(CheckObstacles(location,endPoint)==FALSE)
			{
				foundPath=TRUE;
				path.push_back(location);
				path.push_back(endPoint);
				nextPoint=path.size()-1;
			}
			else
			{
				path.push_back(location);
				vNextPoint = location;

                while(1)
                {
				    while(GetClosestVisiblePointToGoal(vNextPoint))
				    {
					    // get prev point
					    D3DXVECTOR3 temp  = olist.Obstacles[closestObject].Point[closestVertex==0?olist.Obstacles[closestObject].Point.size()-1:closestVertex-1];
					    //get next point
					    D3DXVECTOR3 temp2 = olist.Obstacles[closestObject].Point[closestVertex==olist.Obstacles[closestObject].Point.size()-1?0:closestVertex+1];

					    D3DXVECTOR3 out = GetTriPoint(vNextPoint,temp,temp2);

					    path.push_back(out);
					    vNextPoint = path[path.size()-1];
				    }      
                    
                    if(path.size()>1)
                    {

                        if(olist.Obstacles.size())
					        if(olist.Obstacles[0].Point.size()>2)
					        {
						        // get prev point
						        D3DXVECTOR3 temp = olist.Obstacles[closestObject].Point[closestVertex==0?olist.Obstacles[closestObject].Point.size()-1:closestVertex-1];
						        //get next point
						        D3DXVECTOR3 temp2 = olist.Obstacles[closestObject].Point[closestVertex==olist.Obstacles[closestObject].Point.size()-1?0:closestVertex+1];

						        D3DXVECTOR3 out = GetTriPoint(vNextPoint,temp,temp2);

						        path.push_back(out);
						        vNextPoint = path[path.size()-1];
					        }


                        if(CheckObstacles(path[path.size()-1],endPoint)==false)
                        {
                            path.pop_back();
                            break;
                        }
                        else
                        {
                            path.pop_back();
                            path.pop_back();
                            vNextPoint = path[path.size()-1];
                        }
                    }
                    else
                    {
                        break;
                    }
                    
                    
                }

				DoCleanPath();

                
				if(olist.Obstacles.size())
					if(olist.Obstacles[0].Point.size()>2)
					{
						// get prev point
						D3DXVECTOR3 temp = olist.Obstacles[closestObject].Point[closestVertex==0?olist.Obstacles[closestObject].Point.size()-1:closestVertex-1];
						//get next point
						D3DXVECTOR3 temp2 = olist.Obstacles[closestObject].Point[closestVertex==olist.Obstacles[closestObject].Point.size()-1?0:closestVertex+1];

						D3DXVECTOR3 out = GetTriPoint(vNextPoint,temp,temp2);

						path.push_back(out);
						vNextPoint = path[path.size()-1];
					}
                   
				path.push_back(endPoint);
				
				foundPath=TRUE;
			}
		}
	}

	void DoCleanPath()
	{
		if(useOptimizedPath==1)
			while(!CleanPath()){}
	}

	int CleanPath()
	{
		int foundAt=-1;
		double foundAtDist=100E100;

		//make shure path is the best possible
		for(int i=0; i < path.size()-1; i++)
		{
			for(int j=i+1; j < path.size(); j++)
			{
				if(CheckObstacles(path[i],path[j])==FALSE)
				{
					if(j > i +1)
					{
						double temp=GetDistance(path[j],endPoint);
						if(temp < foundAtDist)
						{
							foundAt=j;
							foundAtDist = temp;
						}
					}
				}
			}
			if(foundAt!=-1)
			{
				for(int k=foundAt-1; k>i; k--)
				{
					path.erase(&path.at(k));
				}
				return 0;
			}
		}
		return 1;
	}


	void UpdatePosition(HWND hWnd)
	{
		if(RunPath)
		{
			if(foundPath==TRUE)
			{
				
				if(location!=path[nextPoint])
				{
					direction = path[nextPoint] -startPoint;
					D3DXVECTOR3 out;
					D3DXVec3Normalize(&out,&direction);
					direction=out;
					if(GetDistance(location,path[nextPoint])<=speed)
					{
						location=path[nextPoint];
						nextPoint++;
						
					}
					else
					{
						location+=direction*speed;
					}
				}
				else
				{
					location=path[nextPoint];
					nextPoint++;
				}
			
			}
			if(nextPoint == path.size())
				RunPath=0;

			startPoint=location;
			InvalidateRect(hWnd,NULL,false);
		}
	}

	void SetStartPoint()
	{
		location=startPoint;
		direction = endPoint-startPoint;
		D3DXVECTOR3 out;
		D3DXVec3Normalize(&out,&direction);
		direction=out;
	}
	
	BOOL CheckObstacles(const D3DXVECTOR3 &point,const D3DXVECTOR3 &dest)
	{
		
		for(int j=0;j<olist.Obstacles.size();j++)
		{
			vObstacle ob = olist.Obstacles[j];

			for(int i=0;i<ob.Point.size();i++)
			{
				D3DXVECTOR3 t1,t2;
				D3DXVECTOR3 a = ob.Point[i];
				D3DXVECTOR3 b = ob.Point[i==ob.Point.size()-1?0:i+1];
				D3DXVECTOR3 result;
				if(IntersectionOfTwoLines(a,b,point,dest,result))
				{
					return true;
				}
			}
		}
		return false;
	}

	
};


pathFinder person;








// Foward declarations of functions included in this code module:
ATOM				MyRegisterClass(HINSTANCE hInstance);
BOOL				InitInstance(HINSTANCE, int);
LRESULT CALLBACK	WndProc(HWND, UINT, WPARAM, LPARAM);


int APIENTRY WinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPSTR     lpCmdLine,
                     int       nCmdShow)
{
 	// TODO: Place code here.
	MSG msg;
	
	MyRegisterClass(hInstance);

	// Perform application initialization:
	if (!InitInstance (hInstance, nCmdShow)) 
	{
		return FALSE;
	}

	// Main message loop:
	while (GetMessage(&msg, NULL, 0, 0)) 
	{
		TranslateMessage(&msg);
		DispatchMessage(&msg);
	}

	return msg.wParam;
}


ATOM MyRegisterClass(HINSTANCE hInstance)
{
	WNDCLASSEX wcex;

	wcex.cbSize = sizeof(WNDCLASSEX); 

	wcex.style			= CS_HREDRAW | CS_VREDRAW;
	wcex.lpfnWndProc	= (WNDPROC)WndProc;
	wcex.cbClsExtra		= 0;
	wcex.cbWndExtra		= 0;
	wcex.hInstance		= hInstance;
	wcex.hIcon			= LoadIcon(hInstance, (LPCTSTR)IDI_TEST1);
	wcex.hCursor		= LoadCursor(NULL, IDC_ARROW);
	wcex.hbrBackground	= (HBRUSH)(COLOR_WINDOW+1);
	wcex.lpszMenuName	= (LPCSTR)IDC_TEST1;
	wcex.lpszClassName	= szWindowClass;
	wcex.hIconSm		= LoadIcon(wcex.hInstance, (LPCTSTR)IDI_SMALL);

	return RegisterClassEx(&wcex);
}


BOOL InitInstance(HINSTANCE hInstance, int nCmdShow)
{
   HWND hWnd;

   hInst = hInstance; // Store instance handle in our global variable

   hWnd = CreateWindow(szWindowClass, szTitle, WS_OVERLAPPEDWINDOW,
      CW_USEDEFAULT, 0, CW_USEDEFAULT, 0, NULL, NULL, hInstance, NULL);

   if (!hWnd)
   {
      return FALSE;
   }

   ShowWindow(hWnd, nCmdShow);
   UpdateWindow(hWnd);

   return TRUE;
}

LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
	int wmId, wmEvent;
	PAINTSTRUCT ps;

	static HDC memdc=0;
	static HBITMAP membmp=0;

	long width,height;

	RECT r;

	HDC hdc1;
	HDC hdc;
	
	switch (message) 
	{
		case WM_COMMAND:
			wmId    = LOWORD(wParam); 
			wmEvent = HIWORD(wParam); 
			// Parse the menu selections:
			switch (wmId)
			{
				case IDM_EXIT:
				   DestroyWindow(hWnd);
				   break;
				case ID_STARTPOINT:
					CreatingNewObstacle=0;
					CreatingObstacles=0;
					SettingEnd =0;
					SettingStart=1;
					break;
				case ID_SETEND:
					SettingEnd = 1;
					CreatingNewObstacle=0;
					CreatingObstacles=0;
					SettingStart=0;
					break;
				case ID_RUNPATH:
					RunPath=~RunPath;
					InvalidateRect(hWnd,NULL,false);
					break;
				case ID_OPTIMIZE:
					person.useOptimizedPath= person.useOptimizedPath==1?0:1;
					person.ClearPath();
					InvalidateRect(hWnd,NULL,false);
					break;
				case ID_EDIT_ADDOBSTACLE:
					SettingStart=0;
					SettingEnd=0;
					CreatingObstacles = ~CreatingObstacles;
					if(CreatingObstacles)
						CreatingNewObstacle = 1;
					InvalidateRect(hWnd,NULL,false);
					break;
				case ID_SAVE:
					{
						FILE *fp=fopen("path.txt","wt");
						for(int x=0;x<olist.Obstacles.size(); x++)
						{
							fprintf(fp,"obstacle\n");
							for(int y=0;y<olist.Obstacles[x].Point.size();y++)
							{
								D3DXVECTOR3 p=olist.Obstacles[x].Point[y];
								fprintf(fp,"%d %d\n", (int)p.x,(int)p.y);
							}
						}		
						fclose(fp);
					}
					break;
				case ID_OPEN:
					{
						olist.ClearList();
						
						FILE *fp=fopen("path.txt","rt");
						if(fp)
						{
							char buff[255];
							while(!feof(fp))
							{
								fgets(buff,255,fp);
								if(strcmp(buff,"obstacle\n")==0)
								{
									olist.Obstacles.push_back(vObstacle());
								}
								else
								{
									int x,y;
									sscanf(buff,"%d %d",&x,&y);

									olist.Obstacles[olist.Obstacles.size()-1].Point.push_back(D3DXVECTOR3(x,y,0));
								}
							}
							fclose(fp);
							person.ClearPath();
							InvalidateRect(hWnd,NULL,false);
						}
					}
					break;
				default:
				   return DefWindowProc(hWnd, message, wParam, lParam);
			}
			break;
		case WM_PAINT:

			

			//create double buffer
			hdc1 = BeginPaint(hWnd, &ps);
			{
				
				GetClientRect(hWnd,&r);
				
				static long lastWidth=0,lastHeight=0;

				width = r.right-r.left;
				height = r.bottom - r.top;

				if(memdc==0 || lastWidth != width || lastHeight != height)
				{
					lastWidth = width;
					lastHeight = height;

					memdc = CreateCompatibleDC(hdc1);
					membmp = CreateCompatibleBitmap(hdc1,width,height);
					SelectObject(memdc,membmp);
				}
			}
			FillRect(memdc,&r,(HBRUSH)GetStockObject(WHITE_BRUSH));
			
			hdc = memdc;

			int hasIntersect;

			hasIntersect = person.CheckObstacles(person.location,person.location+(person.direction*person.speed));

			person.GetPath();
			person.UpdatePosition(hWnd);
			person.DrawPath(hdc);

			
			// TODO: Add any drawing code here...
			RECT rt;
			GetClientRect(hWnd, &rt);
			{
				char buff[255];
				sprintf(buff,"Creating: %d  Distance: %.2f",CreatingObstacles,GetDistance(startPoint,endPoint));
				TextOut(hdc,0,0,buff,strlen(buff));
				sprintf(buff,"Optimized: %d",person.useOptimizedPath);
				TextOut(hdc,0,15,buff,strlen(buff));
			}
			
			{
				

				static int first=1;
				POINT lastPoint;
				for(int i=0;i<olist.Obstacles.size();i++)
				{
					first=1;
					for(int j=0;j<olist.Obstacles[i].Point.size();j++)
					{
						D3DXVECTOR3 zvp=olist.Obstacles[i].Point[j];
						if(first==1)
						{
							MoveToEx(hdc,zvp.x,zvp.y,&lastPoint);
							first=0;
						}
						else
						{
							SelectObject(hdc,GetBlackPen());

							LineTo(hdc,zvp.x,zvp.y);
							if(olist.Obstacles[i].Point.size()-1==j)
							{
								MoveToEx(hdc,zvp.x,zvp.y,&lastPoint);
								LineTo(hdc,olist.Obstacles[i].Point[0].x,olist.Obstacles[i].Point[0].y);
							}
						}
						
					}
				}
			}

			//draw start location box
			RECT r;
			r.left=startPoint.x-5;
			r.top = startPoint.y-5;
			r.bottom = r.top + 25;
			r.right = r.left + 25;
			FillRect(hdc,&r,(HBRUSH)GetStockObject(GRAY_BRUSH));
			TextOut(hdc,startPoint.x,startPoint.y,"S",1);
			

			//draw end location box
			r.left=endPoint.x-5;
			r.top = endPoint.y-5;
			r.bottom = r.top + 25;
			r.right = r.left + 25;
			FillRect(hdc,&r,(HBRUSH)GetStockObject(GRAY_BRUSH));
			TextOut(hdc,endPoint.x,endPoint.y,"E",1);

			//draw direction vector
			{
				SelectObject(hdc,GetBlackPen());

				POINT lastPoint;
				MoveToEx(hdc,person.location.x,person.location.y,&lastPoint);
				LineTo(hdc,person.location.x+person.direction.x*person.speed,person.location.y+ person.direction.y*person.speed);
			}

			//draw the path
			{
				if(person.foundPath)
					person.DrawPath(hdc);
			}

			BitBlt(hdc1,0,0,width,height,memdc,0,0,SRCCOPY);

			EndPaint(hWnd, &ps);
			break;
		case WM_LBUTTONDOWN:
			if(CreatingObstacles)
			{
				person.ClearPath();
				if(CreatingNewObstacle==1)
				{
					CreatingNewObstacle=0;
					olist.Obstacles.push_back(vObstacle());
				}

				olist.Obstacles[olist.Obstacles.size()-1].Point.push_back(D3DXVECTOR3(LOWORD(lParam),HIWORD(lParam),0));
			}
			else if(SettingStart)
			{
				person.ClearPath();
				SettingStart=0;
				startPoint.x = LOWORD(lParam);
				startPoint.y = HIWORD(lParam);
				person.SetStartPoint();
			}
			else if(SettingEnd)
			{
				person.ClearPath();
				SettingEnd=0;
				endPoint.x = LOWORD(lParam);
				endPoint.y = HIWORD(lParam);
				person.SetStartPoint();
			}
			InvalidateRect(hWnd,NULL,false);
			break;
		case WM_RBUTTONDOWN:
			if(CreatingObstacles)
			{
				person.ClearPath();
				CreatingObstacles=0;
			}
			else
			{
				if(olist.Obstacles.size())
				{
					while(olist.Obstacles[olist.Obstacles.size()-1].Point.size())
						olist.Obstacles[olist.Obstacles.size()-1].Point.pop_back();
					olist.Obstacles.pop_back();
				}
				person.ClearPath();
			}
			InvalidateRect(hWnd,NULL,false);
			break;

		case WM_DESTROY:
			olist.ClearList();
		
			DeleteObject(GetRedPen());

			//double buffer
			SelectObject(memdc,NULL);
			DeleteDC(memdc);
			DeleteObject(membmp);

			PostQuitMessage(0);

			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
   }
   return 0;
}