/* The routines in this file are copyright (c) 1987 by Helene (Lee) Taran.
 * Permission is granted for use and free distribution as long as the
 * original author's name is included with the code.
 */

#include "spline.h"

#define ALTSIGN(n,x) ((n) % 2 ? (x) : -(x))
#define VECTOR(n) ((REAL_POINT *)calloc((n),sizeof(REAL_POINT)))
#define MATRIX(n) ((double *)calloc((n)*(n),sizeof(double)))

double G[MAXG];       /* vector of g values */

void *calloc();

/* Init_GValues should be called once before drawing any curves.  Computes
 * the first MAXG number of Gn values using iteration to save time and 
 * stack space.
 */
Init_GValues()
{
  int n;
  G[0] = 0.0;
  G[1] = 1.0;
  for (n = 2; n < MAXG; n++) 
    G[n] = (4 * G[n-1]  - G[n-2]);
}

Print_GValues() /* only for debugging */
{
  int i;
  for (i = 0; i < MAXG; i++)
    printf("G[%d] = %lf\n",i,G[i]);
}


/* Init_OpenSpline_Matrices : allocates an initializes the L and D matrices
 * that are used to create open splines.
 */
Init_OpenSpline_Matrices(n,L,D)
int n;       /* size of each of the square matrices */
double *L;    /* lower triangular matrix used in LDL^t decomposition */
double *D;    /* diagonal matrix used in LDL^t decomposition */
{
   int row,col;
   for (row = 0; row < n; row++)
       for (col = 0; col < n; col++) {

      if (row == col) {  /* assign values to the diagonal elements */
         AssignValue(n,L,row,col,1.0);
         AssignValue(n,D,row,col, G[row+2]/G[row+1]);
      } 
     else if (row < col) {  /* assign values to the upper triangular region */
         AssignValue(n,L,row,col,0.0);
         AssignValue(n,D,row,col,0.0);
     }
     else { /* assign values to the lower triangular region */
         AssignValue(n,D,row,col,0.0);
         if (col < row-1) AssignValue(n,L,row,col,0.0);
         else AssignValue(n,L,row,col,G[row]/G[row+1]);
     }
  }
}
  

AssignValue(n,matrix,row,col, value)
int n;                 /* dimension of NxN matrix */                  
double *matrix;         /* <n> by <n> matrix that will obtain the value */
int row,col;           /* where */
double value;           /* value being assigned */
{
   /* each row is sequence of consequtive columns; first loc = 0,0 */
   *(matrix + n * row + col) = value;
}

double Value(n,matrix,row,col)
int n;                  /* dimension of NxN matrix */                  
double *matrix;         /* <n> by <n> matrix that will obtain the value */
int row,col;            /* where */
{
   /* each row is a sequence of consequtive columns;  first loc = 0,0 */
  return(*(matrix + n * row + col));
}


double TValue(n,matrix,row,col)  /* return the transpose value */
int n;                   /* dimension of NxN matrix */                  
double *matrix;          /* <n> by <n> matrix that will obtain the value */
int row,col;             /* where */
{
  return(*(matrix + n * col + row));
}
          

Init_ClosedSpline_Matrices(n,L,D)
int n;       /* size of each of the square matrices */
double *L;    /* lower triangular matrix used in LDL^t decomposition */
double *D;    /* diagonal matrix used in LDL^t decomposition */
{
   int row,col;

   for (row = 0; row < n; row++)
     for (col =0; col < n; col++)  {

     if (row == col) { /* assign diagonal values */
        AssignValue(n,L,row,col,1.0);
        if (row == n-1) 
          AssignValue(n,D,row,col, (G[row+2] - G[row] - ALTSIGN(row,1) * 2)/G[row+1]);
        else AssignValue(n,D,row,col,G[row+2]/G[row+1]);
     }
     else if (row < col) { /* assign to upper triangular region */
         AssignValue(n,L,row,col,0.0);
         AssignValue(n,D,row,col,0.0);
     }
     else {  /* assign to the lower triangular region */
         AssignValue(n,D,row,col,0.0);
         if (row < n-1) 
	    if (col == row-1) AssignValue(n,L,row,col,G[row]/G[row+1]);
            else AssignValue(n,L,row,col,0.0);
         else if (col < n-2) AssignValue(n,L,row,col,ALTSIGN(col,-1)/G[col+2]);    
         else AssignValue(n,L,row,col,(G[row]+ALTSIGN(col,-1))/G[row+1]);
      }
   }
}


PrintMatrix(n,matrix) /* for debugging only */
int n;
double *matrix;
{
  int row, col;
  for (row = 0; row < n; row++) {
    for (col = 0; col < n; col++)
      printf("%lf ",Value(n,matrix,row,col));
    printf("\n");
  }
}


