#include "framework.h"
#include "clsMaze.h"
#include "Robot.h"
int MazeCellWidth;
int MazeCellHeight;

int ptcRobot_x;
int ptcRobot_y;
int dirRobot;
int sizeRobot;//dimensione del robot
int delay=0;
int kStep=0;//contatore passi
const int NLIST=1024;
int List[NLIST];//Lista Nodi Visitati Max NLIST
//ogni elemento di List e' compattato cioe' contiene nei 2 byte superiori la x mentre nei 2 byte inferiori la y
int iList=0;//Indice Lista

//Modificatori Bit:
//Testa il bit n-eseimo del car b
bool isSet(char b, char n) {
	return (b & ( 1 << (3-n))); 
}
//Set a 1 il bit x-esimo
void setBit(char &c, cards x)
{
	c |= 1 << (3-x);
}//WNSE
//azzera il bit x-esimo
void clearBit(char &c, int x)
{
	c &= ~(1 << (3-x));
}

//Disegno Robot a forma di triangolo
void Robot_Draw(HDC hdc)
{
	int dir = dirRobot;
	int d = sizeRobot;
	POINT Pt[3];
	POINT ptc;// = ptcRobot;
	ptc.x = ptcRobot_x;
	ptc.y = ptcRobot_y;
	dirRobot = dir;

	switch(dir)
	{
	case N:
		{
			Pt[0].x = ptc.x; Pt[0].y = ptc.y-d;
			Pt[1].x = ptc.x-d; Pt[1].y = ptc.y+d;
			Pt[2].x = ptc.x+d; Pt[2].y = ptc.y+d;
		}
		break;
	case E:
		{
			Pt[0].x = ptc.x+d; Pt[0].y = ptc.y;
			Pt[1].x = ptc.x-d; Pt[1].y = ptc.y-d;
			Pt[2].x = ptc.x-d; Pt[2].y = ptc.y+d;
		}
		break;
	case S:
		{
			Pt[0].x = ptc.x; Pt[0].y = ptc.y+d;
			Pt[1].x = ptc.x-d; Pt[1].y = ptc.y-d;
			Pt[2].x = ptc.x+d; Pt[2].y = ptc.y-d;
		}
		break;
	case W:
		{
			Pt[0].x = ptc.x-d; Pt[0].y = ptc.y;
			Pt[1].x = ptc.x+d; Pt[1].y = ptc.y-d;
			Pt[2].x = ptc.x+d; Pt[2].y = ptc.y+d;
		}
		break;
	}
	int Mode;
	Mode=SetROP2(hdc,R2_XORPEN);
	Polygon(hdc, Pt, 3);
	Mode = SetROP2(hdc,Mode);
}

LONG f1p(LONG& a) {
	LONG r=a++;
	return r;
}
LONG f1m(LONG& a) {
	LONG r=a--;
	return r;
}
LONG f0(LONG& a) {
	return a;
}
void invoke(int& x, int& y, void (*func1)(int&), void (*func2)(int&))
{
	func1(x);
	func2(y);
}

void Explore(HDC hdc, POINT ps, char &dirs, cards card,LONG (*fx)(LONG&), LONG (*fy)(LONG&)) {
	int i;
	int MazeSize = MazeCellWidth;
	switch (dirs) {
	case W:
	case S:
		MazeSize = MazeCellHeight;
		break;
	}

	MoveToEx(hdc, ps.x, ps.y, NULL); //spara il raggio esploratore...
	for (i = 0; i < MazeSize; i++) //Ampiezza cella labirinto
	{
		COLORREF pxc = GetPixel(hdc, ps.x, ps.y);
		LineTo(hdc, fx(ps.x), fy(ps.y)); 
		if (clsMaze::WALL_COLOR == pxc) //..fino a che il pixel che legge ha il colore del muro
		{
			setBit(dirs, card);//allora metto a 1 il bit corrispondente alla direzione N
			break;
		}
	}
	if (i == MazeSize)//altrimenti la direzione Nord e' libera
		clearBit(dirs, card);//quindi setyto il bit Nord con 0

}
char DiscoveryCell(HDC hdc)
{
	int i = 0;
	char dirs = 0;// 0xF;
	HPEN hPenOri, hPen;
	enum cards card=S;
	
	hPen = CreatePen(PS_DOT, 0, RGB(0, 64, 198));//raggio di esplorazione
	hPenOri = (HPEN)SelectObject(hdc, hPen);
	POINT ps;

	ps.x = ptcRobot_x;
	ps.y = ptcRobot_y;// -sizeRobot / 2;
	
	Explore(hdc, ps, dirs, W, f1m, f0);//W
	Explore(hdc, ps, dirs, N, f0, f1m);//N
	Explore(hdc, ps, dirs, S, f0, f1p);//S
	Explore(hdc, ps, dirs, E, f1p, f0);//E

	SelectObject(hdc, hPenOri);
	DeleteObject(hPen);

	return dirs;
}

