//////////////////////////////
/* pathfinding.c
  by Hendrik Felix Pohl aka Superku 2011
  http://www.youtube.com/user/FelixPohl
  http://www.superku.de

HOW TO USE:
- Set up bidirectional connected (!) paths in WED.
  When the entities are supposed to travel one-way
  between two nodes, f.i. a drop from a higher level,
  keep the corresponding edge directional.
- Assign a path to every entity that is supposed
  to use pathfinding.
- Write "ent_path_init();" in your main function
  before the first "level_load()" command. (The
  events "on_level_load" and "on_ent_remove" get
  changed.)
- Choose a skill for the "_ent_path" define below
  and don't use it for something else.
- If you want to move an entity to vtarget, simply
  call the following function in a loop
  ent_path_get_target(ENTITY* ent, VECTOR* vtarget, VECTOR* vresult)
  and turn and move the entity to vresult.
  The entity should have a WED path attached to it,
  otherwise it will scan for one automatically.

  Example:

  action enemy()
  {
	while(1)
	{
		VECTOR temp;
		ent_path_get_target(my,player.x,temp);
		if(vec_dist(temp,my.x) > 16)
		{
			turn to temp;
			move to temp;
		}
		wait(1);
	}
  }

- The current target can be read using the macro
  ent_path(ent)->target
  without calling "ent_path_get_target" anew.

//////////////////////////////*/
// configurable stuff

var ent_path_update_rate = 4; // only update path every "ent_path_update_rate" ticks (set the variable to 0 to update every frame)
var ent_path_node_next_dist = 48; // get next position on path if current node is closer than "ent_path_node_next_dist" quants
var ent_path_node_skip_dist = 384; // skip first node if second node is closer than "ent_path_node_skip_dist" and visible (set to 0 to disable node skipping), the same goes for the last node
var ent_path_node_skip_dist2 = 384; // skip last node (set to 0 to disable node skipping)
var ent_path_node_max_dist = 1024; // maximal node->pos distance for "ent_path_get_closest_node"
var ent_path_target_buffer = 16; // don't calculate a new path if "vec_dist(new target, old_target) < ent_path_target_buffer"
var ent_path_auto_update = 1; // automatically calculate all paths when "level_name.wmb" is newer than "level_name.pfd"
var ent_path_auto_attach = 1; // automatically scan for a WED path if no WED path has been attached to the entity
var ent_path_trace_mode = IGNORE_PASSABLE | IGNORE_ME | USE_BOX; // used by "ent_path_get_closest_node" and "ent_path_get"

#define _ent_path skill90 // choose a random skill, but don't use it for something else!

//////////////////////////////
// non-configurable stuff

#define ent_path(ent) ((ENT_PATH*)ent._ent_path)
#define ent_path_array(ent) (((ENT_PATH*)ent._ent_path)->node_array)
#define ent_path_force_update(ent) ((ENT_PATH*)ent._ent_path)->update = 0 // call this macro to force a path update

struct _ENT_PATH
{
	var start_node;
	var target_node;
	var current_node;
	var max_nodes;
	var update; // set to 0 to force a path update (or use the macro above)
	var* node_array;
	VECTOR target,target2,target3; // target is the current target on the path, target2 saves the old target parameter of ent_path_get_target and target3 is the real target
};

typedef struct _ENT_PATH ENT_PATH;

//////////////////////////////
// prototypes

// Create and initialize the ENT_PATH struct of entity "ent".
void ent_path_create(ENTITY* ent);

// Destroy the ENT_PATH struct (if any) before you remove ent.
// When you use ent_path_auto_update (= 1), this
// is automatically handled by the on_ent_remove event.
void ent_path_destroy(ENTITY* ent);

// Call this function to clear the current path.
// Can be called when the entity gets stuck to force
// the calculation of a new path.
void ent_path_clear(ENTITY* ent);

// Return the number of nodes of ent's (WED) path.
var ent_path_get_num_nodes(ENTITY* ent);

// Find the closest visible node using c_trace.
var ent_path_get_closest_node(ENTITY* ent, VECTOR* pos);

