/*  TA3D, a remake of Total Annihilation
    Copyright (C) 2005  Roland BROCHARD

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA*/

/*-----------------------------------------------------------------\
|                           pathfinding.cpp                        |
|   contient toutes les fonctions et classes nécessaires à la mise |
| en place du pathfinding de TA3D                                  |
\-----------------------------------------------------------------*/

#include <math.h>
#include <stdio.h>
#include <string.h>
#include <allegro.h>
#include <alleggl.h>
#include <GL/glu.h>
#include "ta3dbase.h"
#include "3do.h"					// Pour la lecture des fichiers 3D
#include "cob.h"					// Pour la lecture et l'éxecution des scripts
#include "tdf.h"					// Pour la gestion des éléments du jeu
//#include "fbi.h"					// Pour la gestion des unités
//#include "weapons.h"				// Pour la gestion des armes
#include "EngineClass.h"			// Inclus le moteur(dont le fichier pathfinding.h)

#define PATH_MAX_LENGTH		1500

PATH_NODE *direct_path(VECTOR End)		// Chemin direct vers la cible
{
	PATH_NODE *path=(PATH_NODE*) malloc(sizeof(PATH_NODE));
	path->Pos=End;			// Seule étape : l'arrivée
	path->next=NULL;		// Fin du chemin
	return path;
}

PATH_NODE *compute_path(SECTOR **map_data,byte **map,float **h_map,int map_w,int map_h,int bloc_w,int bloc_h,float dh_max,float h_min,VECTOR Start,VECTOR End,bool level,int mw,int mh,int u_idx)
{
	PATH_NODE node_start,node_end;
	node_start.x=(int)((Start.x+map_w*0.5f)*bloc_w/map_w);
	node_start.y=(int)((Start.z+map_h*0.5f)*bloc_h/map_h);
	node_end.x=(int)((End.x+map_w*0.5f)*bloc_w/map_w);
	node_end.y=(int)((End.z+map_h*0.5f)*bloc_h/map_h);

	memset(map[0],0,bloc_w*bloc_h);		// Nettoie la carte de passage
	int h=0;
	PATH_NODE *path;
	if(!level)
		path=find_path(map_data,map,h_map,bloc_w,bloc_h,dh_max,h_min,node_start,node_end,&h,mw,mh,u_idx);	// Cherche un chemin
	else
		path=find_path_level(map_data,map,h_map,bloc_w,bloc_h,dh_max,node_start,node_end,&h,mw,mh,u_idx);	// Cherche un chemin
	if(path) {		// Si on en a trouvé un, on l'optimise
		short_path(path);
		simplify_path(path);
		if(!level)
			make_path_direct(map_data,map,h_map,dh_max,h_min,1000000000.0f,path,mw,mh,bloc_w,bloc_h,u_idx);
		else
			make_path_direct(map_data,map,h_map,1000000000.0f,h_min,dh_max,path,mw,mh,bloc_w,bloc_h,u_idx);
		compute_coord(path,map_w,map_h,bloc_w,bloc_h);
		}
	return path;
}

void make_path_direct(SECTOR **map_data,byte **map,float **h_map,float dh_max,float h_min,float h_max,PATH_NODE *path,int mw,int mh,int bw,int bh,int u_idx)			// Elimine les étapes inutiles (qui rendent un chemin indirect)
{
	if(path==NULL)	return;
	PATH_NODE *cur=path;
	PATH_NODE *old=path;
	while(cur->next!=NULL && cur->next->next!=NULL) {		// Tant qu'il y a des points à tester
		if(is_direct(map_data,map,h_map,dh_max,h_min,h_max,*cur,*(cur->next->next),mw,mh,bw,bh,u_idx)) {		// Si le chemin est direct, on élimine le point du milieu
			PATH_NODE *tmp=cur->next->next;
			free(cur->next);
			cur->next=tmp;
			if(old!=NULL) {
				cur=old;
				old=NULL;
				}
			else
				old=cur=path;			// On recommence jusqu'à ce que le chemin soit "propre"
			}
		else {
			old=cur;
			cur=cur->next;		// Si le chemin n'est pas direct, on avance
			}
		}
}

inline bool check_rect(SECTOR **map_data,int x1,int y1,int w,int h,short c,int bloc_w,int bloc_h)
{
	for(int y=y1;y<y1+h;y++)
		if(y>=0 && y<bloc_h)
			for(int x=x1;x<x1+w;x++)
				if(x>=0 && x<bloc_w)
					if(map_data[y][x].unit_idx!=c && map_data[y][x].unit_idx!=-1)
						return false;
	return true;
}

