/*
                      Math and Geometry Utility Functions

	Functions:

	double RADTODEG(double radians)
	double DEGTORAD(double degrees)
	double SANITIZERADIANS(double radians)

	double MuPolarRotX(double input_angle, double input_radius)
	double MuPolarRotY(double input_angle, double input_radius)

	void MuVectorAdd(
	        double v1_dir, double v1_mag,
        	double v2_dir, double v2_mag,
        	double *v_dir_rtn, double *v_mag_rtn
	)
 	double MuCoordinateDeltaVector(double dx, double dy)
 	double Mu3DDistance(double delta_x, double delta_y, double delta_z)

	int Mu3DInSector(long object_num, long sect_x, long sect_y, long sect_z)
	int Mu3DInSameSector(long object_num1, long object_num2)
        int Mu3DInSameSectorPtr(
                xsw_object_struct *obj1_ptr,
                xsw_object_struct *obj2_ptr
	)
 	int Mu3DInContact(long object_num1, long object_num2)
	int Mu3DInContactPtr(
		xsw_object_struct *obj1_ptr,
		xsw_object_struct *obj2_ptr
	)
        int Mu3DInVectorContact(
		long object_num1, long object_num2,
 		double heading, double heading_variance, double range
	)
 	int Mu3DInRange(long object_num1, long object_num2, double distance)
        int Mu3DInRangePtr(
		xsw_object_struct *obj1_ptr,
		xsw_object_struct *obj2_ptr,
		double distance
	)

 	double MuConvertSLUVelocityToWarp(double slu_velocity)
	double MuMaxRangeByVelocity(double velocity_max, long time)

	---

 	Math convience functions, all trigeometric functions use
 	the clock quadrant as referance and *not* the standard position
 	quadrant.    All angles must be in radians!   All distances
 	are in XSW 'real' units unless indicated otherwise.
 
 	MuPolarRotX() returns the x value for a point in polar
 	coordinates.   input_angle must be in radians.
 
 	MuPolarRotY() is the same as MuPolarRotX() except it returns
        the y value.
 
 	Mu3DDistance() returns the distance of two point's delta
        distances.
 
 	Mu3DInContact() returns 1 if XSW Objects object_num1 and
 	object_num2 are in contact, by calculating their distances
 	apart and their sizes.
 
 */

#include <stdio.h>
#include <db.h>
#include <sys/types.h>
#include <math.h>

#include "../include/reality.h"
#include "../include/objects.h"
#include "../include/mathutil.h"


extern int DBIsObjectGarbage(long object_num);


/*
 *	Converts radians to degrees.
 */
double RADTODEG(double radians)
{
        return(radians * 180 / PI);
}

/*
 *	Converts degrees to radians.
 */        
double DEGTORAD(double degrees)
{
        return(degrees * PI / 180);
}

/*
 *	Sanitizes radian value to [0, (2 * PI)].
 */
double SANITIZERADIANS(double radians)
{
/*
This was needed incase radians was an obscenly huge number,
it shouldn't be needed anymore.

#define RANGEINC(val,min,max)	(val > max) ? max : ((val < min) ? min : val)

	radians = RANGEINC(radians, -25.132741228, 25.132741228);
 */

        while(radians >= 6.2831853)
            radians -= 6.2831853;
        
        while(radians < 0)
            radians += 6.2831853;

	return(radians);
}

/*
 *      Sanitizes degree value to [0', 360'].
 */
double SANITIZEDEGREES(double degrees)
{
        while(degrees > 360)
            degrees -= 360;

        while(degrees < 0)
            degrees += 360;
 
        return(degrees);
}


/*
 *	Returns the X axis coefficient of the given polar bearing
 *	coordinates.
 */
double MuPolarRotX(double theta, double r)
{
        return(
            r *
            cos(SANITIZERADIANS((PI / 2) - theta))
        );
}

/*
 *	Returns the Y axis coefficient of the given polar bearing
 *	coordinates.
 */
double MuPolarRotY(double theta, double r)
{
        return(
	    r *
	    sin(SANITIZERADIANS((PI / 2) - theta))
	);
}


