/*                                           -*- mode: c; c-basic-offset: 2 -*-
 * See:
 *
 *     http://en.wikipedia.org/wiki/Verlet_integration
 *     http://www.teknikus.dk/tj/gdc2001.htm
 *
 * TODO:
 *
 * - Add circle object
 * - Try out this idea: make constraint solver take mean of all
 *   corrections at the end instead of meaning as it goes.
 * - Consider a size, or radius attribute for objects.
 * - Make a gravitation object - initialized with a vector or just a point.
 */

#include <glib.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>

#include "akamaru.h"

static void
link_init (Link *link)
{
  link->prev = link;
  link->next = link;
}

static void
link_insert_before (Link *link, Link *anchor)
{
  link->prev = anchor->prev;
  link->next = anchor;
  link->prev->next = link;
  link->next->prev = link;
}

static void
link_remove (Link *link)
{
  link->prev->next = link->next;
  link->next->prev = link->prev;
}

static void
list_init (List *list, size_t offset)
{
  link_init (&list->head);
  list->offset = offset;
}

static void
list_for_each (List *list, ListFunc func, void *data)
{
  Link *l, *next;

  for (l = list->head.next; l != &list->head; l = next)
    {
      next = l->next;
      func ((char *) l - list->offset, data);
    }
}

static void
list_append (List *list, void *element)
{
  Link *link;

  link = (Link *) ((char *) element + list->offset);
  link_insert_before (link, &list->head);
}

void
object_init (Object *object, double x, double y, double mass)
{
  object->position.x = x;
  object->position.y = y;
  object->previous_position.x = x;
  object->previous_position.y = y;
  object->mass = mass;
}

void
spring_init (Spring *spring, Object *a, Object *b, double length)
{
  spring->a = a;
  spring->b = b;
  spring->length = length;
}

Spring *
model_add_spring (Model *model, Object *a, Object *b, double length)
{
  Spring *spring;

  spring = g_new (Spring, 1);
  spring->a = a;
  spring->b = b;
  spring->length = length;
  list_append (&model->spring_list, spring);

  return spring;
}

void
model_for_each_spring (Model *model, SpringFunc func, void *data)
{
  list_for_each (&model->spring_list, (ListFunc) func, data);
}

void
stick_init (Stick *stick, Object *a, Object *b, double length)
{
  stick->a = a;
  stick->b = b;
  stick->length = length;
}

Stick *
model_add_stick (Model *model, Object *a, Object *b, double length)
{
  Stick *stick;

  stick = g_new (Stick, 1);
  stick->a = a;
  stick->b = b;
  stick->length = length;
  list_append (&model->stick_list, stick);

  return stick;
}

void
model_for_each_stick (Model *model, StickFunc func, void *data)
{
  list_for_each (&model->stick_list, (ListFunc) func, data);
}

void
string_init (String *string, Object *a, Object *b, double length)
{
  string->a = a;
  string->b = b;
  string->length = length;
}

String *
model_add_string (Model *model, Object *a, Object *b, double length)
{
  String *string;

  string = g_new (String, 1);
  string->a = a;
  string->b = b;
  string->length = length;
  list_append (&model->string_list, string);

  return string;
}

void
model_for_each_string (Model *model, StringFunc func, void *data)
{
  list_for_each (&model->string_list, (ListFunc) func, data);
}

OffsetSpring *
model_add_offset_spring (Model *model, Object *a, Object *b,
			 double dx, double dy)
{
  OffsetSpring *spring;

  spring = g_new (OffsetSpring, 1);
  spring->a = a;
  spring->b = b;
  spring->dx = dx;
  spring->dy = dy;
  list_append (&model->offset_spring_list, spring);

  return spring;
}

void
model_for_each_offset_spring (Model *model, OffsetSpringFunc func, void *data)
{
  list_for_each (&model->offset_spring_list, (ListFunc) func, data);
}

void
spacer_init (Spacer *spacer, Object *a, Object *b, double length)
{
  spacer->a = a;
  spacer->b = b;
  spacer->length = length;
}

