/*
Copyright (C) 1996-1997 Id Software, Inc.

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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/
// sv_move.c -- monster movement

#include "quakedef.h"

// Tei f0.1 Scale step from 18
// Thunder Cats value is not 2

extern cvar_t	sv_stepsize;

int GetLastChase (edict_t *ent, vec3_t out) ;//Tei
float TraceLine (vec3_t start, vec3_t end, vec3_t impact, vec3_t normal);//Tei 
//#define	STEPSIZE	8
// Tei f0.1 Scale step

/*
=============
SV_CheckBottom

Returns false if any part of the bottom of the entity is off an edge that
is not a staircase.

=============
*/
int c_yes, c_no;
int SV_HullPointContents (hull_t *hull, int num, vec3_t p);

qboolean SV_CheckBottom (edict_t *ent)
{
	vec3_t	mins, maxs, start, stop;
	trace_t	trace;
	int		x, y;
	float	mid, bottom;
	
	VectorAdd (ent->v.origin, ent->v.mins, mins);
	VectorAdd (ent->v.origin, ent->v.maxs, maxs);

// if all of the points under the corners are solid world, don't bother
// with the tougher checks
// the corners must be within 16 of the midpoint
	start[2] = mins[2] - 1;
	for	(x=0 ; x<=1 ; x++)
		for	(y=0 ; y<=1 ; y++)
		{
			start[0] = x ? maxs[0] : mins[0];
			start[1] = y ? maxs[1] : mins[1];
			if (SV_PointContents (start) != CONTENTS_SOLID)
				goto realcheck;
		}

	c_yes++;
	return true;		// we got out easy

realcheck:
	c_no++;
//
// check it for real...
//
	start[2] = mins[2];
	
// the midpoint must be within 16 of the bottom
	start[0] = stop[0] = (mins[0] + maxs[0])*0.5;
	start[1] = stop[1] = (mins[1] + maxs[1])*0.5;
	stop[2] = start[2] - 2*sv_stepsize.value;
	trace = SV_Move (start, vec3_origin, vec3_origin, stop, true, ent);

	if (trace.fraction == 1.0)
		return false;
	mid = bottom = trace.endpos[2];
	
// the corners must be within 16 of the midpoint	
	for	(x=0 ; x<=1 ; x++)
		for	(y=0 ; y<=1 ; y++)
		{
			start[0] = stop[0] = x ? maxs[0] : mins[0];
			start[1] = stop[1] = y ? maxs[1] : mins[1];
			
			trace = SV_Move (start, vec3_origin, vec3_origin, stop, true, ent);
			
			if (trace.fraction != 1.0 && trace.endpos[2] > bottom)
				bottom = trace.endpos[2];
			if (trace.fraction == 1.0 || mid - trace.endpos[2] > sv_stepsize.value)
				return false;
		}

	c_yes++;
	return true;
}


