/* waypoint.m,
 *
 * This file calculates the points along a spline for waypoints.
 */

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "debug.inc"
#include "linklist.h"
#include "waypoint.h"


/* The math trick '1^3 = [(1-t) + t]^3' expands out to these: */
#define B0(t)	((1-t) * (1-t) * (1-t))
#define B1(t)	(3 * (1-t) * (1-t) * t)
#define B2(t)	(3 * (1-t) * t * t)
#define B3(t)	(t * t * t)

/*------------------------------------------------------------*/

/* Keep a list of allocated waypoints_t for fast access. */
//static waypoint_t *preallocated_waypoints;

/*------------------------------------------------------------*/

@implementation ControlPoint
- (id) init
{
    [super init];
    steps = 50;
    return self;
}

- (id) setControlPoint:(unsigned int)n X:(double)x_ Y:(double)y_
{
    assert(n <= 1);
    control_point[n].x = x_;
    control_point[n].y = y_;
    return self;
}

- (id) setSteps:(unsigned int)steps_ { steps = steps_; return self; }
- (id) setX:(double)x_ Y:(double)y_  { x = x_; y = y_; return self; }
- (id) setX:(double)x_ Y:(double)y_ steps:(unsigned int)steps_ { return [[self setX:x_ Y:y_] setSteps:steps_]; }

- (void) getControlPoint:(unsigned int)n X:(double *)x_ Y:(double *)y_
{
    assert(n <= 1);
    if (x_) *x_ = x + control_point[n].x;
    if (y_) *y_ = y + control_point[n].y;
}

- (unsigned int) steps { return steps; }

- (void) getX:(double *)x_ Y:(double *)y_
{
    if (x_) *x_ = x;
    if (y_) *y_ = y;
}
@end

/*------------------------------------------------------------*/

unsigned int spline_length(List *list)
{
    ListIterator *it, *nx;
    unsigned int len = 0;

    foreach_nx (it, nx, list) {
	ControlPoint *curr = [it getItem];
	nx = [it next];

	/* Ignore the last control point. */
	if (![nx getItem])
	    break;

	len += [curr steps];
    }

    return len;
}

/*------------------------------------------------------------*/

/* Finds the dots along a spline given by a List of control points and
   stores them into path.  Path must already be allocated, and have
   sufficient space. */
waypoint_t *reticulate_spline(waypoint_t waypoints[], List *cp_list)
{
    ListIterator *it, *nx;
    unsigned int i;

    assert(waypoints && cp_list);

    /* Do the articulation. */
    i = 0;
    foreach_nx (it, nx, cp_list) {
	ControlPoint *curr, *next;
	double t = 0.0, step_size;
	int steps = 0;

	curr = [it getItem];
	nx   = [it next];
	next = [nx getItem];

	if (!next)
	    break;

	steps = [curr steps];
	step_size = 1.0 / steps;

	do {
	    double x0, x1, x2, x3, y0, y1, y2, y3;

	    [curr getX:&x0 Y:&y0];
	    [curr getControlPoint:1 X:&x1 Y:&y1];
	    [next getControlPoint:0 X:&x2 Y:&y2];
	    [next getX:&x3 Y:&y3];

	    waypoints[i].x = x0*B0(t) + x1*B1(t) + x2*B2(t) + x3*B3(t);
	    waypoints[i].y = y0*B0(t) + y1*B1(t) + y2*B2(t) + y3*B3(t);

	    i++;
	    t += step_size;
	} while ((t <= 1.0) && (--steps));
    }

    return waypoints;
}

/* Articulate a spline.  Memory is automatically allocated. */
waypoint_t *articulate_spline(List *cp_list)
{
    waypoint_t *waypoints;
    unsigned int spline_len;

    assert(cp_list);
    spline_len = spline_length(cp_list);

    /* Allocate the required space. */
#ifdef DEBUG
    printf("Allocating for %d waypoint_t's.\n", list_length);
#endif

    waypoints = malloc(spline_len * sizeof(waypoint_t));
    assert(waypoints);

    return reticulate_spline(waypoints, cp_list);
}

/*------------------------------------------------------------*/

typedef struct {
    int x, y;			/* X/Y relative to first point. */
    unsigned int steps;
    int x0, y0, x1, y1;		/* Control point's x/y. */
} save_struct_t;

static long igetl(FILE *f)
{
   int b1, b2, b3, b4;
   assert(f);

   if ((b1 = fgetc(f)) != EOF)
      if ((b2 = fgetc(f)) != EOF)
	 if ((b3 = fgetc(f)) != EOF)
	    if ((b4 = fgetc(f)) != EOF)
	       return (((long)b4 << 24) | ((long)b3 << 16) |
		       ((long)b2 << 8) | (long)b1);

   return EOF;
}