/*
 *	Adds two vectors togeather, setting the new vector's
 *	direction and magnitude values in v_dir_rtn and v_mag_rtn
 *	(respectivly).
 */
void MuVectorAdd(
        double v1_dir, double v1_mag,
        double v2_dir, double v2_mag,
        double *v_dir_rtn, double *v_mag_rtn
)
{
        double x1, y1, x2, y2, xr, yr;


        if((v_dir_rtn == NULL) ||
           (v_mag_rtn == NULL)
        )
            return;


        /* Magitudes must be positive. */
        if(v1_mag < 0)
            v1_mag *= -1;
        if(v2_mag < 0)
            v2_mag *= -1;

        /* Calculate right angle displacements. */
        x1 = sin(v1_dir) * v1_mag;
        y1 = cos(v1_dir) * v1_mag;
 
        x2 = sin(v2_dir) * v2_mag;
        y2 = cos(v2_dir) * v2_mag;

	/* Add vectors togeather. */
	xr = x1 + x2;
	yr = y1 + y2;

        *v_mag_rtn = hypot(xr, yr);
        *v_dir_rtn = MuCoordinateDeltaVector(xr, yr);


        return;
}

/*
 *	Calculates vector of the unitless delta coordinates.
 */
double MuCoordinateDeltaVector(double dx, double dy)
{
	return(SANITIZERADIANS((PI / 2) - atan2(dy, dx)));
}


/*
 *	Calculates the 3D unitless delta distance.
 */
double Mu3DDistance(double dx, double dy, double dz)
{
        if(dx < 0)
            dx *= -1;
        if(dy < 0)
            dy *= -1;
        if(dz < 0)  
            dz *= -1;

        return(
	    hypot(hypot(dx, dy), dz)
	);
}

/*
 *	Checks if the object is in the given sector.
 */
int Mu3DInSector(long object_num, long sect_x, long sect_y, long sect_z)
{
	xsw_object_struct *obj_ptr;


        if(DBIsObjectGarbage(object_num))
            return(0);
	else
            obj_ptr = xsw_object[object_num];

	if((obj_ptr->sect_x == sect_x) &&
           (obj_ptr->sect_y == sect_y) &&
           (obj_ptr->sect_z == sect_z)
	)
	    return(1);
	else
	    return(0);
}

/*
 *	Checks if both objects are valid and in the same sector.
 */
int Mu3DInSameSector(long object_num1, long object_num2)
{
	xsw_object_struct *o1_ptr, *o2_ptr;


        /* object_num1 and object_num2 must be valid. */
        if(DBIsObjectGarbage(object_num1) ||
           DBIsObjectGarbage(object_num2)
        )
            return(0);

	o1_ptr = xsw_object[object_num1];
        o2_ptr = xsw_object[object_num2];

	return(
	    (o1_ptr->sect_x == o2_ptr->sect_x) &&
            (o1_ptr->sect_y == o2_ptr->sect_y) &&
            (o1_ptr->sect_z == o2_ptr->sect_z)
	);
}

/*
 *	Checks if the two objects are in the same sector.
 */
int Mu3DInSameSectorPtr(
	xsw_object_struct *obj1_ptr,
	xsw_object_struct *obj2_ptr
)
{
        if((obj1_ptr == NULL) ||
           (obj2_ptr == NULL)
        )
            return(0);

	if(obj1_ptr == obj2_ptr)
	    return(1);

	return(
            (obj1_ptr->sect_x == obj2_ptr->sect_x) &&
            (obj1_ptr->sect_y == obj2_ptr->sect_y) &&
            (obj1_ptr->sect_z == obj2_ptr->sect_z)
        );
}        


/*
 *	Checks if the two objects are in contact, takes size
 *	of each object into account.
 */