/*
=============
SV_movestep

Called by monster program code.
The move will be adjusted for slopes and stairs, but if the move isn't
possible, no move is done, false is returned, and
pr_global_struct->trace_normal is set to the normal of the blocking wall
=============
*/
qboolean SV_movestep (edict_t *ent, vec3_t move, qboolean relink)
{
	float		dz;
	vec3_t		oldorg, neworg, end;
	trace_t		trace;
	int			i;
	edict_t		*enemy;
	vec3_t		enemyorg;

	//Tei chase
	//enemy = PROG_TO_EDICT(ent->v.enemy);

	GetLastChase(ent,enemyorg);
	/*
	if (enemy->lastChaseLoc[0])
	{
		lastc 
		VectorCopy(enemy->lastChaseLoc,enemyorg);
		//Con_Printf("Chasing..\n");
	}
	else
	{
		//Con_Printf("UnChasing..\n");

		VectorCopy(enemy->v.origin,enemyorg);
	}*/
	//Tei chase


// try the move	
	VectorCopy (ent->v.origin, oldorg);
	VectorAdd (ent->v.origin, move, neworg);

// flying monsters don't step up
	if ( (int)ent->v.flags & (FL_SWIM | FL_FLY) )
	{
	// try one move with vertical motion, then one without
		for (i=0 ; i<2 ; i++)
		{
			VectorAdd (ent->v.origin, move, neworg);
			enemy = PROG_TO_EDICT(ent->v.enemy);
			if (i == 0 && enemy != sv.edicts)
			{
				//dz = ent->v.origin[2] - PROG_TO_EDICT(ent->v.enemy)->v.origin[2];
				dz = ent->v.origin[2] - enemyorg[2];
				if (dz > 40)
					neworg[2] -= 8;
				if (dz < 30)
					neworg[2] += 8;
			}
			trace = SV_Move (ent->v.origin, ent->v.mins, ent->v.maxs, neworg, false, ent);
	
			if (trace.fraction == 1)
			{
				if ( ((int)ent->v.flags & FL_SWIM) && (SV_PointContents(trace.endpos) == CONTENTS_EMPTY))
					return false;	// swim monster left water
	
				VectorCopy (trace.endpos, ent->v.origin);
				if (relink)
					SV_LinkEdict (ent, true);
				return true;
			}
			
			if (enemy == sv.edicts)
				break;
		}
		
		return false;
	}

// push down from a step height above the wished position
	neworg[2] += sv_stepsize.value;
	VectorCopy (neworg, end);
	end[2] -= sv_stepsize.value*2;

	trace = SV_Move (neworg, ent->v.mins, ent->v.maxs, end, false, ent);

	if (trace.allsolid)
		return false;

	if (trace.startsolid)
	{
		neworg[2] -= sv_stepsize.value;
		trace = SV_Move (neworg, ent->v.mins, ent->v.maxs, end, false, ent);
		if (trace.allsolid || trace.startsolid)
			return false;
	}
	if (trace.fraction == 1)
	{
	// if monster had the ground pulled out, go ahead and fall
		if ( (int)ent->v.flags & FL_PARTIALGROUND )
		{
			VectorAdd (ent->v.origin, move, ent->v.origin);
			if (relink)
				SV_LinkEdict (ent, true);
			ent->v.flags = (int)ent->v.flags & ~FL_ONGROUND;
	//Con_Printf ("fall down\n"); //Xfx
			return true;
		}
	
//		Con_Printf ("elige no caer\n");//ZZZ
		return false;		// walked off an edge
	}

// check point traces down for dangling corners
	VectorCopy (trace.endpos, ent->v.origin);
	
	if (!SV_CheckBottom (ent))
	{
		if ( (int)ent->v.flags & FL_PARTIALGROUND )
		{	// entity had floor mostly pulled out from underneath it
			// and is trying to correct
			if (relink)
				SV_LinkEdict (ent, true);
			return true;
		}
		VectorCopy (oldorg, ent->v.origin);
		return false;
	}

	if ( (int)ent->v.flags & FL_PARTIALGROUND )
	{
//		Con_Printf ("back on ground\n"); 
		ent->v.flags = (int)ent->v.flags & ~FL_PARTIALGROUND;
	}
	ent->v.groundentity = EDICT_TO_PROG(trace.ent);

// the move is ok
	if (relink)
		SV_LinkEdict (ent, true);
	return true;
}


//============================================================================

/*
======================
SV_StepDirection

Turns to the movement direction, and walks the current distance if
facing it.

======================
*/
void PF_changeyaw (void);
qboolean SV_StepDirection (edict_t *ent, float yaw, float dist)
{
	vec3_t		move, oldorigin;
	float		delta;
	
	ent->v.ideal_yaw = yaw;
	PF_changeyaw();
	
	yaw = yaw*6.283186307179586 / 360;	// Tomaz Speed
	move[0] = cos(yaw)*dist;
	move[1] = sin(yaw)*dist;
	move[2] = 0;

	VectorCopy (ent->v.origin, oldorigin);
	if (SV_movestep (ent, move, false))
	{
		delta = ent->v.angles[YAW] - ent->v.ideal_yaw;
		if (delta > 45 && delta < 315)
		{		// not turned far enough, so don't take the step
			VectorCopy (oldorigin, ent->v.origin);
		}
		SV_LinkEdict (ent, true);
		return true;
	}
	SV_LinkEdict (ent, true);
		
	return false;
}