bool is_direct(SECTOR **map_data,byte **map,float **h_map,float dh_max,float h_min,float h_max,PATH_NODE A,PATH_NODE B,int mw,int mh,int bw,int bh,int u_idx)
{
	int dx=B.x-A.x;
	int dy=B.y-A.y;
	if(abs(dx)>abs(dy)) {			// Vérifie si le chemin en ligne convient
		if(A.x>B.x) {
			PATH_NODE C=A;
			A=B;
			B=C;
			}
		for(int x=A.x;x<B.x;x++) {
			int y=A.y+(B.y-A.y)*(x-A.x)/(B.x-A.x);
			if(map_data[y][x].dh>dh_max || h_map[y][x]<h_min || h_map[y][x]>h_max || check_rect(map_data,x-(mw>>1),y-(mh>>1),mw,mh,u_idx,bw,bh))		// Si il y a un obstacle
				return false;
			}
		}
	else {
		if(A.y>B.y) {
			PATH_NODE C=A;
			A=B;
			B=C;
			}
		for(int y=A.y;y<B.y;y++) {
			int x=A.x+(B.x-A.x)*(y-A.y)/(B.y-A.y);
			if(map_data[y][x].dh>dh_max || h_map[y][x]<h_min || h_map[y][x]>h_max || check_rect(map_data,x-(mw>>1),y-(mh>>1),mw,mh,u_idx,bw,bh))		// Si il y a un obstacle
				return false;
			}
		}
	return true;
}

void destroy_path(PATH_NODE *path)		// Détruit un chemin
{
	if(path) {
		destroy_path(path->next);
		free(path);
		}
}

PATH_NODE *next_node(PATH_NODE *path)		// Passe au noeud suivant
{
	if(path) {
		PATH_NODE *tmp=path;
		path=path->next;
		free(tmp);
		}
	return path;
}

void compute_coord(PATH_NODE *path,int map_w,int map_h,int bloc_w,int bloc_h)
{
	PATH_NODE *tmp=path;
	while(tmp) {
		tmp->Pos.x=((float)(tmp->x*map_w))/bloc_w-map_w*0.5f;
		tmp->Pos.z=((float)(tmp->y*map_h))/bloc_h-map_h*0.5f;
		tmp->Pos.y=0.0f;
		tmp=tmp->next;
		}
}

PATH_NODE *find_path(SECTOR **map_data,byte **map,float **h_map,int map_w,int map_h,float dh_max,float h_min,PATH_NODE Start,PATH_NODE End,int *h,int mw,int mh,int u_idx,int l)			// Trouve le meilleur chemin pour aller de Start à End
{
	if(l>=PATH_MAX_LENGTH) {
		*h=0;
		return NULL;
		}
	if(Start.x==End.x && Start.y==End.y)			// On a trouvé ce qu'on cherchait
		return NULL;

	int px[]={-1,0,1,-1,1,-1,0,1};
	int py[]={-1,-1,-1,0,0,1,1,1};
	bool r[8];

	PATH_NODE node[8];
	int n=0,i;
	for(i=0;i<8;i++) {
		node[n].x=Start.x+px[i];
		node[n].y=Start.y+py[i];
		if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h) {
			r[i]=false;
			continue;		// On ne peut pas sortir du terrain
			}
		r[i]=(check_rect(map_data,node[n].x-(mw>>1),node[n].y-(mh>>1),mw,mh,u_idx,map_w,map_h) && map_data[node[n].y][node[n].x].dh<=dh_max && h_map[node[n].y][node[n].x]>=h_min);		// Si on peut passer, on l'ajoute à la liste
		}
	if(!r[1]) r[0]=r[2]=false;
	if(!r[3]) r[0]=r[5]=false;
	if(!r[4]) r[2]=r[7]=false;
	if(!r[6]) r[5]=r[7]=false;
	for(i=0;i<8;i++) {
		node[n].x=Start.x+px[i];
		node[n].y=Start.y+py[i];
		if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
		if(r[i])		// Si on peut passer, on l'ajoute à la liste
			n++;
		}
	again:
	if(n==0) {		// On a rien trouvé
		for(i=0;i<8;i++) {
			node[n].x=Start.x+px[i];
			node[n].y=Start.y+py[i];
			if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
			map[node[n].y][node[n].x]++;
			}
		*h=0;
		return NULL;
		}
	int m=-1;
	int mdist=999999999;
	int pass=1000000000;
	for(i=0;i<n;i++) {
		int dist=(End.x-node[i].x)*(End.x-node[i].x)+(End.y-node[i].y)*(End.y-node[i].y);
		if((dist<=mdist && pass==map[node[i].y][node[i].x]) || pass>map[node[i].y][node[i].x] || i==0) {
			pass=map[node[i].y][node[i].x];
			m=i;
			mdist=dist;
			}
		}
	if(m==-1 || map[node[m].y][node[m].x]>=1) {
		for(i=0;i<8;i++) {
			node[n].x=Start.x+px[i];
			node[n].y=Start.y+py[i];
			if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
			map[node[n].y][node[n].x]++;
			}
		*h=0;
		return NULL;
		}
	map[node[m].y][node[m].x]++;
	PATH_NODE *next=(PATH_NODE*) malloc(sizeof(PATH_NODE));
	*next=node[m];
	next->next=find_path(map_data,map,h_map,map_w,map_h,dh_max,h_min,node[m],End,h,mw,mh,u_idx,l+1);		// Cherche récursivement

	if(next->next==NULL && (next->x!=End.x || next->y!=End.y)) {		// En cas d'échec
		free(next);
		next=NULL;
		n=m;
		if(*h>=10) {
			*h=0;
			goto again;
			}
		else {
			for(i=0;i<8;i++) {
				node[n].x=Start.x+px[i];
				node[n].y=Start.y+py[i];
				if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
				map[node[n].y][node[n].x]++;
				}
			(*h)++;
			return NULL;
			}
		}

	return next;
}

