#include "../include/zf.h"

typedef struct Dynamic Dynamic;
typedef struct StaticCollider StaticCollider;

struct Dynamic
{
    void* data;

    ZfSmartPointer* smart_pointer;
    ZfDynamicCollider* dynamic_collider;

    ZfType type;
    float mass;
    float radius;

    /* runtime */
    CLvertex position;
    CLvertex last_position;
};

struct StaticCollider
{
    void* data;

    ZfSmartPointer* smart_pointer;
    ZfQueryCollision* query_collision;

    ZfType type;
};

static ZfSmartPointer dynamic_smart_pointer;
static ZfSmartPointer static_collider_smart_pointer;

static GList* dynamic_list;
static GList* static_collider_list;

static bool
dynamic_is_valid(const Dynamic* dynamic)
{
    return dynamic->smart_pointer->is_valid(dynamic->data);
}

static void
dynamic_reference(Dynamic* dynamic)
{
    dynamic->smart_pointer->reference(dynamic->data);
}

static void
dynamic_release(Dynamic* dynamic)
{
    dynamic->smart_pointer->release(dynamic->data);

    g_free(dynamic);
}

static bool
static_collider_is_valid(const StaticCollider* static_collider)
{
    return static_collider->smart_pointer->is_valid(static_collider->data);
}

static void
static_collider_reference(StaticCollider* static_collider)
{
    static_collider->smart_pointer->reference(static_collider->data);
}

static void
static_collider_release(StaticCollider* static_collider)
{
    static_collider->smart_pointer->release(static_collider->data);

    g_free(static_collider);
}

static
void update_position(Dynamic* dynamic)
{
    /* copy (current) position to last position */
    clCopyVertex(&dynamic->last_position, &dynamic->position);
    
    /* get (current) position */
    dynamic->dynamic_collider->query_position(dynamic->data,
					      &dynamic->position);
}

/* WARNING - assumes that dynamic is valid!!! only use in functions
   that have already checked or don't need to! */
static inline  void
dynamic_query_sphere(const Dynamic* dynamic,
		     CLUsphere* sphere)
{
    clCopyVertex(&sphere->origin, &dynamic->position);
    sphere->radius = dynamic->radius;
}

/* WARNING - assumes that dynamic is valid!!! only use in functions
   that have already checked or don't need to! 

   This calculates the KINETIC ENERGY (OMG!)
*/
static inline float
dynamic_query_velocity(const Dynamic* dynamic,
		       CLnormal* velocity)
{	
    cluNormalDifference(velocity,
			&dynamic->position,
			&dynamic->last_position);
}

static inline float
dynamic_query_kinetic_energy(const Dynamic* dynamic,
			     const CLnormal* velocity)
{
    return (0.5 * dynamic->mass *
	    cluNormalDotProduct(velocity, velocity));
}