static BOOL read_spline_data(save_struct_t *data, FILE *fp)
{
    data->x = igetl(fp);
    if (feof(fp))
	return NO;

    /* Assume the rest exist. */
    data->y = igetl(fp);
    data->steps = igetl(fp);
    data->x0 = igetl(fp);
    data->y0 = igetl(fp);
    data->x1 = igetl(fp);
    data->y1 = igetl(fp);
    return YES;
}

static int mem_getc(void **buffer)
{
    int ret = **((int **)buffer);
    (*buffer)++;
    return ret;
}

static long igetl_memory(void **b)
{
   int b1, b2, b3, b4;
   assert(b);

   if ((b1 = mem_getc(b)) != EOF)
      if ((b2 = mem_getc(b)) != EOF)
	 if ((b3 = mem_getc(b)) != EOF)
	    if ((b4 = mem_getc(b)) != EOF)
	       return (((long)b4 << 24) | ((long)b3 << 16) |
		       ((long)b2 << 8) | (long)b1);

   return EOF;
}

static BOOL read_spline_data_from_memory(save_struct_t *data, void **buffer, int *size)
{
    if ((unsigned)(*size) < sizeof(*data))
	return NO;
    *size -= sizeof(*data);

    data->x = igetl_memory(buffer);
    data->y = igetl_memory(buffer);
    data->steps = igetl_memory(buffer);
    data->x0 = igetl_memory(buffer);
    data->y0 = igetl_memory(buffer);
    data->x1 = igetl_memory(buffer);
    data->y1 = igetl_memory(buffer);
    return YES;
}

/* Load a spline.  Can supply list parameter, in which case the list
   will not be destroyed.  Returns the number of items in a list. */
unsigned int load_spline(const char *fn, waypoint_t *waypoints[], List **ll)
{
    FILE *fp;
    unsigned int len;
    save_struct_t data;
    List *l;
    assert(fn && waypoints);

    if (!(fp = fopen(fn, "rb"))) {
	fprintf(stderr, "[Spline] Error opening %s.\n", fn);
	return 0;
    }

    if (ll)
	l = *ll;
    else
	l = [LIST_NEW];	   /* Create a temporary list of waypoints. */
    assert(l);

    while (read_spline_data(&data, fp)) {
	ControlPoint *cp;
	cp = [[[[ControlPoint new] setX:data.x Y:data.y steps:data.steps]
		  setControlPoint:0 X:data.x0 Y:data.y0]
		 setControlPoint:1 X:data.x1 Y:data.y1];
	[l insertItemAtEnd:cp];
    }
    fclose(fp);

    len = spline_length(l);
    (*waypoints) = articulate_spline(l);

    if (ll)
	*ll = l;
    else
	l = [l free];

    return len;
}

/* Load a spline from memory.  For loading from within a zip file. */
unsigned int load_spline_from_memory(const void *buffer, int size, waypoint_t *waypoints[], List **ll)
{
    unsigned int len;
    void *bp = (void *)buffer;
    save_struct_t data;
    List *l;
    assert(buffer && waypoints);

    if (ll)
	l = *ll;
    else
	l = [LIST_NEW];	   /* Create a temporary list of waypoints. */
    assert(l);

    while (read_spline_data_from_memory(&data, &bp, &size)) {
	ControlPoint *cp;
	cp = [[[[ControlPoint new] setX:data.x Y:data.y steps:data.steps]
		  setControlPoint:0 X:data.x0 Y:data.y0]
		 setControlPoint:1 X:data.x1 Y:data.y1];
	[l insertItemAtEnd:cp];
    }
    len = spline_length(l);
    (*waypoints) = articulate_spline(l);

    if (ll)
	*ll = l;
    else
	l = [l free];

    return len;
}

/*------------------------------------------------------------*/

void free_spline(waypoint_t waypoints[])
{
    if (waypoints)
	free(waypoints);
}

#if 0
void free_spline(waypoint_t waypoints[])
{
    if (!waypoints)
	return;

#if 0
    /* Append this list to the head of preallocated_waypoints. */
    waypoints[len-1]->next = preallocated_waypoints;
    preallocated_waypoints = waypoints[0];
    free(waypoints);
#else
    free(waypoints);
    if (waypoints) {
	while (len) {
	    free(waypoints[len-1]);
	    len--;
	}

	free(waypoints);
    }
#endif
}

/*------------------------------------------------------------*/

void waypoint_init(void)
{
    //preallocated_waypoints = NULL;
}

void waypoint_shutdown(void)
{
    while (preallocated_waypoints) {
	waypoint_t *next = preallocated_waypoints->next;
	free(preallocated_waypoints);
	preallocated_waypoints = next;
    }
}
#endif