// Read the path (start_node -> target_node) from "level_name.pfd".
// Optionally check if the first node can be skipped.
void ent_path_get(ENTITY* ent, var start_node, var target_node);

// Grab the next node position (if ent has an ENT_PATH path assigned).
void ent_path_next_target(ENTITY* ent);

// Primary pathfinding function, can be called inside a loop without
// noticeable loss of performance. The vector "vresult" will contain
// the position where ent has to (turn and) move to to reach "vtarget".
// "ent" has to have a WED path assigned (otherwise it will automatically
// scan for one).
VECTOR* ent_path_get_target(ENTITY* ent, VECTOR* vtarget, VECTOR* vresult);

// Draw the current path for debugging purposes.
void ent_path_draw(ENTITY* ent);

// Calculate all possible paths on ent's WED path and write the result into "file".
// Based on an algorithm by George Pirvu/ AUM.
void ent_path_calculate(ENTITY* ent, var max_nodes, var file);

// Call "ent_path_calculate()" for all paths in the current level.
void ent_path_calculate_all();

// Recalculate paths if level has been modified.
void ent_path_update();

// Call this function in your main function before the first "level_load()".
void ent_path_init();

//////////////////////////////
// functions

// Create and initialize the ENT_PATH struct of entity "ent".
void ent_path_create(ENTITY* ent)
{
	if(ent._ent_path) return;
	
	ENT_PATH* new_path = sys_malloc(sizeof(ENT_PATH)); 	
	new_path->start_node = 0;
	new_path->target_node = 0;
	new_path->current_node = 0;
	new_path->max_nodes = 0;
	new_path->update = 0;
	new_path->node_array = NULL;
	vec_set(new_path->target,nullvector);
	vec_set(new_path->target2,vector(999999,999999,999999));
	vec_set(new_path->target3,nullvector);
	ent._ent_path = (ENT_PATH*)new_path;
}

// Destroy the ENT_PATH struct (if any) before you remove ent.
// When you use ent_path_auto_update (= 1), this
// is automatically handled by the on_ent_remove event.
void ent_path_destroy(ENTITY* ent)
{
	if(!ent._ent_path) return;
	if(ent_path_array(ent)) sys_free(ent_path_array(ent));
	sys_free(ent_path(ent));
	ent._ent_path = 0;
}

// Call this function to clear the current path.
// Can be called when the entity gets stuck to force
// the calculation of a new path.
void ent_path_clear(ENTITY* ent)
{
	if(!ent._ent_path) return;
	if(ent_path_array(ent))
	{
		sys_free(ent_path_array(ent));
		ent_path_array(ent) = NULL;
	}
}

// Return the number of nodes of ent's (WED) path.
var ent_path_get_num_nodes(ENTITY* ent)
{
	var max_nodes;
	STRING* str_tmp = str_create("");

	path_set(ent,str_tmp);
	max_nodes = path_set(ent,str_tmp);
	
	ptr_remove(str_tmp);
	return max_nodes;
}

// Find the closest visible node using c_trace.
var ent_path_get_closest_node(ENTITY* ent, VECTOR* pos)
{
	var i,j,k,l, max_nodes;
	var* path_sort, *path_dist;
	VECTOR temp;
	
	// sort nodes by distance
	max_nodes = ent_path_get_num_nodes(ent);
	if(!max_nodes) return -1;
	path_sort = (var*)sys_malloc(max_nodes*sizeof(var));
	path_dist = (var*)sys_malloc(max_nodes*sizeof(var));
	
	j = 0;
	for(i = 0; i < max_nodes; i++)
	{
		path_getnode(ent, i+1, temp, NULL);
		k = vec_dist(pos,temp);
		if(k < ent_path_node_max_dist)
		{
			path_sort[j] = i+1;
			path_dist[j] = k;
			j++;
		}
	}
	max_nodes = j;
	
	for(i = 1; i < max_nodes; i++)
	{
		k = path_dist[i];
		l = path_sort[i];
		j = i;
		while(j > 0 && path_dist[j-1] > k)
		{
			path_dist[j] = path_dist[j-1];
			path_sort[j] = path_sort[j-1];
			j--;
		}
		path_dist[j] = k;
		path_sort[j] = l;
	}
	
	// grab first visible node
	for(i = 0; i < max_nodes; i++)
	{
		path_getnode(ent, path_sort[i], temp, NULL);
		c_trace(pos,temp,ent_path_trace_mode);
		if(!trace_hit) break;
	}
	
	if(i < max_nodes) k = path_sort[i];
	else k = 0;
	
	sys_free(path_sort);
	sys_free(path_dist);
	
	return k;
}