//Ruota il robot di +1/-1
char Robot_Rotate(int iAngle)
{
	char oldDir = dirRobot;
	char newDir=0;
	switch (dirRobot)
	{
	case N:
		if(1==iAngle)
			newDir = E;
		else if(-1==iAngle)
			newDir =W;
		break;
	case E:
		if(1==iAngle)
			newDir = S;
		else if(-1==iAngle)
			newDir =N;
		break;
	case S:
		if(1==iAngle)
			newDir = W;
		else if(-1==iAngle)
			newDir =E;
		break;
	case W:
		if(1==iAngle)
			newDir = N;
		else if(-1==iAngle)
			newDir =S;
		break;
	}
	dirRobot=newDir;
	return oldDir;
}

//Ruoto in base all'angolo passato
char Robot_Rotate(char dir)
{
	char oldDir = dirRobot;
	if ((dir == dirRobot) || (dir == 3 && dirRobot == 0) || (dir == 0 && dirRobot == 3) || (dir == 1 && dirRobot == 2) || (dir == 2 && dirRobot == 1))
		return oldDir;

	dirRobot=dir;
	return oldDir;
}

//Sposta il robot nel punto passato
void Robot_Move(int pt_x, int pt_y)
{
	ptcRobot_x = pt_x;
	ptcRobot_y = pt_y;
}

//Inizializza lo scenario
//WCell:Larghezza cella
//HCell: Altezza cella
//RPos: Riga iniziale del robot
//CPos:Colonna Iniziale ddel robot
//dir:direzione iniziale robot
//delay:animazione
void Robot_Init(int WCell, int HCell,int dir,int Delay)//,int RPos, int CPos
{
	kStep=iList=0;
	MazeCellWidth=WCell;
	MazeCellHeight=HCell;
	//ptcRobot.x = 0*MazeCellWidth + MazeCellWidth/2;//CPos
	//ptcRobot.y = 0*MazeCellHeight + MazeCellHeight/2;//RPos
	ptcRobot_x = MazeCellWidth/2;
	ptcRobot_y = MazeCellHeight/2;
	sizeRobot=min(MazeCellWidth,MazeCellHeight)/4;
	int delay=Delay;

}

//aggiunta di un nodo visitato (e compatta)
void List_add(int x,int y)
{
	if(iList>=NLIST)
		throw exception("List Overflow");
	List[iList++] = (x<<16 & 0xFFFF0000) | (y & 0X0000FFFF);
}

//Data la posizione corrente del Robot mi restituisce le coo di dove devo andare secondo la direzione richiesta
void Robot_GetPtcNew(int pt_x, int pt_y, char dir, int & ptNew_x, int &ptNew_y)
{
	ptNew_x=pt_x;
	ptNew_y=pt_y;
	switch(dir)
	{
	case N:
		{
			ptNew_y -= MazeCellHeight;
			break;
		}
	case E:
		{
			ptNew_x += MazeCellWidth ;
			break;
		}
	case S:
		{
			ptNew_y += MazeCellHeight;
			break;
		}
	case W:
		{
			ptNew_x -= MazeCellWidth ;
			break;
		}
	}
	return ;
}

//Decodifica bits direzioni 
char DecodeDir(int i)
{
	switch(i)
	{
	case 0:
		return W;
	case 1:
		return N;
	case 2:
		return S;
	case 3:
		return E;
	}
	return 0;
}
//dato una COO mi dice se ci sono stato
bool IsVisited(int pt_x, int pt_y)
{
	int v = (pt_x <<16 & 0xFFFF0000) | (pt_y & 0X0000FFFF);
	for(int i=0; i < iList; i++)
	{
		if(List[i] == v)
			return true;
	}
	return false;
}