/*
======================
SV_FixCheckBottom

======================
*/
void SV_FixCheckBottom (edict_t *ent)
{
//	Con_Printf ("SV_FixCheckBottom\n");
	
	ent->v.flags = (int)ent->v.flags | FL_PARTIALGROUND;
}

extern cvar_t mod_smartroute;

/*
================
SV_NewChaseDir

================
*/
#define	DI_NODIR	-1
#define SEE(mensaje) 
		//Con_Printf("msg: '%s'\n",mensaje);

qboolean visible (edict_t *self, edict_t *other);

float vectoyaw (vec3_t value1);


#define MAXCHASE 10
int PushLastChase(edict_t *ent)
{
	edict_t * enemy;
	
	if (!ent->v.enemy)
		return false;//DEBUG

	enemy = PROG_TO_EDICT(ent->v.enemy);

	if (visible(ent,enemy))
	{
		VectorCopy(enemy->v.origin, ent->chase);
		VectorCopy(ent->v.origin, ent->chasefrom);
		VectorCopy(enemy->v.origin, enemy->chaseme);

		return true;
	}

	if(ent->chase[0])
	{
		VectorCopy(enemy->v.origin, ent->chase);
		VectorCopy(ent->v.origin, ent->chasefrom);
	}

	return false;


#if 0	
	if (visible(ent,enemy))//TODO: remove that line
	{
		ent->indexChase++;
		if (ent->indexChase>MAXCHASE-1)
			ent->indexChase = 0;
		SEE("Saving your pos!");
		VectorCopy(enemy->v.origin,ent->lastChaseLoc[ent->indexChase]);//Save last seek pos		
	}
	else
	{
		Con_Printf("This is a BUG in pushlastchase\n");
	}
	return 0;//ent->indexChase;
#endif

};


void R_BeamColor3f (vec3_t origin, vec3_t end,float red, float green, float blue); // raw line
void R_Beam (vec3_t origin, vec3_t end) ;

#define AI_VISIBLE 1
#define AI_RUTABLE 2
#define AI_SEARCH 4

int SimpleTrace(vec3_t from, vec3_t to)
{
	vec3_t dumy;
	return TraceLine(from,to,dumy,dumy)==1;
};

void vectoangles (vec3_t value1,vec3_t out);

void GetAlternate(edict_t *ent, vec3_t alt)
{
	vec3_t org,eorg, stop, normal,forward,angles;
	vec3_t alte,alts,right,up;
	int f,dir;

	edict_t *enemy;
	if(!ent->v.enemy)
		return;//DEBUG

	enemy = PROG_TO_EDICT(ent->v.enemy);
	VectorCopy(ent->v.origin,org);
	VectorCopy(enemy->v.origin,eorg);

	VectorSubtract(eorg,org,forward);

	vectoangles(forward,angles);
	AngleVectors(angles,forward, right, up);

	f = lhrandom(10,200);

	if (f&1)
		dir = -1;
	else
		dir = 1;

	VectorMA(org,f*dir,right,alts);


	if (!SimpleTrace(org,alts))
	{
		VectorCopy(eorg,alt);
		return;
	}

	VectorMA(eorg,f*dir,right,alte);

	if (!SimpleTrace(eorg,alte) || !SimpleTrace(alts,alte))
	{
		VectorCopy(eorg,alt);
		return;
	}

//	R_BeamColor3f(org,alts,255,255,255);
//	R_BeamColor3f(alts,alte,255,255,255);
//	R_BeamColor3f(alte,eorg,255,255,255);


	VectorCopy(alts,alt);	
}