/*
  WARNING:

  This function does not handle the case in which the difference
  between the two sphere origins is small (< CL_EPSILON). In this
  case, the force and offsets should be calculate from fixed direction
  (might be okay), calculated from the velocities (maybe), or just
  some random direction (probably as good as any other solution).
*/
static
void dynamic_collision(Dynamic* dynamic0,
		       Dynamic* dynamic1)
{
    CLUsphere sphere0;
    CLUsphere sphere1;

    dynamic_query_sphere(dynamic0, &sphere0);
    dynamic_query_sphere(dynamic1, &sphere1);

    /* currently checking if spheres intersect (maybe use swept paths?) */
    if (cluSphereIntersectSphere(&sphere0, &sphere1))
    {
	CLnormal velocity0;
	CLnormal velocity1;

	CLvertex position0; /* "safe" point to move to */
	CLvertex position1;

	CLnormal Pforce0; /* Perpendicular force *due to* dynamic0 */
	CLnormal Pforce1; /* Perpendicular force *on to* dynamic0 */

	CLnormal Tforce0; /*  force *due to* dynamic0 */
	CLnormal Tforce1; /* Tangential force *on to* dynamic0 */
	
	dynamic_query_velocity(dynamic0, &velocity0);
	dynamic_query_velocity(dynamic1, &velocity1);
	
	{
	    float d;
	    float overlap;

	    CLnormal offset;

	    d = cluVertexDistance(&dynamic0->position,
				  &dynamic1->position);
	    overlap = (dynamic0->radius + dynamic1->radius) - d;
	    overlap *= 0.5f + CL_EPSILON;
	    
	    cluNormalDifference(&offset,
				&dynamic0->position,
				&dynamic1->position);
	    cluNormalNormalise(&offset);
	    cluNormalScale(&offset, overlap);
	    clCopyVertex(&position0, &dynamic0->position);
	    cluVertexAdd(&position0, &offset);

	    cluNormalNormalise(&offset);
	    cluNormalScale(&offset, -overlap);
	    clCopyVertex(&position1, &dynamic1->position);
	    cluVertexAdd(&position1, &offset);
	}

#if 0
	/* direction of Pforce is between sphere centres */
	cluNormalDifference(&Pforce0,
			    &dynamic1->position,
			    &dynamic0->position);


	cluNormalDifference(&Pforce1, /* use -Pforce0 before scaling? */
			    &dynamic0->position,
			    &dynamic1->position);

	/* magnitude of Pforce is kinetic energy (ke = 0.5 * mass *
	   speed^2) */	

	cluNormalScale(&Pforce0,
		       dynamic_query_kinetic_energy(dynamic0, &velocity0));

	cluNormalScale(&Pforce1,
		       dynamic_query_kinetic_energy(dynamic1, &velocity1));
#endif
	/* Working out the tangential force */

	{
	    GLfloat dot0, dot1;
	    CLnormal normal0, normal1;
	    CLnormal velocity_relative0, velocity_relative1;
	    
	    clCopyNormal(&velocity_relative0, &velocity0);
	    clCopyNormal(&velocity_relative1, &velocity1);

	    cluNormalSubtract(&velocity_relative0, &velocity1);
	    cluNormalSubtract(&velocity_relative1, &velocity0);
	    
	    cluNormalDifference(&normal0,
				&dynamic1->position,
				&dynamic0->position);

	    cluNormalDifference(&normal1,
				&dynamic0->position,
				&dynamic1->position);

	    cluNormalNormalise(&normal0);	    
	    cluNormalNormalise(&normal1);

	    /* generate normal component */
	    dot0 = cluNormalDotProduct(&normal1, &velocity_relative0);
	    clCopyNormal(&Pforce0, &normal1);
	    cluNormalScale(&Pforce0, dot0);
	    dot1 = cluNormalDotProduct(&normal0, &velocity_relative1);
	    clCopyNormal(&Pforce1, &normal0);
	    cluNormalScale(&Pforce1, dot1);

	    /* generate tangential component */
	    clCopyNormal(&Tforce0, &velocity0);
	    cluNormalSubtract(&Tforce0, &Pforce0);
	    clCopyNormal(&Tforce1, &velocity1);
	    cluNormalSubtract(&Tforce1, &Pforce1);
    
	    /* WARNING; HACK HACK HACK numbers!!!! must fix!! */
	    /* generate reflection vector */   
	    cluNormalScale(&Tforce0, 0.9f); /* friction*/
	    cluNormalScale(&Pforce0, - 0.5f); /* restitution */
	    cluNormalScale(&Tforce1, 0.9f); /* friction*/
	    cluNormalScale(&Pforce1, - 0.5f); /* restitution */
/*
	    if(cluNormalMagnitude(&Tforce0) > 100.0f|| cluNormalMagnitude(&Tforce1) > 100.0f)
	    {  
		printf("---------------------------\n");
		clPrintNormal(&Tforce0);
		clPrintNormal(&Tforce1);
		clPrintNormal(&velocity0);
		clPrintNormal(&velocity1);
		printf("##############################\n");
	    }*/
	}


	dynamic0->dynamic_collider->collision_response(dynamic0->data,
						       dynamic1->data,
						       dynamic1->type,
						       &position0,
						       &Pforce0,
						       &Tforce0);
	
	dynamic1->dynamic_collider->collision_response(dynamic1->data,
						       dynamic0->data,
						       dynamic0->type,
						       &position1,
						       &Pforce1,
						       &Tforce1);

	/* NICK ROCKS!!! (try removing these lines) */
	update_position(dynamic0);
	update_position(dynamic1);
    }
}