PATH_NODE *find_path_level(SECTOR **map_data,byte **map,float **h_map,int map_w,int map_h,float h_max,PATH_NODE Start,PATH_NODE End,int *h,int mw,int mh,int u_idx,int l)			// Trouve le meilleur chemin pour aller de Start à End
{
	if(l>=PATH_MAX_LENGTH) {
		*h=0;
		return NULL;
		}
	if(Start.x==End.x && Start.y==End.y)			// On a trouvé ce qu'on cherchait
		return NULL;

	int px[]={-1,0,1,-1,1,-1,0,1};
	int py[]={-1,-1,-1,0,0,1,1,1};
	bool r[8];

	PATH_NODE node[8];
	int n=0,i;
	for(i=0;i<8;i++) {
		node[n].x=Start.x+px[i];
		node[n].y=Start.y+py[i];
		if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h) {
			r[i]=false;
			continue;		// On ne peut pas sortir du terrain
			}
		r[i]=(check_rect(map_data,node[n].x-(mw>>1),node[n].y-(mh>>1),mw,mh,u_idx,map_w,map_h) && h_map[node[n].y][node[n].x]<=h_max);		// Si on peut passer, on l'ajoute à la liste
		}
	if(!r[1]) r[0]=r[2]=false;
	if(!r[3]) r[0]=r[5]=false;
	if(!r[4]) r[2]=r[7]=false;
	if(!r[6]) r[5]=r[7]=false;
	for(i=0;i<8;i++) {
		node[n].x=Start.x+px[i];
		node[n].y=Start.y+py[i];
		if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
		if(r[i])		// Si on peut passer, on l'ajoute à la liste
			n++;
		}
	again:
	if(n==0) {		// On a rien trouvé
		for(i=0;i<8;i++) {
			node[n].x=Start.x+px[i];
			node[n].y=Start.y+py[i];
			if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
			map[node[n].y][node[n].x]++;
			}
		*h=0;
		return NULL;
		}
	int m=-1;
	int mdist=999999999;
	int pass=1000000000;
	for(i=0;i<n;i++) {
		int dist=(End.x-node[i].x)*(End.x-node[i].x)+(End.y-node[i].y)*(End.y-node[i].y);
		if((dist<=mdist && pass==map[node[i].y][node[i].x]) || pass>map[node[i].y][node[i].x] || i==0) {
			pass=map[node[i].y][node[i].x];
			m=i;
			mdist=dist;
			}
		}
	if(m==-1 || map[node[m].y][node[m].x]>=1) {
		for(i=0;i<8;i++) {
			node[n].x=Start.x+px[i];
			node[n].y=Start.y+py[i];
			if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
			map[node[n].y][node[n].x]++;
			}
		*h=0;
		return NULL;
		}
	map[node[m].y][node[m].x]++;
	PATH_NODE *next=(PATH_NODE*) malloc(sizeof(PATH_NODE));
	*next=node[m];
	next->next=find_path_level(map_data,map,h_map,map_w,map_h,h_max,node[m],End,h,mw,mh,u_idx,l+1);		// Cherche récursivement

	if(next->next==NULL && (next->x!=End.x || next->y!=End.y)) {		// En cas d'échec
		free(next);
		next=NULL;
		n=m;
		if(*h>=10) {
			*h=0;
			goto again;
			}
		else {
			for(i=0;i<8;i++) {
				node[n].x=Start.x+px[i];
				node[n].y=Start.y+py[i];
				if(node[n].x<0 || node[n].y<0 || node[n].x>=map_w || node[n].y>=map_h)	continue;		// On ne peut pas sortir du terrain
				map[node[n].y][node[n].x]++;
				}
			(*h)++;
			return NULL;
			}
		}

	return next;
}