int GetLastChase(edict_t *ent, vec3_t out)
{
	edict_t * enemy;
	vec3_t org, stop, normal,forward;
	float f;
	//int init = ent->indexChase, pos = init,t;
	
	if(!ent->v.enemy)
		return false;//DEBUG

	enemy = PROG_TO_EDICT(ent->v.enemy);
	VectorCopy(ent->v.origin,org);

	//Its chase the perfect location?
	if (TraceLine(ent->chase,enemy->v.origin,stop,normal)==1)
	{
	//	SEE("Player In a know location!");
		//Chase is a good location to trace player, go for it!
		VectorCopy(ent->chase, out);
		ent->aiflags |= AI_VISIBLE;

		return true;
	}

	//Chase is not good

	ent->aiflags &= ~AI_VISIBLE;


	//Its chasefrom a better location?
	if  (TraceLine(ent->chasefrom,enemy->v.origin,stop,normal)==1)
	{
		SEE("Returning to last know good location");
		//chasefrom is good!, go for it
		VectorCopy(ent->chasefrom, out);

		ent->aiflags |= AI_RUTABLE;
		return true;
	}
	
	//Chasefrom is not good, shit!

	ent->aiflags &= AI_RUTABLE;

	//Can get the chase loc, anyway?
	if (TraceLine(ent->chase,ent->v.origin,stop,normal)==1 && lhrandom(0,100)<50)
	{
		SEE("Search at the last know location");
		//Go and get chase location
		VectorCopy(ent->chase, out);
		return true;	
	}


	//Make "org+forward*200" vector
	f = lhrandom(100,800);
	AngleVectors(ent->v.angles,forward,stop,normal);

	VectorMA(ent->v.origin,f,forward,forward);

	//"org+forward*200" is good?
	
	if (SV_PointContents(forward) == CONTENTS_EMPTY)
	if (TraceLine(forward,enemy->v.origin,stop,normal)==1)
	{
		SEE("Humm... I will check forward angle");
		//Go and get chase location
		VectorCopy(forward, out);
		return true;	
	}


	//Make "forward*200" vector
	VectorSubtract(ent->chase,ent->v.origin,forward);

	f += VectorNormalize(forward);
	//VectorScale(forward,f,forward);
	//VectorAdd(forward,ent->v.origin,forward);
	VectorMA(ent->v.origin,f,forward,forward);

	
	if (SV_PointContents(forward) == CONTENTS_EMPTY)
	if (TraceLine(forward,enemy->v.origin,stop,normal)==1)
	{
		SEE("Humm... I will check forward chase");
		//Go and get chase location
		VectorCopy(forward, out);
		return true;	
	}


	//SEE("Has been seen by other army?..");
	if (TraceLine(ent->v.origin,enemy->chaseme,stop,normal)==1)
	{
		SEE("Its a know bandit, here!");
		//Go and get chase location
		VectorCopy(forward, out);
		return true;	
	}


	SEE("I can see player... patrol here");
	//Try blindly get player
	//GetAlternate(ent, out);

	VectorCopy(enemy->v.origin,out);
	return false;


};