static
void static_collision(Dynamic* dynamic,
		      StaticCollider* static_collider)
{
    CLUsphere sphere;
    CLnormal velocity;
    CLnormal force;
    CLvertex collision_position;
    CLnormal collision_force_perp;
    CLnormal collision_force_tan;

    dynamic_query_sphere(dynamic, &sphere);
    dynamic_query_velocity(dynamic, &velocity);
    clCopyNormal(&force, &velocity);

    /* MAJOR HACK! - PASS VELOCITY INSTEAD OF FORCE */
    /*
    cluNormalNormalise(&force);
    cluNormalScale(&force, dynamic_query_kinetic_energy(dynamic, &velocity));
    */

    if (static_collider->query_collision(static_collider->data,
					 &sphere,
					 &force,
					 dynamic->type,
					 &collision_position,
					 &collision_force_perp,
					 &collision_force_tan))
    {
	dynamic->dynamic_collider->collision_response(dynamic->data,
						      static_collider->data,
						      static_collider->type,
						      &collision_position,
						      &collision_force_perp,
						      &collision_force_tan);
    }
}		       

void
zf_collision_system_init(void)
{
    dynamic_list = 0;
    static_collider_list = 0;

    dynamic_smart_pointer.is_valid = (ZfIsValid*) dynamic_is_valid;
    dynamic_smart_pointer.reference = (ZfReference*) dynamic_reference;
    dynamic_smart_pointer.release = (ZfRelease*) dynamic_release;

    static_collider_smart_pointer.is_valid = (ZfIsValid*) static_collider_is_valid;
    static_collider_smart_pointer.reference = (ZfReference*) static_collider_reference;
    static_collider_smart_pointer.release = (ZfRelease*) static_collider_release;
}

void
zf_collision_system_close(void)
{
    zf_list_destroy(dynamic_list,
		    &dynamic_smart_pointer);

    zf_list_destroy(static_collider_list,
		    &static_collider_smart_pointer);
}

/*
  Step collision system.
*/
void
zf_collision_system_step(void)
{
    /* hit dynamics against statics and each other */

    zf_list_foreach_valid(dynamic_list,
			  &dynamic_smart_pointer,
			  (void *) update_position);


    zf_list_foreach_valid_pair(dynamic_list,
			       &dynamic_smart_pointer,
			       (void *) dynamic_collision);


    /* statics are last so that they don't get pushed into them by
       dynamics afterwards */
    zf_list_foreach_valid_twisted_pair(dynamic_list,
				       &dynamic_smart_pointer,
				       static_collider_list,
				       &static_collider_smart_pointer,
				       (void *) static_collision);


    /* remove invalids for both lists */

    dynamic_list = zf_list_remove_invalids(dynamic_list,
					   &dynamic_smart_pointer);


    static_collider_list = zf_list_remove_invalids(static_collider_list,
						   &static_collider_smart_pointer);
}

/*
  Add a static collider to the collision system.
  
  The static collider needs to provide list management functions, a
  type (so that dynamic colliders know what they have hit) and a
  collide function (to check when collisions with dynamic colliders
  occur).
*/
void
zf_collision_system_add_static(void* data,
			       ZfSmartPointer* smart_pointer,
			       ZfQueryCollision* query_collision,
			       ZfType type)
{
    StaticCollider* static_collider;
    
    static_collider = g_new(StaticCollider, 1);

    static_collider->data = data;
    static_collider->smart_pointer = smart_pointer;
    static_collider->query_collision = query_collision;
    static_collider->type = type;

    static_collider_list = zf_list_add_data(static_collider_list,
					    static_collider,
					    &static_collider_smart_pointer);  
}