// Read the path (start_node -> target_node) from "level_name.pfd".
// Optionally check if the first node can be skipped.
void ent_path_get(ENTITY* ent, var start_node, var target_node)
{
	var filehandle, max_nodes,i;
	var *new_path;
	STRING* str_tmp;
	VECTOR temp;
	
	str_tmp = str_create("");
	str_cpy(str_tmp,level_name);
	str_trunc(str_tmp,3);
	str_cat(str_tmp,"pfd");
	filehandle = file_open_read(str_tmp);
	if(!filehandle) return;

	str_cpy(str_tmp,"");
	path_set(my,str_tmp);
	result = file_find(filehandle,str_printf(NULL,"[%s]",_chr(str_tmp)));
	result = file_find(filehandle,str_printf(NULL,"(%d,%d)",(int)start_node,(int)target_node));
	max_nodes = file_var_read(filehandle);

	new_path = (var*)sys_malloc(max_nodes*sizeof(var));
	new_path[0] = start_node;
	for(i = 1; i < max_nodes-1; i++)
	{
		result = file_var_read(filehandle);
		new_path[i] = result;
	}
	new_path[max_nodes-1] = target_node;

	if(!ent._ent_path) ent_path_create(ent);
	if(ent_path_array(ent)) sys_free(ent_path_array(ent));
	ent_path_array(ent) = new_path;
	ent_path(ent)->start_node = start_node;
	ent_path(ent)->target_node = target_node;
	ent_path(ent)->current_node = 0;
	ent_path(ent)->max_nodes = max_nodes;
	
	// try to skip first node for a smoother (and shorter) path movement
	if(ent_path_node_skip_dist > 0)
	{
		path_getnode(ent,ent_path_array(ent)[0],ent_path(ent)->target,NULL);
		path_getnode(ent,ent_path_array(ent)[1],temp,NULL);
		if(vec_dist(ent.x,temp) < ent_path_node_skip_dist)
		{
			i = is(ent,PASSABLE);
			set(ent,PASSABLE);
			c_trace(ent.x,temp,ent_path_trace_mode);
			if(!trace_hit)
			{
				vec_set(ent_path(ent)->target,temp);
				ent_path(ent)->current_node = 1;
			}		
			if(!i) reset(ent,PASSABLE);
		}
	}
	else path_getnode(ent,ent_path_array(ent)[0],ent_path(ent)->target,NULL);

	file_close(filehandle);
	ptr_remove(str_tmp);
	sys_marker(NULL);
}

// Grab the next node position.
void ent_path_next_target(ENTITY* ent)
{
	var i;
	VECTOR temp;

	if(!ent._ent_path) return;
	ent_path(ent)->current_node++;
	
	// try to skip last node for a smoother (and shorter) path movement
	if(ent_path(ent)->current_node == ent_path(ent)->max_nodes-1 && ent_path(ent)->current_node > 0 && ent_path_node_skip_dist2 > 0)
	{
		path_getnode(ent,ent_path_array(ent)[ent_path(ent)->current_node-1],temp,NULL);
		if(vec_dist(temp,ent_path(ent)->target3) < ent_path_node_skip_dist2)
		{
			i = is(ent,PASSABLE);
			set(ent,PASSABLE);
			c_trace(temp,ent_path(ent)->target3,ent_path_trace_mode);
			if(!trace_hit) ent_path(ent)->current_node++;
			if(!i) reset(ent,PASSABLE);
		}
	}

	if(ent_path(ent)->current_node < ent_path(ent)->max_nodes) path_getnode(ent,ent_path_array(ent)[ent_path(ent)->current_node],ent_path(ent)->target,NULL);
	else ent_path_clear(ent);
}