Spacer *
model_add_spacer (Model *model, Object *a, Object *b, double length)
{
  Spacer *spacer;

  spacer = g_new (Spacer, 1);
  spacer->a = a;
  spacer->b = b;
  spacer->length = length;
  list_append (&model->spacer_list, spacer);

  return spacer;
}

void
model_for_each_spacer (Model *model, SpacerFunc func, void *data)
{
  list_for_each (&model->spacer_list, (ListFunc) func, data);
}

void
anchor_init (Anchor *anchor, Object *object, double x, double y)
{
  anchor->object = object;
  anchor->x = x;
  anchor->y = y;
}

Anchor *
model_add_anchor (Model *model, Object *object, double x, double y)
{
  Anchor *anchor;

  anchor = g_new (Anchor, 1);
  anchor->object = object;
  anchor->x = x;
  anchor->y = y;
  list_append (&model->anchor_list, anchor);

  return anchor;
}

void
model_delete_anchor (Model *model, Anchor *anchor)
{
  link_remove (&anchor->link);
  g_free (anchor);
}

void
model_for_each_anchor (Model *model, AnchorFunc func, void *data)
{
  list_for_each (&model->anchor_list, (ListFunc) func, data);
}

void
polygon_init_from_va_list (Polygon *p, int enclosing,
			   int num_points, va_list ap)
{
  double dx, dy, length;
  int i, j;

  /* Polygons are defined counter-clock-wise in a coordinate system
   * with the y-axis pointing down. */

  p->num_points = num_points;
  p->points = g_new (Point, num_points);
  p->enclosing = enclosing;

  for (i = 0; i < num_points; i++) {
    p->points[i].x = va_arg (ap, double);
    p->points[i].y = va_arg (ap, double);
  }

  p->normals = g_new (Vector, p->num_points);
  /* Compute outward pointing normals.  p->normals[i] is the normal
   * for the edged between p->points[i] and p->points[i + 1]. */
  for (i = 0; i < p->num_points; i++) {
    j = (i + 1) % p->num_points;
    dx = p->points[j].x - p->points[i].x;
    dy = p->points[j].y - p->points[i].y;
    length = sqrt (dx * dx + dy * dy);
    p->normals[i].x = -dy / length;
    p->normals[i].y = dx / length;
  }
}

void
polygon_init (Polygon *polygon, int enclosing, int num_points, ...)
{
  va_list ap;

  va_start (ap, num_points);
  polygon_init_from_va_list (polygon, enclosing, num_points, ap);
  va_end (ap);
}

Polygon *
model_add_polygon (Model *model, int enclosing, int num_points, ...)
{
  Polygon *polygon;
  va_list ap;

  polygon = g_new0 (Polygon, 1);
  va_start (ap, num_points);
  polygon_init_from_va_list (polygon, enclosing, num_points, ap);
  va_end (ap);
  list_append (&model->polygon_list, polygon);

  return polygon;
}

void
model_for_each_polygon (Model *model, PolygonFunc func, void *data)
{
  list_for_each (&model->polygon_list, (ListFunc) func, data);
}

Polygon *
model_add_diamond (Model *model, double x, double y)
{
  return model_add_polygon (model, FALSE, 5,
			    x, y,
			    x + 10, y + 40,
			    x + 90, y + 40,
			    x + 100, y,
			    x + 50, y - 20);
}

