//pathfinding

waypoint map[WAYPOINTS];



int map_counter=0;													//anzahl der mapeinträge (indizes: -1)



void find_possible_ways(int point, char *points, int *num_points)

{

	int i;

	for(i=point;i<map_counter;i++)

	{

		if((map[i].x_start == map[point].x_stop && map[i].y_start == map[point].y_stop))	

		{			

			points[(*num_points)++]=i;

		}

	}

}



void arraycopy(char *from,char *to,int anzahl)

{

	int i;

	for(i=0;i<anzahl;i++)

	{

		to[i]=from[i];

	}

}



void checkwege(char wege[MAXWEGE][WAYPOINTS],int len_weg[MAXWEGE],int *num_wege, int aktueller_weg)

{

	char possible_ways[3];

	int num_points=0,i,len_tmp=0;

	char tmp[WAYPOINTS];

	int aktueller_punkt;

	

	aktueller_punkt = wege[aktueller_weg][len_weg[aktueller_weg]-1];

	

	find_possible_ways(aktueller_punkt,possible_ways,&num_points);

		

	if((num_points == 0) || (aktueller_punkt >= map_counter))

	{

		//fertig

	}

	else

	{		

		arraycopy(wege[aktueller_weg],tmp, len_weg[aktueller_weg]);

		len_tmp=len_weg[aktueller_weg];

		

		wege[aktueller_weg][len_weg[aktueller_weg]++]=possible_ways[0];

		checkwege(wege,len_weg,num_wege, aktueller_weg);

		

		for(i=1;i<num_points;i++)

		{	

			arraycopy(tmp,wege[(*num_wege)++],len_tmp);

			wege[(*num_wege)-1][len_tmp]=possible_ways[i];

			len_weg[(*num_wege)-1] = len_tmp+1;

			

			checkwege(wege,len_weg,num_wege, *num_wege-1);		

		}

	}

}



long int length_of_weg(char dieser_weg[WAYPOINTS],int hops)	

{

	long int laenge=0;

	int i,h;

	for(i=0;i<hops;i++)

	{

		h=(map[(int)dieser_weg[i]].x_start-map[(int)dieser_weg[i]].x_stop);

		laenge+=(h*h);

		h=(map[(int)dieser_weg[i]].y_start-map[(int)dieser_weg[i]].y_stop);

		laenge+=(h*h);

	}

	return laenge;

}



void addwp(unsigned int start, unsigned int end, int direction)

{

	

	if((map_counter) <= WAYPOINTS)

	{

		unsigned int difference=(end-start);

		signed char x_tmp=0,y_tmp=0;

		int i,found=0;

		

		map[map_counter].direction=direction;

		

		map[map_counter].x_start=map[map_counter-1].x_stop;

		map[map_counter].y_start=map[map_counter-1].y_stop;

				

		if(direction == MAP_LEFT)

		{

			x_tmp=map[map_counter-1].x_stop-difference;

			y_tmp=map[map_counter-1].y_stop;

		}

		else if(direction == MAP_RIGHT)

		{

			x_tmp=map[map_counter-1].x_stop+difference;

			y_tmp=map[map_counter-1].y_stop;

			

		}

		else if(direction == MAP_UP)

		{

			x_tmp=map[map_counter-1].x_stop;

			y_tmp=map[map_counter-1].y_stop+difference;			

		}

		else if(direction == MAP_DOWN)

		{

			x_tmp=map[map_counter-1].x_stop;

			y_tmp=map[map_counter-1].y_stop-difference;			

		}

		

		for(i=0;i<map_counter;i++)

		{

			if((abs(map[i].x_start-x_tmp) < (TURNLIMIT-2)) && abs(map[i].y_start-y_tmp) < (TURNLIMIT-2))

			{

				found=1;

				map[map_counter].x_stop=map[i].x_start;

				map[map_counter].y_stop=map[i].y_start;

				break;

			}

			else if(0)

			{

				ledsOff();

				motor(MOT_LEFT,0);

				motor(MOT_RIGHT,0);

				ledOn(2);

				waitForButton(2);

				showBin(i);

				waitForButton(3);

				showBin(map[i].x_start);

				waitForButton(3);

				showBin(x_tmp);

				waitForButton(3);

				showBin(map[i].y_start);

				waitForButton(3);

				showBin(y_tmp);

				waitForButton(3);

			}			

		}

		

		if(!found)

		{

			map[map_counter].x_stop=x_tmp;

			map[map_counter].y_stop=y_tmp;

		}

		

		map_counter++;		

	}

	else

	{

		error(1);

	}

}



bool getdirection(int index1, int index2,int direction)

{

	if(direction == MAP_UP)

	{

		if((map[index1].direction == map[index2].direction) && map[index1].direction == MAP_UP)

		{

			return true;

		}

	}

	else if(direction == MAP_RIGHT)

	{

		if((map[index1].direction == MAP_UP && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_UP))

		{

			return true;

		}

	}

	else if(direction == MAP_LEFT)

	{

		if((map[index1].direction == MAP_UP && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_UP))

		{

			return true;

		}

	}

	return false;

}



int where_should_i_go(int index1,int index2)

{

	if((map[index1].direction == MAP_UP && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_UP))

	{

		return MAP_LEFT;

	}

	else if((map[index1].direction == MAP_UP && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_UP))

	{

		return MAP_RIGHT;

	}

	else return MAP_UP;

}