/* solves for x in the equation LDL^t * x = b and puts the result in <b> */
Solve(n,L,D,b)  
int n;            /* number of elements in <x> and <b> */
double *L;        /* lower triangular matrix : assumed to be initialized */
double *D;        /* diagonal matrix assumed to be initialized */
REAL_POINT b[];   /* vector of known point values & result destination */
{
   int row, col;

   for (row = 1; row < n; row++) /* forward substitution */
       for (col = 0; col < row; col++) {
           b[row].x -= (Value(n,L,row,col) * b[col].x);
           b[row].y -= (Value(n,L,row,col) * b[col].y);
       }

   b[n-1].x /= Value(n,D,n-1,n-1);
   b[n-1].y /= Value(n,D,n-1,n-1);

   for (row = n-2; row >= 0; row--) { /* back substitution */
       b[row].x /= Value(n,D,row,row);
       b[row].y  /= Value(n,D,row,row);
       for (col = row +1; col < n; col++ ) {
           b[row].x -= (TValue(n,L,row,col) * b[col].x);
           b[row].y -= (TValue(n,L,row,col) * b[col].y);
       }
   }
}


PrintPoint(p)
REAL_POINT *p;
{
  printf("%lf, %lf\n", p->x, p->y);
}

PrintVector(n,v)
int n;
REAL_POINT v[];
{
   int i;
   for (i = 0; i < n; i++) PrintPoint(&v[i]);
   printf("\n");
}


FillVector(n,v,list,xadj,yadj)
int n;                /* size of vectors <= length of list */
REAL_POINT v[];       /* vector to receive the x,y values in list */
DLISTPTR list;        /* first element to get the values from  */ 
float xadj, yadj;     /* how to adjust the x,y values  (1 if no adjustment) */
{
   DLISTPTR tmp = list; 
   int i;
    
   if (n > 0) {
      for (i = 0; i < n; i++) {
          v[i].x = xadj * (POINT(tmp)->x) ;
          v[i].y = yadj * (POINT(tmp)->y) ;
          tmp = NEXT(tmp);
     }
  }
}


/* ListVector : takes a vector of points and makes it into a list 
 * can be passed to the any of the bspline drawing routines.
 */
DLISTPTR ListVector(n,v)
int n;                     /* n <= length of <v>  */
REAL_POINT v[];
{
   DLISTPTR list = (DLISTPTR)calloc(1,sizeof(DLIST_ELEMENT));
   DLISTPTR element;
   int i;

   Init_List(list);
   for (i = 0; i < n; i++) {
       element = (DLISTPTR)calloc(1,sizeof(DLIST_ELEMENT));
       element->contents = &(v[i]);
       INSERT_LAST(element,list);  /* insert in same order */
   }
   return(list);
}


Draw_Open_Ispline(w,control_points)
struct Window *w;
DLISTPTR control_points;
{
   int n = LENGTH(control_points) -2;
   DLISTPTR newpoints;

   REAL_POINT *b = VECTOR(n);
   double *L  = MATRIX(n);
   double *D  = MATRIX(n);

   FillVector(n, b, NEXT(FIRST(control_points)), 6.0, 6.0);
   b[0].x -= POINT(FIRST(control_points))->x ;
   b[0].y -= POINT(FIRST(control_points))->y ;
   b[n-1].x -= POINT(LAST(control_points))->x ;
   b[n-1].y -= POINT(LAST(control_points))->y ;

   Init_OpenSpline_Matrices(n,L,D);
   Solve(n,L,D,b);

   newpoints =  ListVector(n,b);
   MOVE_FIRST(control_points,newpoints);
   MOVE_LAST(control_points, newpoints);

   Draw_Natural_Bspline(w,newpoints);

   MOVE_FIRST(newpoints,control_points);
   MOVE_LAST(newpoints,control_points);
   ClearList(newpoints,FALSE);      
   free(newpoints);  free(L); free(D); free(b);
   
}


Draw_Closed_Ispline(w,control_points)
struct Window *w;
DLISTPTR control_points;
{
   int n = LENGTH(control_points);
   DLISTPTR newpoints;

   REAL_POINT *b = VECTOR(n);
   double *L  = MATRIX(n);
   double *D  = MATRIX(n);

   FillVector(n, b, FIRST(control_points), 6.0, 6.0);

   Init_ClosedSpline_Matrices(n,L,D);
   Solve(n,L,D,b);

   newpoints =  ListVector(n,b);
   Draw_Closed_Bspline(w,newpoints);

   ClearList(newpoints,FALSE);      
   free(newpoints);  free(L); free(D); free(b);
   
}