// Primary pathfinding function, can be called inside a loop without
// noticeable loss of performance. The vector "vresult" will contain
// the position where ent has to (turn and) move to to reach "vtarget".
// "ent" has to have a WED path assigned.
VECTOR* ent_path_get_target(ENTITY* ent, VECTOR* vtarget, VECTOR* vresult)
{
	var target_node,start_node;
	
	if(!ent._ent_path) ent_path_create(ent);
	if(!ent.path_index && ent_path_auto_attach) path_scan(me,my.x,my.pan,vector(360,180,ent_path_node_max_dist));

	if(ent.path_index)
	{	
		vec_set(ent_path(ent)->target3,vtarget);
		ent_path(ent)->update = maxv(ent_path(ent)->update-time_step,0);
		if(!ent_path(ent)->update && vec_dist(vtarget,ent_path(ent)->target2) > ent_path_target_buffer)
		{
			vec_set(ent_path(ent)->target2,vtarget);
			ent_path(ent)->update = ent_path_update_rate;
			if(!ent_path_array(ent))
			{
				start_node = ent_path_get_closest_node(ent,ent.x);
				target_node = ent_path_get_closest_node(ent,vtarget);
				if(start_node == target_node || !start_node || !target_node) vec_set(ent_path(ent)->target,vtarget);
				else ent_path_get(ent,start_node,target_node);
			}
			else
			{		
				start_node = ent_path_get_closest_node(ent,ent.x);
				target_node = ent_path_get_closest_node(ent,vtarget);
				if(start_node == target_node || !start_node || !target_node)
				{
					vec_set(ent_path(ent)->target,vtarget);
					ent_path_clear(ent);
				}
				else
				{
					if(ent_path(ent)->target_node != target_node) ent_path_get(ent,start_node,target_node);
				}
			}
		}
		
		if(ent_path_array(ent))
		{
			if(vec_dist(ent.x,ent_path(ent)->target) < ent_path_node_next_dist) ent_path_next_target(ent);
			if(!ent_path_array(ent)) vec_set(ent_path(ent)->target,vtarget);
		}
	}
	else vec_set(ent_path(ent)->target,vtarget);

	vec_set(vresult,ent_path(ent)->target);
	return vresult;
}

// Draw the current path for debugging purposes.
void ent_path_draw(ENTITY* ent)
{
	var i;
	VECTOR temp;
	
	if(!ent_path_array(ent)) return;
	path_getnode(ent,ent_path_array(ent)[0],temp,NULL);
	draw_line3d(temp,NULL,100);
	draw_line3d(temp,COLOR_GREEN,100);
	for(i = 0; i < ent_path(ent)->max_nodes; i++)
	{
		path_getnode(ent,ent_path_array(ent)[i],temp,NULL);
		if(i < ent_path(ent)->current_node) draw_line3d(temp,COLOR_RED,100);
		else draw_line3d(temp,COLOR_GREEN,100);
	}
	draw_line3d(ent_path(ent)->target3,COLOR_GREEN,100);
}