//decompatta un elemento della lista
bool List_toInt(int i, int &x, int &y)
{
	if(!(i <= iList))
		return false;
	int v =	List[i];
	y=v & 0x0000FFFF;
	x=(v>>16) & 0x0000FFFF;
	if(x & 0x8000)
		x |= 0xFFFFF0000;
	if(y & 0x8000)
		y |= 0xFFFFF0000;
	return true;
}

//Algoritmo di risoluzione (DFS)
int Robot_SolveDFS(HDC hdc,HDC hdcMem, int pt_x, int pt_y, char dir)
{ 
	Sleep(delay);
	int ptNew_x=0;
	int ptNew_y=0;
	RECT cr;
	HWND hWnd = WindowFromDC(hdc);
	GetClientRect(hWnd, &cr);

	int width = cr.right - cr.left;
	int height = cr.bottom - cr.top;

	List_add(pt_x,pt_y);//memorizza il nodo visitato

	Robot_Draw(hdcMem);
	Robot_Move(pt_x,pt_y);
	Robot_Rotate(dir);
	Robot_Draw(hdcMem);
	

	unsigned char dirs=0;
	dirs = DiscoveryCell(hdcMem);//esplorazione celle vicine
	/*if (0 == kStep)//se sono all'inizio mi ruoto e esploro
	{
		Robot_Draw(hdc);
		Robot_Rotate(1);
		Robot_Draw(hdc);

		char dirs2=0xF;
		dirs2 = DiscoveryCell(hdc);//cella esplorata
		dirs |= dirs2;//unione (or) cella corrente e cella precedente
	}*/
	kStep++;
	for(int i=0; i < sizeof(cards); i++)//4 vicini
	{
		if(!isSet(dirs,i))//e' libero
		{
			char newDir = DecodeDir(i);//Direzione
			//Robot_GetPtcNew(ptcRobot_x,ptcRobot_y,newDir,ptNew_x,ptNew_y);//calcolo Nuova coo
			Robot_GetPtcNew(pt_x, pt_y, newDir, ptNew_x, ptNew_y);//calcolo Nuova coo
			if(!IsVisited(ptNew_x,ptNew_y)){//E' stato visitato?
				Robot_SolveDFS(hdc,hdcMem,ptNew_x,ptNew_y,newDir);//No: Contiua ricorsivamente
}
			Robot_Draw(hdcMem);
			Robot_Move(pt_x,pt_y);
			Robot_Rotate(dir);
			Robot_Draw(hdcMem);
			BitBlt(hdc, 0, 0, width, height, hdcMem, 0, 0, SRCCOPY);
			
			Sleep(delay);
		}
	}
	return kStep;
}

/////////////////TODO Overlap Tolleranza
//struct myRect
//{
//	int left;
//	int top;
//	int right;
//	int bottom;
//	int w;
//	int h;
//
//	bool overlap(myRect &B)
//	{
//		bool bOverlap=false;
//		if(left==B.left && right==B.right && B.top==top && B.bottom == bottom)
//			return true;
//		bOverlap= ((left < B.right &&  B.left < right )
//			&& (top<B.bottom  && B.top < bottom));
//		return bOverlap;
//
//	}
//
//	bool overlap(POINT &p)
//	{
//		bool bOverlap=false;
//		bOverlap = (left<=p.x && p.x <= right) && (p.y >= top && p.y <= bottom );
//		return bOverlap;
//	}
//
//	myRect()
//	{
//		w=CELL_WIDTH;
//		h=CELL_HEIGHT;
//	}
//	myRect(int wx,int hy)
//	{
//		w=wx;
//		h=hy;
//	}
//	myRect &toRect(POINT pt)
//	{
//		this->left = pt.x - w/2;  
//		this->top = pt.y - h/2;  
//		this->right = pt.x + w/2;  
//		this->bottom = pt.y + h/2;  
//		return *this;
//	}
//};
//
//bool overlap(POINT p, POINT q)
//{
//	int dx=4;
//	int dy=4;
//	myRect rcx(dx,dy);
//	rcx=rcx.toRect(p);
//	return rcx.overlap(q); 
//}
//
//bool bVisited(int x, int y)
//{
//	bool bOverlap=false;
//	POINT ptx;
//	ptx.x=x;
//	ptx.y=y;
//	list<POINT>::iterator it;
//	for(it=List.begin(); it != List.end(); it++)
//	{
//		POINT p;
//		p = *it;
//		if(overlap(p,ptx))
//		{
//			bOverlap=true;
//			break;
//		}
//	}
//	return bOverlap;
//}
//