
#include "coord.h"

static inline void	internal_rotate (GLfloat *angle, GLfloat target)
{
	float			closest;

	closest = target + fmod (*angle - target, 360.0f);

	if (closest < target - 180)
		*angle = closest + 360;
	else if (closest > target + 180)
		*angle = closest - 360;
	else
		*angle = closest;
}

static inline void	internal_interpolate_relative (GLfloat *point, GLfloat target, float factor)
{
	*point += (target - *point) * factor * gl_vertex.speed;
}

static inline void	internal_interpolate_speed (GLfloat *point, GLfloat *speed, GLfloat target, GLfloat factor, GLfloat accel)
{
	GLfloat			dist;
	GLfloat			max;

	dist = target - *point;
	max = dist * factor;

	if (COORD_ABS (dist) < COORD_EPSILON)
		*speed = 0;
	else if ((max > 0 && *speed < max) || (max < 0 && *speed > max))
		*speed += dist * accel * gl_vertex.speed;
	else
		*speed = max;

	*point += *speed * gl_vertex.speed;
}

void	coord_interpolate_pos_relative (struct s_coord *coord, const struct s_coord *target, float factor)
{
	internal_interpolate_relative (&coord->px, target->px, factor);
	internal_interpolate_relative (&coord->py, target->py, factor);
	internal_interpolate_relative (&coord->pz, target->pz, factor);
}

void	coord_interpolate_rot_relative (struct s_coord *coord, const struct s_coord *target, float factor, char modulo)
{
	if (modulo)
	{
		internal_rotate (&coord->rx, target->rx);
		internal_rotate (&coord->ry, target->ry);
		internal_rotate (&coord->rz, target->rz);
	}

	internal_interpolate_relative (&coord->rx, target->rx, factor);
	internal_interpolate_relative (&coord->ry, target->ry, factor);
	internal_interpolate_relative (&coord->rz, target->rz, factor);
}

void	coord_interpolate_pos_speed (struct s_coord *coord, struct s_coord *speed, const struct s_coord *target, float factor, float accel)
{
	internal_interpolate_speed (&coord->px, &speed->px, target->px, factor, accel);
	internal_interpolate_speed (&coord->py, &speed->py, target->py, factor, accel);
	internal_interpolate_speed (&coord->pz, &speed->pz, target->pz, factor, accel);
}

void	coord_interpolate_rot_speed (struct s_coord *coord, struct s_coord *speed, const struct s_coord *target, float factor, float accel, char modulo)
{
	if (modulo)
	{
		internal_rotate (&coord->rx, target->rx);
		internal_rotate (&coord->ry, target->ry);
		internal_rotate (&coord->rz, target->rz);
	}

	internal_interpolate_speed (&coord->rx, &speed->rx, target->rx, factor, accel);
	internal_interpolate_speed (&coord->ry, &speed->ry, target->ry, factor, accel);
	internal_interpolate_speed (&coord->rz, &speed->rz, target->rz, factor, accel);
}

void	coord_copy_pos (struct s_coord *dst, const struct s_coord *src)
{
	dst->px = src->px;
	dst->py = src->py;
	dst->pz = src->pz;
}

void	coord_copy_rot (struct s_coord *dst, const struct s_coord *src)
{
	dst->rx = src->rx;
	dst->ry = src->ry;
	dst->rz = src->rz;
}

void	coord_set_pos (struct s_coord *coord, float x, float y, float z)
{
	coord->px = x;
	coord->py = y;
	coord->pz = z;
}

void	coord_set_rot (struct s_coord *coord, float x, float y, float z)
{
	coord->rx = x;
	coord->ry = y;
	coord->rz = z;
}

void	coord_item (const struct s_coord *coord)
{
	glPushMatrix ();
	glTranslatef (coord->px, coord->py, coord->pz);
	glRotatef (coord->rx, 1, 0, 0);
	glRotatef (coord->ry, 0, 1, 0);
	glRotatef (coord->rz, 0, 0, 1);
}

void	coord_stop (void)
{
	glPopMatrix ();
}

void	coord_view (const struct s_coord *coord)
{
	glPushMatrix ();
	glTranslatef (-coord->px, -coord->py, -coord->pz);
	glRotatef (coord->rz, 0, 0, -1);
	glRotatef (coord->ry, 0, -1, 0);
	glRotatef (coord->rx, -1, 0, 0);
}