float path_length(PATH_NODE *path)
{
	if(path!=NULL) {
		if(path->next==NULL)
			return 1.0f;
		return (sqrt((path->x-path->next->x)*(path->x-path->next->x)+(path->y-path->next->y)*(path->y-path->next->y))+path_length(path->next));
		}
	return 0.0f;
}

void simplify_path(PATH_NODE *path)
{
	if(path && path->next && path->next->next) {
		if((path->next->next->x - path->x)*(path->next->y - path->y) == (path->next->next->y - path->y)*(path->next->x - path->x)) {
			PATH_NODE *tmp=path->next;
			path->next=path->next->next;
			free(tmp);
			simplify_path(path);
			}
		else
			simplify_path(path->next);
		}
}

void short_path(PATH_NODE *path)
{
	if(path) {
		PATH_NODE *tmp=path->next;
		PATH_NODE *old=path;
		while(tmp!=NULL) {
			if(old!=path && (abs(path->x-tmp->x)<=1 && abs(path->y-tmp->y)<=1)) {
				old->next=NULL;
				destroy_path(path->next);
				path->next=tmp;
				old=tmp;
				}
			else
				old=tmp;
			tmp=tmp->next;
			}
		short_path(path->next);
		}
}

/*#define precision	MSEC_TO_TIMER(1)					// Fonctions pour tester le pathfinding

volatile int Atimer;

void Timer()            // procédure Timer
{
	Atimer+=precision;
}

END_OF_FUNCTION(Timer);

void draw_path(PATH_NODE *path)
{
	if(path) {
		putpixel(screen,path->x,path->y,0x0000FF);
		if(path->next) {
			line(screen,path->x,path->y,path->next->x,path->next->y,0x0000FF);
			}
		draw_path(path->next);
		}
}

int main()
{
	allegro_init();

	install_mouse();
	install_keyboard();
	install_timer();			// Initialise allegro

	install_int_ex(Timer,precision);

	set_color_depth(32);
	set_gfx_mode(GFX_AUTODETECT_WINDOWED,640,480,0,0);

	clear(screen);

	int i;
	SECTOR **map_data=(SECTOR**) malloc(sizeof(SECTOR*)*480);
	map_data[0]=(SECTOR*) malloc(sizeof(SECTOR)*640*480);
	for(i=1;i<480;i++)
		map_data[i]=&(map_data[0][i*640]);
	byte **map=(byte**) malloc(sizeof(byte*)*480);
	map[0]=(byte*) malloc(sizeof(byte)*640*480);
	for(i=1;i<480;i++)
		map[i]=&(map[0][i*640]);

	BITMAP *buf=create_bitmap(640,480);

	for(int y=0;y<480;y++)
		for(int x=0;x<640;x++) {
			if((x&3)==0 && (y&3)==0) {
				map_data[y][x].dh=(rand()%101)*0.01f;
				map_data[y][x].dh*=(rand()%101)*0.01f;
				map_data[y][x].dh*=(rand()%101)*0.01f;
				map_data[y][x].dh*=(rand()%101)*0.01f;
				map_data[y][x].dh*=(rand()%101)*0.01f;
				}
			else {
				map_data[y][x].dh=map_data[y&0xFFFC][x&0xFFFC].dh;
				}
			if((x==320 && y>200 && y<300) || (y==240 && x>200 && x<440))
				map_data[y][x].dh=1.0f;
			putpixel(buf,x,y,makecol(map_data[y][x].dh*255,map_data[y][x].dh*255,map_data[y][x].dh*255));
			}

	int timer=Atimer;

	PATH_NODE Start,End;
	Start.x=0;
	Start.y=0;
	End.x=419;
	End.y=251;

	PATH_NODE *path;
	int n=0;
	do {
		blit(buf,screen,0,0,0,0,640,480);

		int h=0;

		if(mouse_b==1) {
			Start.x=mouse_x;
			Start.y=mouse_y;
			}
		else if(mouse_b==2) {
			End.x=mouse_x;
			End.y=mouse_y;
			}

		memset(map[0],0,640*480);
		path=find_path(map_data,map,640,480,0.19f,Start,End,&h);		// Cherche un chemin

		short_path(path);
		simplify_path(path);
		n++;

		draw_path(path);

		destroy_path(path);

	} while(!key[KEY_ESC]);

	printf("%f sec.\n",((float)(Atimer-timer))/SECS_TO_TIMER(1)/n);
	printf("path_length=%f\n",path_length(path));

	readkey();

	free(map[0]);
	free(map);
	free(map_data[0]);
	free(map_data);

	allegro_exit();

	return 0;
}
END_OF_MAIN();
*/