void SV_NewChaseDir (edict_t *actor, edict_t *enemy, float dist)
{
	float		deltax,deltay;
	float			d[3];
	float		tdir, olddir, turnaround;
//	trace_t		trace;
	vec3_t		enemyorigin,dx;
//	float f;

	/*
	if (mod_smartroute.value)
	{
		R_Beam(actor->v.origin,  actor->chase);//XFX
		R_BeamColor3f(actor->chase,enemy->v.origin,0,255,0);
	}
	else
	{
		R_BeamColor3f(actor->v.origin,enemy->v.origin,255,0,0);
	}*/

	//Remenver last enemy pos

	if(mod_smartroute.value && !strcmp("player",enemy->v.classname + pr_strings))
	{
		//This code save the last know pos of enemy, and try to 
		// walk to these point first
		if (visible(actor,enemy)/* || !(actor->lastChaseLoc[0])*/)
		{
			SEE("I see you!");
			//lastc = GetLastChase(actor);
			PushLastChase(actor);
			//VectorCopy(enemy->v.origin, *lastc);
			VectorCopy(enemy->v.origin, enemyorigin);
			//SEE(pr_strings + enemy->v.classname);
		}
		else
		{
			//if( enemy->lastChaseLoc[0])
			//{
				SEE("I remenber you!");
				//VectorCopy(actor->lastChaseLoc,enemyorigin);
				GetLastChase(actor,enemyorigin);
				
				VectorSubtract(enemyorigin,actor->v.origin,dx);
				
				if(lhrandom(0,100)<5)
					actor->v.ideal_yaw = vectoyaw(dx);
				/*
				if( lhrandom(0,100)<10 )
				{
					SEE("I interpolate my path");
					VectorAdd(enemyorigin,enemy->v.origin,enemyorigin);
					enemyorigin[0] = enemyorigin[0]*0.5;
					enemyorigin[1] = enemyorigin[1]*0.5;
					enemyorigin[2] = enemyorigin[2]*0.5;
					f = TraceLine(actor->v.origin,enemyorigin, stop, normal);
					//if (f==1)
					//{
						SEE("This is a improvement");
						VectorCopy(stop,enemyorigin);
					//}
				}*/
			//}
			//else
			//	SEE("I dont remenber you...");
		}
	//	R_Beam(actor->v.origin,enemyorigin);//XFX
	//	R_Beam(enemyorigin,  enemy->v.origin);//XFX
	}
	else
		VectorCopy(enemy->v.origin,enemyorigin);


	olddir = anglemod( (int)(actor->v.ideal_yaw/45)*45 );
	turnaround = anglemod(olddir - 180);

	deltax = enemyorigin[0] - actor->v.origin[0];
	deltay = enemyorigin[1] - actor->v.origin[1];

	if (deltax>10)
		d[1]= 0;
	else if (deltax<-10)
		d[1]= 180;
	else
		d[1]= DI_NODIR;
	if (deltay<-10)
		d[2]= 270;
	else if (deltay>10)
		d[2]= 90;
	else
		d[2]= DI_NODIR;

// try direct route
	if (d[1] != DI_NODIR && d[2] != DI_NODIR)
	{				
		if( mod_smartroute.value >2 )
		{
			if (d[1] == 0)
				tdir = d[2] == 90 ? 45 - 22 : 315 +22  ;
			else
				//tdir = d[2] == 90 ? 135 : 215;//
				tdir = d[2] == 90 ? 135 + 22: 225 - 22;//
				
			if (tdir != turnaround && SV_StepDirection(actor, tdir, dist))
				return;
		}


		if (d[1] == 0)
			tdir = d[2] == 90 ? 45 : 315;
		else
			//tdir = d[2] == 90 ? 135 : 215;//
			tdir = d[2] == 90 ? 135 : 225;//
			
		if (tdir != turnaround && SV_StepDirection(actor, tdir, dist))
			return;
		/*
		if( mod_smartroute.value )
		{
			if (d[1] == 0)
				tdir = d[2] == 90 ? 45 + 22 : 315 -22  ;
			else
				//tdir = d[2] == 90 ? 135 : 215;//
				tdir = d[2] == 90 ? 135 - 22: 225 + 22;//
				
			if (tdir != turnaround && SV_StepDirection(actor, tdir, dist))
				return;

			if (d[1] == 0)
				tdir = d[2] == 90 ? lhrandom(22,67) : lhrandom(315-22,315+22);//medium angle
			else
				tdir = d[2] == 90 ? lhrandom(135-22,135+22) : lhrandom(225-22,225+22);//medium angle
				
			if (tdir != turnaround && SV_StepDirection(actor, tdir, dist))
				return;
		}
		*/
	}

// try other directions
	if ( ((rand()&3) & 1) ||  abs(deltay)>abs(deltax))
	{
		tdir=d[1];
		d[1]=d[2];
		d[2]=tdir;
	}

	if (d[1]!=DI_NODIR && d[1]!=turnaround 
	&& SV_StepDirection(actor, d[1], dist))
			return;

	if (d[2]!=DI_NODIR && d[2]!=turnaround
	&& SV_StepDirection(actor, d[2], dist))
			return;

/* there is no direct path to the player, so pick another direction */

	//Con_Printf ("no encuentro camino al player\n");//ZZZ

	//Tei smarther movetogoal
	/*
	if (mod_smartroute.value && actor->v.blocked)
	{
		
				pr_global_struct->self = EDICT_TO_PROG(actor);
				VectorCopy( d,pr_global_struct->trace_plane_normal );
				PR_ExecuteProgram (actor->v.blocked);
				VectorCopy( pr_global_struct->trace_plane_normal,d );
				
				if (SV_StepDirection(actor, d[2], dist))
							return;
				//return;
	}*/
	//Tei smarther movetogoal

	if (olddir!=DI_NODIR && SV_StepDirection(actor, olddir, dist))
			return;

	if (rand()&1) 	/*randomly determine direction of search*/
	{

#define ANGLETURN (45/2)
	//	Con_Printf ("aleogiro iz\n");//ZZZ
		for (tdir=0 ; tdir<=315 ; tdir += ANGLETURN)
			if (tdir!=turnaround && SV_StepDirection(actor, tdir, dist) )
					return;
	}
	else
	{
	//	Con_Printf ("aleogiro der\n");//ZZZ
		for (tdir=315 ; tdir >=0 ; tdir -= ANGLETURN)
			if (tdir!=turnaround && SV_StepDirection(actor, tdir, dist) )
					return;
	}

	if (turnaround != DI_NODIR && SV_StepDirection(actor, turnaround, dist) )
			return;

	actor->v.ideal_yaw = olddir;		// can't move

	

	//Con_Printf ("no puede mover\n");//ZZZ

// if a bridge was pulled out from underneath a monster, it may not have
// a valid standing position at all

	if (!SV_CheckBottom (actor))
		SV_FixCheckBottom (actor);

}


