/*{{{  on cubic splining the cheesey way*/
/*
  spline path follower - reads in a set of control points
  with x, y, z, rx, ry, rz

  computes dx, dy, dz, drx, dry, drz per control point by cheesey
  difference - dx(n) = 0.5 * (x(n+1) - x(n-1))
  these differences are wrt t, the parametric, not time.

*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*}}}  */

/*{{{  spline types*/
typedef struct s_cntl_point {
  int knot_id;   /* checking */
  float pos[3];
  float vel[3];
  float ang[3];
  float rot[3];
  float x_pos_cubic[4];
  float y_pos_cubic[4];
  float z_pos_cubic[4];
  float x_rot_cubic[4];
  float y_rot_cubic[4];
  float z_rot_cubic[4];
  struct s_cntl_point *next;
  struct s_cntl_point *prev;
} cntl_point;
/*}}}  */

/*{{{  void solve_cubic ( float *coeffs, float v0, float v1, float d0, float d1 )*/
static void solve_cubic ( float *coeffs, float v0, float v1, float d0, float d1 )
{
  /*
      v  = at^3  + bt^2 + ct + d
      dv = 3at^2 + 2bt  + c

      d = v0
      a+b+c+d = v1

      c = d0
      3a + 2b + c = d1

      2b + 3a = d1 - d0
      a + b   = v1 - v0 - d0

      3a + 2b - 2a - 2b = d1 - d0 - 2v1 + 2v0 + 2d0
      a = d1 - d0 - 2v1 + 2v0 + 2d0
      a = d1 + d0 - 2v1 + 2v0
      b = v1 - v0 - d0 - a

  */

  coeffs[3] = v0;
  coeffs[2] = d0;
  coeffs[0] = d1 + d0 - (2.0f*v1) + (2.0f*v0);
  coeffs[1] = v1 - v0 - d0 - coeffs[0];

}
/*}}}  */
/*{{{  static void solve_rot_cubic ( float *coeffs, float v0, float v1, float d0, float d1 )*/
static void solve_rot_cubic ( float *coeffs, float v0, float v1, float d0, float d1 )
{
  /*
      v  = at^3  + bt^2 + ct + d
      dv = 3at^2 + 2bt  + c

      d = v0
      a+b+c+d = v1

      c = d0
      3a + 2b + c = d1

      2b + 3a = d1 - d0
      a + b   = v1 - v0 - d0

      3a + 2b - 2a - 2b = d1 - d0 - 2v1 + 2v0 + 2d0
      a = d1 - d0 - 2v1 + 2v0 + 2d0
      a = d1 + d0 - 2v1 + 2v0
      b = v1 - v0 - d0 - a

  */
  if (d0>360.0f) d0-=360.0f;
  if (d1>360.0f) d1-=360.0f;

  coeffs[3] = v0;
  coeffs[2] = d0;
  coeffs[0] = d1 + d0 - (2.0f*v1) + (2.0f*v0);
  coeffs[1] = v1 - v0 - d0 - coeffs[0];

}
/*}}}  */
/*{{{  void fill_knot ( cntl_point *knot )*/
static void fill_knot ( cntl_point *knot )
{
  cntl_point *next=knot->next;

  /*{{{  x*/
  solve_cubic ( knot->x_pos_cubic,
                knot->pos[0], next->pos[0],
                knot->vel[0], next->vel[0] );
  /*}}}  */
  /*{{{  y*/
  solve_cubic ( knot->y_pos_cubic,
                knot->pos[1], next->pos[1],
                knot->vel[1], next->vel[1] );
  /*}}}  */
  /*{{{  z   pos*/
  solve_cubic ( knot->z_pos_cubic,
                knot->pos[2], next->pos[2],
                knot->vel[2], next->vel[2] );
  /*}}}  */

  /*{{{  x*/
  solve_rot_cubic ( knot->x_rot_cubic,
                knot->ang[0], next->ang[0],
                knot->rot[0], next->rot[0] );
  /*}}}  */
  /*{{{  y*/
  solve_rot_cubic ( knot->y_rot_cubic,
                knot->ang[1], next->ang[1],
                knot->rot[1], next->rot[1] );
  /*}}}  */
  /*{{{  z   rot*/
  solve_rot_cubic ( knot->z_rot_cubic,
                knot->ang[2], next->ang[2],
                knot->rot[2], next->rot[2] );
  /*}}}  */

}
/*}}}  */
/*{{{  void knot_velocity ( cntl_point *prev, cntl_point *this, cntl_point *next )*/
static void knot_velocity ( cntl_point *prev, cntl_point *this, cntl_point *next )
{
  int i;

  for (i=0; i<3; i++ ) {
    float v1, v2, delta;

    this->vel[i] = (next->pos[i] - prev->pos[i]) / 2.0f;

    v1=this->ang[i];
    v2=next->ang[i];

    delta=v2-v1;

    while (delta>180)  delta-=180;
    while (delta<-180) delta+=180;

    this->rot[i] =delta / 2.0f;

    /*
    if (v1<0)      this->ang[i]+=360.0f;
    else
    */
    while (this->ang[i]>360.0f) this->ang[i]-=360.0f;
  }
}
/*}}}  */
/*{{{  float eval ( float *coeffs, float t )*/
float eval ( float *coeffs, float t )
{
  /* return a*t^3 + b*t^2 + c*t + d */
  return (coeffs[0]*t*t*t) + (coeffs[1]*t*t) + (coeffs[2]*t) + coeffs[3];
}
/*}}}  */
/*{{{  cntl_point *walk_spline ( cntl_point *v, float *t, float dt )*/
cntl_point *walk_spline ( cntl_point *v, float *t, float dt )
{
  float s=dt + (*t);
  int fix=1;

  while (fix) {
    if (s < 0.0f) {
      s+=1.0f;
      v=v->prev;
    }
    else if (s > 1.0f) {
      s-=1.0f;
      v=v->next;
    }
    else fix=0;
  }

  *t=s;
  return v;
}
/*}}}  */