/*
  Add a dynamic collider to the collision system.

  The dynamic collider needs to provide list management functions, a
  type (so that other colliders know what they have hit), physical
  details (mass, radius, and queriable position), and collision
  response functions (to both dynamic and static colliders).
*/
void
zf_collision_system_add_dynamic(void* data,
				ZfSmartPointer* smart_pointer,
				ZfDynamicCollider* dynamic_collider,
				ZfType type,
				float mass,
				float radius)
{
    Dynamic* dynamic;

    dynamic = g_new(Dynamic, 1);

    dynamic->data = data;
    dynamic->smart_pointer = smart_pointer;
    dynamic->dynamic_collider = dynamic_collider;
    dynamic->type = type;
    dynamic->mass = mass;
    dynamic->radius = radius;

    dynamic_collider->query_position(data, &dynamic->position);
    clCopyVertex(&dynamic->last_position, &dynamic->position);

    dynamic_list = zf_list_add_data(dynamic_list,
				    dynamic,
				    &dynamic_smart_pointer);
}

/*!
  \brief Query all dynamic objects in the collision system and return all
  those whose bounding sphere intersects the given ray and whose type matches
  the given type.

  \todo Check documentation for accuracy.
*/
bool
zf_collision_system_query_dynamic_target(const CLUray* ray,
					 ZfType type,
					 void** collider_data,
					 ZfSmartPointer** smart_pointer,
					 ZfDynamicCollider** dynamic_collider)
{
    GList* li;

    for (li = dynamic_list; li; li = li->next)
    {
	Dynamic* dynamic;
	
	dynamic = (Dynamic*)li->data;
	
	if(dynamic->type & type)
	{
	    CLUsphere sphere;
	    float     t;          /* intersection time along ray, unused by function */

	    clCopyVertex(&sphere.origin, &dynamic->position);
	    sphere.radius = dynamic->radius*1.5f; /* more forgiving locking range */

	    /* WARNING: Does not take into account nearest collision yet !*/
	    if(cluRayIntersectSphere(&t, ray, &sphere))
	    {
		*collider_data = dynamic->data;
		*smart_pointer = dynamic->smart_pointer;
		*dynamic_collider = dynamic->dynamic_collider;
		
		return true;
	    }
	}
    }
    return false;
}

bool
zf_collision_system_query_static_target(const CLUray* ray,
					CLvertex** position)
{
    GList* li;
    
    for (li = static_collider_list; li; li = li->next)
    {
	StaticCollider* static_collider;
	
	static_collider = (StaticCollider*)li->data;
	
	if(static_collider->type & ZF_HEXFIELD)
	{
	    
	    if(zf_hexfluxfield_query_ray_intercept(ray, 
						   static_collider->data,
						   position))
		return true;
	}
	/*
	  else if(static_collider->type & ZF_BOSS_TARGET)
	  {
	  
	  if(zf_target_query_ray_intercept(ray, 
	  static_collider->data,
	  position))
	  return true;
	}
	*/
	else if(static_collider->type & ZF_DRONE)
	{
	    
	    if(zf_drone_query_ray_intercept(ray, 
					    static_collider->data,
					    position))
		return true;
	}

    }
    return false;
}



bool
zf_collision_system_nearest_target(const CLvertex* vertex,
				   ZfType type,
				   void** collider_data,
				   ZfSmartPointer** smart_pointer,
				   ZfDynamicCollider** dynamic_collider)
{
    GList* li;
    Dynamic* min_dynamic;
    float min_distance;
    
    min_dynamic = 0;
    min_distance = MAXFLOAT;

    for (li = dynamic_list; li; li = li->next)
    {
	Dynamic* dynamic;
	float distance;

	dynamic = (Dynamic*)li->data;

	if (dynamic->type & type)
	{
	    CLUsphere sphere;
	    
	    clCopyVertex(&sphere.origin, &dynamic->position);
	    sphere.radius = dynamic->radius;

	    distance = cluSphereDistance(&sphere, vertex);

	    if (distance < min_distance)
	    {
		min_dynamic = dynamic;
		min_distance = distance;
	    }
	}
    }

    if (min_dynamic)
    {
	*collider_data = min_dynamic->data;
	*smart_pointer = min_dynamic->smart_pointer;
	*dynamic_collider = min_dynamic->dynamic_collider;
	
	return true;
    }

    return false;
}

unsigned int
zf_collision_system_get_collider_count(void)
{
    return g_list_length(dynamic_list) + g_list_length(static_collider_list);
}