/*
======================
SV_CloseEnough

======================
*/
qboolean SV_CloseEnough (edict_t *ent, edict_t *goal, float dist)
{
	int		i;
	
	for (i=0 ; i<3 ; i++)
	{
		if (goal->v.absmin[i] > ent->v.absmax[i] + dist)
			return false;
		if (goal->v.absmax[i] < ent->v.absmin[i] - dist)
			return false;
	}
	return true;
}

/*
======================
SV_MoveToGoal

======================
*/


void WPAdd( vec3_t self, vec3_t target);
void WPAddSmart(  vec3_t self, vec3_t target);

void SV_MoveToGoal (void)
{
	edict_t		*ent, *goal;
	float		dist;

	ent = PROG_TO_EDICT(pr_global_struct->self);
	goal = PROG_TO_EDICT(ent->v.goalentity);
	dist = G_FLOAT(OFS_PARM0);

	if ( !( (int)ent->v.flags & (FL_ONGROUND|FL_FLY|FL_SWIM) ) )
	{
		G_FLOAT(OFS_RETURN) = 0;
		return;
	}

//	if( ent->v.goalentity && Length(goal->v.origin)>1 && Length(ent->v.origin)>1)
//		WPAddSmart( ent->v.origin,goal->v.origin);

// if the next step hits the enemy, return immediately
	if ( PROG_TO_EDICT(ent->v.enemy) != sv.edicts &&  SV_CloseEnough (ent, goal, dist) )
		return;

// bump around...
	if ( (rand()&3)==1 ||
	!SV_StepDirection (ent, ent->v.ideal_yaw, dist))
	{
		SV_NewChaseDir (ent, goal, dist);

		if (mod_smartroute.value)//Tei, erk!,... this will make then faster >:
		{
			if ( !SV_StepDirection (ent, ent->v.ideal_yaw, dist))
			{
				SV_NewChaseDir (ent, goal, dist);
			}	
		}

	}
}