cntl_point *init_splines ( char *fname )
{
  FILE *fp=fopen(fname, "rt");
  if (fp==NULL) {
    printf ("Failed to open spline file %s\n", fname );
    return NULL;
  }
  else {
    int n_pts, i;
    cntl_point *knots, *prev=NULL;
  
    /*  read in cntl points */
    fscanf ( fp, "%d\n", &n_pts );
    
    printf ("%d cntl points\n", n_pts );
    
    knots = (cntl_point *) malloc ( n_pts * sizeof (cntl_point));
    
    printf ("malloced knots\n", n_pts );
    
    for (i=0; i<n_pts; i++ ) {
      int n_read=0;

      n_read= fscanf ( fp, "%f %f %f",
            &knots[i].pos[0],
            &knots[i].pos[1],
            &knots[i].pos[2] );
      n_read+=fscanf ( fp, "%f %f %f\n",
            &knots[i].ang[0],
            &knots[i].ang[1],
            &knots[i].ang[2] );
    
      if (n_read == 6) {
        knots[i].knot_id = i;
        if (prev) {
          prev->next=&knots[i];
          knots[i].prev=prev;
        }
        prev=&knots[i];
      }
      else {
        n_pts=i;
        break;
      }
    }
    printf ("closing spline file\n" );
    fclose(fp);
    
    knots[0].prev=&knots[n_pts-1];
    knots[n_pts-1].next=&knots[0];
    
    printf ("patched up %d knots\n", n_pts );
    
    printf ("approximating velocities\n" );
    /*  approx velocities */
    for (i=0; i<n_pts; i++ )
      knot_velocity ( knots[i].prev, &knots[i], knots[i].next );
    
    /*  compute cubics */
    printf ("computing cubics\n" );
    for (i=0; i<n_pts; i++ )
      fill_knot ( &knots[i] );
    
    return &knots[0];
  }
}