// Calculate all possible paths on ent's WED path and write the result into "file".
// Based on an algorithm by George Pirvu/ AUM.
void ent_path_calculate(ENTITY* ent, var max_nodes, var file)
{
	var* node_to_node, *visited;
	var target_node = 0, start_node = 0, node = 0, next_node = 0;
	var i,j,k,l;
	var path_skills[6];
	STRING* str_tmp;

	str_tmp = str_create("");
	node_to_node = (var*)sys_malloc(max_nodes*max_nodes*sizeof(var));
	visited = (var*)sys_malloc(max_nodes*max_nodes*sizeof(var));

	for(i = 0; i < max_nodes*max_nodes; i++)
	{
		node_to_node[i] = 999999;
		visited[i] = 0;
	}

	for(i = 0; i < max_nodes; i++)
	{
		for(j = 0; j < max_nodes; j++)
		{
			result = path_nextnode(ent, i+1, j+1);
			if(result)
			{
				path_getedge(ent, i+1, j+1, path_skills);
				node_to_node[i*max_nodes+(result-1)] = integer(path_skills[0]);
			}
			else break;
		}
	}

	for(i = 0; i < max_nodes; i++)
	{
		for(j = 0; j < max_nodes; j++)
		{
			for(k = 0; k < max_nodes; k++)
			{
				if(node_to_node[j + i * max_nodes] + node_to_node[i + k * max_nodes] < node_to_node[j + k * max_nodes])
				{ 
					node_to_node[j + k * max_nodes] = node_to_node[j + i * max_nodes] + node_to_node[i + k * max_nodes]; 
					visited[j + k * max_nodes] = 1; 
				}
				if(j == k)
				{ 
					node_to_node[j + k * max_nodes] = 0; 
					visited[j + k * max_nodes] = 0; 
				} 
			}
		} 
	} 
	for(start_node = 0; start_node < max_nodes; start_node++)
	{
		for(target_node = 0; target_node < max_nodes; target_node++)
		{
			if(start_node != target_node)
			{
				str_cpy(str_tmp,"");
				next_node = start_node;
				k = 0;
				l = 1;
				while(l)
				{
					for(i = 0; i < max_nodes; i++)
					{
						if(i != next_node && !visited[i + max_nodes * next_node] && node_to_node[target_node + max_nodes * next_node] == node_to_node[target_node + max_nodes * i] + node_to_node[i + max_nodes * next_node])
						{ 
							k++;
							if(i == target_node) l = 0;
							else
							{
								str_cat(str_tmp,str_printf(NULL,"%d ",(int)(i+1)));
								next_node = i;
							}
							break;
						} 
					}
				}
				file_str_write(file,str_printf(NULL,"\n(%d,%d) %d ",(int)(start_node+1),(int)(target_node+1),(int)(k+1)));
				file_str_write(file,str_tmp);
				
			}
		}
	}

	ptr_remove(str_tmp);
	sys_free(node_to_node);
	sys_free(visited);	
}

// Calculate all possible paths in the current level.
void ent_path_calculate_all()
{
	int i;
	var num_nodes,filehandle;
	STRING* str_tmp;

	var num_paths = 2;

	str_tmp = str_create("");
	str_cpy(str_tmp,level_name);
	str_trunc(str_tmp,3);
	str_cat(str_tmp,"pfd");
	filehandle = file_open_write(str_tmp);

	me = ent_create(NULL,nullvector,NULL);

	for(i = 0; i < num_paths; i++)
	{
		num_nodes = path_next(my);
		str_cpy(str_tmp,"");
		path_set(my,str_tmp);
		file_str_write(filehandle,str_printf(NULL,"[%s] ",_chr(str_tmp)));
		ent_path_calculate(my,num_nodes,filehandle);
	}

	beep();

	ptr_remove(str_tmp);
	ptr_remove(me);
	file_close(filehandle);
}

// Recalculate paths if level has been modified.
void ent_path_update()
{
	STRING* str_tmp;

	str_tmp = str_create("");
	str_cpy(str_tmp,level_name);
	str_trunc(str_tmp,3);
	str_cat(str_tmp,"pfd");

	if(!file_exists(str_tmp)) ent_path_calculate_all();
	else
	{
		if(file_date(level_name) > file_date(str_tmp)) ent_path_calculate_all();		
	}	

	ptr_remove(str_tmp);
}

// Call this function in your main function before the first "level_load()".
void ent_path_init()
{
	if(ent_path_auto_update) on_level_load = ent_path_update;
	on_ent_remove = ent_path_destroy;
}

//////////////////////////////
// pathfinding.c
//////////////////////////////

vec_set.... // trace infront
c_trace...
if (you) my.pan += random(20);

move_friction