int Mu3DInContact(long object_num1, long object_num2)
{
        double d, s1, s2;	/* In Screen units. */
	xsw_object_struct *o1_ptr, *o2_ptr;


	/* Check if objects are valid and in the same sector. */
	if(!Mu3DInSameSector(object_num1, object_num2))
	    return(0);


        /* Get object pointers. */
        o1_ptr = xsw_object[object_num1];
        o2_ptr = xsw_object[object_num2];


        /* Both objects must have a size greater than 0. */
        s1 = ((o1_ptr->size < 0) ? 0 : o1_ptr->size);
        s2 = ((o2_ptr->size < 0) ? 0 : o2_ptr->size);


        /* Get the distance in Screen units. */
        d = Mu3DDistance(
            o2_ptr->x - o1_ptr->x,
            o2_ptr->y - o1_ptr->y,
            o2_ptr->z - o1_ptr->z
        ) * 1000;			/* Convert to Screen units. */


        /* Is distance less than the two sizes? */
        if(d <= (s1 + s2))
            return(1);
        else
            return(0);
}

/*
 *	Checks if the two objects are in contact.
 */
int Mu3DInContactPtr(
	xsw_object_struct *obj1_ptr,
	xsw_object_struct *obj2_ptr
)
{
	double d, s1, s2;	/* In Screen units. */


	if((obj1_ptr == NULL) ||
           (obj2_ptr == NULL)
	)
	    return(0);

        if((obj1_ptr->sect_x != obj2_ptr->sect_x) ||
           (obj1_ptr->sect_y != obj2_ptr->sect_y) ||
           (obj1_ptr->sect_z != obj2_ptr->sect_z)
        )
            return(0);


        /* Both objects must have a size greater than 0. */
        s1 = ((obj1_ptr->size < 0) ? 0 : obj1_ptr->size);
        s2 = ((obj2_ptr->size < 0) ? 0 : obj2_ptr->size);

        /* Get the distance in Screen units. */
        d = Mu3DDistance(
            obj2_ptr->x - obj1_ptr->x,
            obj2_ptr->y - obj1_ptr->y,
            obj2_ptr->z - obj1_ptr->z
        ) * 1000;                       /* Convert to Screen units. */


        /* Is distance less than the two sizes? */
        if(d <= (s1 + s2))
            return(1);
        else
            return(0);
}


/*
 *	Checks if object 1 is in contact with object 2 in the given
 *	heading (from object 1 to object 2) with the given heading
 *	variance and range in Screen units.
 *
 *	The size of object 1 is not taken into account, however the
 *	size of object 2 is.
 */
int Mu3DInVectorContact(
        long object_num1,
        long object_num2,
        double heading,		/* Object 1 to object 2. */
        double heading_variance,
        double range		/* In Screen units. */
)
{
	double d, s2;			/* In Screen units. */
	double hdg_res_norm, hdg_res_lower, hdg_res_upper;
	double hdgb_min, hdgb_max;	/* Heading bounds. */
        xsw_object_struct *o1_ptr, *o2_ptr;


        /* Check if objects are valid and in the same sector. */
        if(!Mu3DInSameSector(object_num1, object_num2))
            return(0);

        /* Get object pointers. */
        o1_ptr = xsw_object[object_num1];
        o2_ptr = xsw_object[object_num2];

        /* Get size of object 2 in Screen units. */
        s2 = ((o2_ptr->size < 0) ? 0 : o2_ptr->size);

        /* Range must be positive. */
	if(range < 0)
	    range *= -1;

        /* Sanitize heading. */
	heading = SANITIZERADIANS(heading);

        /* Heading variance must be positive. */
	if(heading_variance < 0)
	    heading_variance *= -1;

	/* Calculate min and max heading bounds. */
        hdgb_min = heading - heading_variance;
	hdgb_max = heading + heading_variance;


        /* Get the distance in Screen units. */
        d = Mu3DDistance(
            o2_ptr->x - o1_ptr->x,
            o2_ptr->y - o1_ptr->y,
            o2_ptr->z - o1_ptr->z
        ) * 1000;		/* Convert to screen units. */


        /* If out of range, then definatly not in contact
	 * (do not take object 1's size into account).
	 */
        if(d > (s2 + range))
            return(0);

        /* At this point, target object is within range. */

        /* Calculate bearing from object 1 to object 2. */
        hdg_res_norm = MuCoordinateDeltaVector(
            o2_ptr->x - o1_ptr->x,
            o2_ptr->y - o1_ptr->y
        );
	hdg_res_lower = hdg_res_norm - (2 * PI);
	hdg_res_upper = hdg_res_norm + (2 * PI);

	/* Check heading result order; normal, upper, lower. */
	if((hdg_res_norm >= hdgb_min) &&
           (hdg_res_norm <= hdgb_max)
	)
	    return(1);
	else if((hdg_res_upper >= hdgb_min) &&
                (hdg_res_upper <= hdgb_max)
        )
            return(1);
        else if((hdg_res_lower >= hdgb_min) &&
                (hdg_res_lower <= hdgb_max)
        )
            return(1);
        else
            return(0);
}

