/* Bezier Spline methods from https://ovpwp.wordpress.com/2008/12/17/how-to-draw-a-smooth-curve-through-a-set-of-2d-points-with-bezier-methods/ */

#include <stdio.h>
#include <stdlib.h>

#define DIMENSIONS 2 /* add as many as you need ... */
#define X 0
#define Y 1
#define Z 2
#define T 3  /* for keyframing */

#define FIRST 0  /* control points per knot */
#define SECOND 1

/*  Solves a tridiagonal system for one of coordinates of Bezier control points. */
static double *GetFirstControlPoints (double *rhs, int Length)
{
   int i, n = Length;
   double *x = malloc (sizeof (double) * n /* or Length? */);	/* Solution vector. */
   double *tmp = malloc (sizeof (double) * n /* or Length? */);	/* Temp workspace. */
   double b = 2.0;

   x[0] = rhs[0] / b;
   for (i = 1; i < n; i++) { /* Decomposition and forward substitution. */
      tmp[i] = 1 / b;
      b = (i < n - 1 ? 4.0 : 2.0) - tmp[i];
      x[i] = (rhs[i] - x[i - 1]) / b;
   }
   for (i = 1; i < n; i++)
      x[n - i - 1] -= tmp[n - i] * x[n - i];	/* Backsubstitution. */

   free(tmp);
   return x;
}

/*  Get open-ended Bezier Spline Control Points. */
static void GetCurveControlPoints (double   **knots,
                                   double ****ControlPointsP,
				   int        Length) {
#define ControlPoints (*ControlPointsP)
   int i, n = Length - 1, dim, order;
   double *rhs, *xy[DIMENSIONS];

   if (n < 1) {
      for (order = FIRST; order <= SECOND; order++) ControlPoints[order] = NULL;
      return;
   }

   /* Calculate Bezier control points */
   rhs = malloc (sizeof (double) * Length /* or n? */);     /* Right hand side vector */

   for (dim = X; dim < DIMENSIONS; dim++) {
     /* Set right hand side X or Y values */
     for (i = 1; i < n - 1; ++i)
        rhs[i] = 4 * knots[i][dim] + 2 * knots[i + 1][dim];
     rhs[0] = knots[0][dim] + 2 * knots[1][dim];
     rhs[n - 1] = 3 * knots[n - 1][dim];
     xy[dim] = GetFirstControlPoints (rhs, n /* or Length? */);
   }

   /* Fill output arrays. */
   for (order = FIRST; order <= SECOND; order++) {
     ControlPoints[order] =  malloc (sizeof (double *) * Length);
     for (i = 0; i < Length /* Must be 'Length'! */; ++i)
       ControlPoints[order][i] = malloc (sizeof (double) * DIMENSIONS);
   }

   for (i = 0; i < n; ++i)
     for (dim = X; dim < DIMENSIONS; dim++) {
        ControlPoints[FIRST][i][dim] = xy[dim][i];
	ControlPoints[SECOND][i][dim] = (i < n - 1) ? 2 * knots[i + 1][dim] - xy[dim][i + 1] : (knots[n][dim] + xy[dim][n - 1]) / 2;
     }

#undef ControlPoints
}

/* The procedure below is based on */
/* http://stackoverflow.com/questions/5443653/opengl-coordinates-from-bezier-curves */
/* - it's a 1D Bezier than can be called twice to create a 2D Bezier spline and */
/* 3 times to create a 3D Bezier spline etc.  Each axis is independent. */

double bezier(double A,  /* Start value */
              double B,  /* First control value */
              double C,  /* Second control value */
              double D,  /* Ending value */
              int t, int max_slots, /* eg 0..255/256 slots */
              double *delta)  /* Parameter 0 <= t <= 1 */
{
  double s = (max_slots-1) - t;
  double AB = (A*s + B*t)/max_slots;
  double BC = (B*s + C*t)/max_slots;
  double CD = (C*s + D*t)/max_slots;
  //  double ABC = (AB*s + CD*t)/max_slots; // This 'Bug fix' was wrong...
  double ABC = (AB*s + BC*t)/max_slots; // Original version was correct...
  double BCD = (BC*s + CD*t)/max_slots;
  if (delta) *delta = ABC-BCD;
  return (ABC*s + BCD*t)/max_slots;
}

double **letter;
double ***controlpt;

void curve(int p1, int p2) {
  double xy[DIMENSIONS];
  int step, dim;
  for (step = 0; step < 256; step++) {
    for (dim = X; dim < DIMENSIONS; dim++)
      xy[dim] = bezier(letter[p1][dim], controlpt[FIRST][p1][dim], controlpt[SECOND][p1][dim], letter[p2][dim], step, 256, NULL);
    fprintf(stderr, "     (%f,%f)\n", xy[X], xy[Y]);
  }
}

int main(int argc, char **argv) {
#define TEST_SIZE 5
  int i;
  controlpt = malloc(sizeof(double **) * 2 /* first and second control points */);
  letter = malloc(sizeof(double *) * TEST_SIZE);
  for (i = 0; i < TEST_SIZE; i++)
    letter[i] = malloc(sizeof(double) * DIMENSIONS);

  /* Trivial test */
  letter[0][X] = 10; letter[0][Y] = 0;
  letter[1][X] = 10; letter[1][Y] = 50;
  letter[2][X] = 28; letter[2][Y] = 50;
  letter[3][X] = 30; letter[3][Y] = 0;
  letter[4][X] = 10; letter[4][Y] = 0;

  GetCurveControlPoints (letter, &controlpt, TEST_SIZE-1);
  for (i = 0; i < TEST_SIZE-1; i++) {
    fprintf(stderr, "%d: (%f,%f) (%f,%f) (%f,%f)\n",
            i, letter[i][X], letter[i][Y],
            controlpt[FIRST][i][X], controlpt[FIRST][i][Y],
            controlpt[SECOND][i][X], controlpt[SECOND][i][Y]);
    if (i < TEST_SIZE-2) curve(i, i+1);
  }
  exit(0);
  return(0);
}