Polygon *
model_add_rectangle (Model *model, double x0, double y0, double x1, double y1)
{
  return model_add_polygon (model, FALSE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
}

Polygon *
model_add_enclosing_rectangle (Model *model, double x0, double y0,
			       double x1, double y1)
{
  return model_add_polygon (model, TRUE, 4, x0, y0, x0, y1, x1, y1, x1, y0);
}

Offset *
model_add_offset (Model *model, int num_objects, double dx, double dy)
{
  Offset *offset;

  offset = g_new (Offset, 1);
  offset->num_objects = num_objects;
  offset->objects = g_new (Object *, num_objects);
  offset->dx = dx;
  offset->dy = dy;
  list_append (&model->offset_list, offset);

  return offset;
}

void
model_for_each_offset (Model *model, OffsetFunc func, void *data)
{
  list_for_each (&model->offset_list, (ListFunc) func, data);
}

void
model_init (Model *model)
{
  memset (model, 0, sizeof *model);
  list_init (&model->object_list, G_STRUCT_OFFSET (Object, link));
  list_init (&model->spacer_list, G_STRUCT_OFFSET (Spacer, link));
  list_init (&model->string_list, G_STRUCT_OFFSET (String, link));
  list_init (&model->stick_list, G_STRUCT_OFFSET (Stick, link));
  list_init (&model->spring_list, G_STRUCT_OFFSET (Spring, link));
  list_init (&model->anchor_list, G_STRUCT_OFFSET (Anchor, link));
  list_init (&model->polygon_list, G_STRUCT_OFFSET (Polygon, link));
  list_init (&model->offset_list, G_STRUCT_OFFSET (Offset, link));
  list_init (&model->offset_spring_list,
	     G_STRUCT_OFFSET (OffsetSpring, link));
}

Object *
model_add_object (Model *model, double x, double y, double mass, void *data)
{
  Object *object;

  object = g_new (Object, 1);
  object->position.x = x;
  object->position.y = y;
  object->previous_position.x = x;
  object->previous_position.y = y;
  object->mass = mass;
  list_append (&model->object_list, object);
  object->data = data;

  return object;
}

void
model_for_each_object (Model *model, ObjectFunc func, void *data)
{
  list_for_each (&model->object_list, (ListFunc) func, data);
}

static void
free_element (void *element, void *data)
{
  g_free (element);
}

static void
free_polygon (Polygon *polygon, void *data)
{
  g_free (polygon->points);
  g_free (polygon->normals);
  g_free (polygon);
}

static void
free_offset (Offset *offset, void *data)
{
  g_free (offset->objects);
  g_free (offset);
}

void
model_fini (Model *model)
{
  list_for_each (&model->object_list, free_element, NULL);
  list_for_each (&model->spacer_list, free_element, NULL);
  list_for_each (&model->string_list, free_element, NULL);
  list_for_each (&model->stick_list, free_element, NULL);
  list_for_each (&model->spring_list, free_element, NULL);
  list_for_each (&model->anchor_list, free_element, NULL);
  model_for_each_polygon (model, free_polygon, NULL);
  model_for_each_offset (model, free_offset, NULL);
  list_for_each (&model->offset_spring_list, free_element, NULL);
}

static void
object_accumulate_forces (Object *object, void *data)
{
  Model *model = data;
  Vector v;

  /* Gravity */
  object->force.x = model->gravity.x * object->mass;
  object->force.y = model->gravity.y * object->mass;

  /* Friction */
  v.x = object->position.x - object->previous_position.x;
  v.y = object->position.y - object->previous_position.y;
  object->force.x -= v.x * model->friction;
  object->force.y -= v.y * model->friction;
}

static void
spring_accumulate_forces (Spring *spring, void *data)
{
  Model *model = data;
  double x, y, dx, dy, distance, displacement;
  Vector u;

  x = spring->a->position.x;
  y = spring->a->position.y;
  dx = spring->b->position.x - x;
  dy = spring->b->position.y - y;
  distance = sqrt (dx * dx + dy * dy);
  u.x = dx / distance;
  u.y = dy / distance;
  displacement = distance - spring->length;
  spring->a->force.x += u.x * model->k * displacement;
  spring->a->force.y += u.y * model->k * displacement;
  spring->b->force.x -= u.x * model->k * displacement;
  spring->b->force.y -= u.y * model->k * displacement;
}

void
offset_spring_accumulate_forces (OffsetSpring *spring, void *data)
{
  Model *model = data;
  double dx, dy;
  Point middle;

  middle.x = (spring->a->position.x + spring->b->position.x) / 2;
  middle.y = (spring->a->position.y + spring->b->position.y) / 2;

  dx = (middle.x - spring->dx / 2) - spring->a->position.x;
  dy = (middle.y - spring->dy / 2) - spring->a->position.y;

  spring->a->force.x += dx * model->k;
  spring->a->force.y += dy * model->k;
  spring->b->force.x -= dx * model->k;
  spring->b->force.y -= dy * model->k;
}

static void
model_accumulate_forces (Model *model)
{
  model_for_each_object (model, object_accumulate_forces, model);
  model_for_each_spring (model, spring_accumulate_forces, model);
  model_for_each_offset_spring (model,
				offset_spring_accumulate_forces, model);
}

static void
object_integrate (Object *object, void *data)
{
  double x, y, step;

  step = * (double *) data;
  x = object->position.x;
  y = object->position.y;

  object->position.x =
    x + (x - object->previous_position.x) + object->force.x * step * step;
  object->position.y =
    y + (y - object->previous_position.y) + object->force.y * step * step;

  object->previous_position.x = x;
  object->previous_position.y = y;
}

/* The square root in the distance computation for the string and
 * stick constraints can be aproximated using Newton:
 *
 *    distance =
 *      (model->sticks[i].length +
 *       (dx * dx + dy * dy) / model->sticks[i].length) / 2;
 *
 * This works really well, since the constraints aren't typically
 * violated much.  Thus, the distance is really close to the stick
 * length, which then makes a good initial guess.  However, the
 * approximation seems to be slower that just calling sqrt()...
 */

static inline double
estimate_distance (double dx, double dy, double r)
{
#ifdef APPROXIMATE_SQUARE_ROOTS
  return (r + (dx * dx + dy * dy) / r) / 2;
#else
  return sqrt (dx * dx + dy * dy);
#endif
}

static int
polygon_contains_point (Polygon *polygon, Point *point)
{
  int i;
  double dx, dy;

  for (i = 0; i < polygon->num_points; i++) {
    dx = point->x - polygon->points[i].x;
    dy = point->y - polygon->points[i].y;

    if (polygon->normals[i].x * dx + polygon->normals[i].y * dy >= 0)
      return polygon->enclosing;
  }

  return !polygon->enclosing;
}

static void
polygon_reflect_object (Polygon *polygon, Object *object, double elasticity)
{
  int i, edge;
  double d, distance;
  Vector *n;

  distance = -1000;
  for (i = 0; i < polygon->num_points; i++) {
    d = polygon->normals[i].x * (object->position.x - polygon->points[i].x) +
      polygon->normals[i].y * (object->position.y - polygon->points[i].y);

    if (d > distance) {
      distance = d;
      edge = i;
      n = &polygon->normals[i];
    }
  }

  object->position.x -= (1 + elasticity) * distance * n->x;
  object->position.y -= (1 + elasticity) * distance * n->y;

  distance =
    n->x * (object->previous_position.x - polygon->points[edge].x) +
    n->y * (object->previous_position.y - polygon->points[edge].y);

  object->previous_position.x -= (1 + elasticity) * distance * n->x;
  object->previous_position.y -= (1 + elasticity) * distance * n->y;
}

typedef struct _ObjectConstrainPolygonClosure {
  Model *model;
  Polygon *polygon;
} ObjectConstrainPolygonClosure;

static void
object_constrain_polygon (Object *object, void *data)
{
  ObjectConstrainPolygonClosure *closure = data;

  if (polygon_contains_point (closure->polygon, &object->position))
    polygon_reflect_object (closure->polygon, object,
			    closure->model->elasticity);
}

static void
model_constrain_polygon (Polygon *polygon, void *data)
{
  ObjectConstrainPolygonClosure closure;
  closure.polygon = polygon;
  closure.model = data;

  model_for_each_object (closure.model, object_constrain_polygon, &closure);
}

static void
model_constrain_anchor (Anchor *anchor, void *data)
{
  if (anchor->object == NULL)
    return;

  anchor->object->position.x = anchor->x;
  anchor->object->position.y = anchor->y;
}

static void
model_constrain_offset (Offset *offset, void *data)
{
  double x, y;
  int i;

  x = 0;
  y = 0;
  for (i = 0; i < offset->num_objects; i++)
    {
      x += offset->objects[i]->position.x;
      y += offset->objects[i]->position.y;
    }

  x = x / offset->num_objects - offset->dx * (offset->num_objects - 1) / 2;
  y = y / offset->num_objects - offset->dy * (offset->num_objects - 1) / 2;

  for (i = 0; i < offset->num_objects; i++)
    {
      offset->objects[i]->position.x = x + offset->dx * i;
      offset->objects[i]->position.y = y + offset->dy * i;
    }
}

static void
model_constrain_string (String *string, void *data)
{
  double x, y, dx, dy, distance, fraction;

  x  = string->a->position.x;
  y  = string->a->position.y;
  dx = string->b->position.x - x;
  dy = string->b->position.y - y;
  distance = estimate_distance (dx, dy, string->length);

  if (distance < string->length)
    return;

  fraction = (distance - string->length) / distance / 2;
  string->a->position.x = x + dx * fraction;
  string->a->position.y = y + dy * fraction;
  string->b->position.x = x + dx * (1 - fraction);
  string->b->position.y = y + dy * (1 - fraction);
}


static void
model_constrain_spacer (Spacer *spacer, void *data)
{
  double x, y, dx, dy, distance, fraction;

  x = spacer->a->position.x;
  y = spacer->a->position.y;
  dx = spacer->b->position.x - x;
  dy = spacer->b->position.y - y;

  distance = estimate_distance (dx, dy, spacer->length);
  if (distance > spacer->length)
    return;

  fraction = (distance - spacer->length) / distance / 2;
  spacer->a->position.x = x + dx * fraction;
  spacer->a->position.y = y + dy * fraction;
  spacer->b->position.x = x + dx * (1 - fraction);
  spacer->b->position.y = y + dy * (1 - fraction);
}

static void
model_constrain_stick (Stick *stick, void *data)
{
  double x, y, dx, dy, distance, fraction;

  x  = stick->a->position.x;
  y  = stick->a->position.y;
  dx = stick->b->position.x - x;
  dy = stick->b->position.y - y;

  distance = estimate_distance (dx, dy, stick->length);

  fraction = (distance - stick->length) / distance / 2;
  stick->a->position.x = x + dx * fraction;
  stick->a->position.y = y + dy * fraction;
  stick->b->position.x = x + dx * (1 - fraction);
  stick->b->position.y = y + dy * (1 - fraction);
}

static void
model_constrain (Model *model)
{
  model_for_each_anchor (model, model_constrain_anchor, model);
  model_for_each_string (model, model_constrain_string, model);
  model_for_each_spacer (model, model_constrain_spacer, model);
  model_for_each_stick (model, model_constrain_stick, model);
  model_for_each_offset (model, model_constrain_offset, model);
  model_for_each_polygon (model, model_constrain_polygon, model);
}

void
model_step (Model *model, double delta_t)
{
  int i;

  model_accumulate_forces (model);
  model_for_each_object (model, object_integrate, &delta_t);
  for (i = 0; i < model->constrain_iterations; i++)
    model_constrain (model);

  model->theta += delta_t;
}

static double
object_distance (Object *object, double x, double y)
{
  double dx, dy;

  dx = object->position.x - x;
  dy = object->position.y - y;

  return sqrt (dx*dx + dy*dy);
}

typedef struct {
  double x;
  double y;
  double distance;
  Object *object;
} FindNearestClosure;

static void
model_update_closest (Object *object, void *data)
{
  FindNearestClosure *closure = data;
  double distance;

  distance = object_distance (object, closure->x, closure->y);
  if (closure->object == NULL || distance < closure->distance) {
    closure->distance = distance;
    closure->object = object;
  }
}

Object *
model_find_nearest (Model *model, double x, double y)
{
  FindNearestClosure closure;

  closure.x = x;
  closure.y = y;
  closure.object = NULL;
  model_for_each_object (model, model_update_closest, &closure);

  return closure.object;
}