/*
 *	Checks if the two objects are in the same sector and in range,
 *	both objects must be valid and distance is in XSW Real units.
 */
int Mu3DInRange(
	long object_num1,
	long object_num2,
	double distance		/* In XSW Real units. */
)
{
	double distance_apart;
	double s1, s2;
	xsw_object_struct *o1_ptr, *o2_ptr;


	/* Check if objects are valid and in the same sector. */
        if(!Mu3DInSameSector(object_num1, object_num2))
            return(0);

        o1_ptr = xsw_object[object_num1];
        o2_ptr = xsw_object[object_num2];


        /* Distance must be positive and convert to Screen units. */
        distance = ((distance < 0) ?
            (distance * -1000) : (distance * 1000)
	);

	/* Get sizes in Screen units. */
	s1 = ((o1_ptr->size < 0) ? 0 : o1_ptr->size);
	s2 = ((o2_ptr->size < 0) ? 0 : o2_ptr->size);

        /* Get the distance in Screen units. */
        distance_apart = Mu3DDistance(
            o2_ptr->x - o1_ptr->x,
            o2_ptr->y - o1_ptr->y,
            o2_ptr->z - o1_ptr->z
        ) * 1000;

        /* See if distance apart is closer than distance. */
        if((distance_apart - (s1 + s2)) <= distance)
            return(1);
        else
            return(0);
}

/*
 *      Checks if the two objects are in the same sector and in range,
 *      both objects must be valid and distance is in XSW Real units.
 */
int Mu3DInRangePtr(
	xsw_object_struct *obj1_ptr,
        xsw_object_struct *obj2_ptr,
        double distance		/* In XSW Real units. */
)
{
	double distance_apart;
	double s1, s2;


	/* Check if objects are in the same sector. */
	if((obj1_ptr == NULL) ||
           (obj2_ptr == NULL)
	)
	    return(0);

	if(obj1_ptr == obj2_ptr)
	    return(1);

	if((obj1_ptr->sect_x != obj2_ptr->sect_x) ||
           (obj1_ptr->sect_y != obj2_ptr->sect_y) ||
           (obj1_ptr->sect_z != obj2_ptr->sect_z)
	)
	    return(0);

        /* Distance must be positive and convert to Screen units. */ 
        distance = ((distance < 0) ?
            (distance * -1000) : (distance * 1000)
	);

        /* Get sizes in Screen units. */
        s1 = ((obj1_ptr->size < 0) ? 0 : obj1_ptr->size);
        s2 = ((obj2_ptr->size < 0) ? 0 : obj2_ptr->size);

        /* Calculate distance apart in Screen units. */
        distance_apart = Mu3DDistance(
            obj2_ptr->x - obj1_ptr->x,
            obj2_ptr->y - obj1_ptr->y,
            obj2_ptr->z - obj1_ptr->z
        ) * 1000;

        /* See if distance apart is closer than distance. */
        if((distance_apart - (s1 + s2)) <= distance)
            return(1);
        else
            return(0);
}

/*
 *	Calculates the `theoretical' maximum range based on
 *	given maximum velocity v_max and time t.
 *
 *	If v_max is in XSW Real units, then the returned value will be
 *	in XSW Real units.  Same goes for XSW Screen units.
 *
 *	t must be in milliseconds.
 */
double MuMaxRangeByVelocity(double v_max, long t)
{
	return((v_max / (double)CYCLE_LAPSE_MS) * (double)t);
}