/* Author: Jonathan Brumberg
   Date  : 2006-04-26
   File  : TPS.c

   Description: Adapted into C specifically for in CS580 project 4 from 
   Geometric Tools, Inc, WM3IntpThinPlateSpline2() originally written in
   C++

   // Geometric Tools, Inc.
   // http://www.geometrictools.com
   // Copyright (c) 1998-2006.  All Rights Reserved
   //
   // The Wild Magic Library (WM3) source code is supplied under the terms of
   // the license agreement
   //     http://www.geometrictools.com/License/WildMagic3License.pdf
   // and may not be copied or disclosed except in accordance with the terms
   // of that agreement.
   
   Defines a Thin Plate spline, which does not require uniformly
   sampled control points.
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "TPS.h"
#include "matrix.h"

#define DEBUG 0

/*------------------------------------------------------------*/
void initializeTPS(TPS *T, float *X, float *Y, float *F, int N, float smoothVal) {
  int i, row, col;
  matrix A,B,Ainv,P,Q,Qinv;
  float PF[3];
  float *tmp;

  float dx,dy;

  if(!T->allocated) {
    /* allocate memory for structure elements */
    T->dataX = (float *)calloc((size_t)N,sizeof(float));
    T->dataY = (float *)calloc((size_t)N,sizeof(float));
    T->coefA = (float *)calloc((size_t)N,sizeof(float));
    T->allocated = 1;
  }

  tmp = (float *)calloc((size_t)N,sizeof(float));

  if(T->dataX==NULL || T->dataY==NULL || T->coefA==NULL || tmp==NULL) {
    fprintf(stderr,"Can't allocate Memory\n");
    exit(-1);
  }

  if(DEBUG)
    printf("Assigning data points X,Y\n");

  /* Assign X,Y data points */
  for(i = 0; i < N; i++) {
    T->dataX[i] = X[i];
    T->dataY[i] = Y[i];    
  }

  T->N = N;
  if(DEBUG)
    printf("N=%d data points\n",T->N);


  /* Compute matrix A = E+smooth*I */
  A = createMatrix(N,N);
  initMatrix(A,MAT_ZEROS);

  if(DEBUG)
    printf("Computing A = E+smooth*I\n");

  col = 0;
  for(row = 0; row < N; row++) {
    if(DEBUG) {
      if(row%5==0)
	printf("completed: %1.2f\n",(100.0f*(row+1)*N+(col+1))/(N*N));
    }
    for(col = 0; col < N; col++) {
      if(row == col) {
	A.mat[row][col] = smoothVal;
      }
      else {
	dx = X[row] - X[col];
	dy = Y[row] - Y[col];
	A.mat[row][col] += solveTPS(sqrt(dx*dx+dy*dy));
      }
    }
  }
  
  /* Compute matrix B */
  B = createMatrix(N,3);
  initMatrix(B,MAT_ZEROS);

  if(DEBUG)
    printf("Compute B\n");

  for(row = 0; row < N; row++) {
    B.mat[row][0] = 1.0f;
    B.mat[row][1] = X[row];
    B.mat[row][2] = Y[row];
  }

  if(DEBUG)
    printf("Compute A^-1\n");
  /* Compute A^{-1} */
  Ainv = createMatrix(N,N);
  Ainv = invertMatrix(A);

  if(DEBUG)
    printf("compute P = B^T * A^-1\n");

  /* Compute P = B^T * A^{-1} */
  P = createMatrix(3,N);
  P = multMatrix(transposeMatrix(B),Ainv);

  if(DEBUG)
    printf("compute Q = P*B\n");

  /* Compute Q = P*B */
  Q = createMatrix(3,3);
  Q = multMatrix(P,B);

  if(DEBUG)
    printf("Compute Q^-1\n");

  /* Compute Qinv */
  Qinv = createMatrix(3,3);
  Qinv = invertMatrix(Q);

  if(DEBUG)
    printf("Calc: PF = P*f(x,y)\n");

  /* calculate PF = P*f(x,y) */
  for(row = 0; row < 3; row++) {
    PF[row] = 0.0f;
    for(i = 0; i < N; i++) {
      PF[row] += 1.0f*P.mat[row][i]*F[i];
    }
  }

  if(DEBUG)
    printf("Compute b\n");

  /* compute coefB */
  for(row = 0; row < 3; row++) {
    T->coefB[row] = 0.0f;
    for(i = 0; i < 3; i++) {
      T->coefB[row] += Qinv.mat[row][i]*PF[i];
    }
  }

  if(DEBUG)
    printf("Compute tmp\n");

  /* compute tmp = z-B*b */
  for(row = 0; row < N; row++) {
    tmp[row] = F[row];
    for(i = 0; i < 3; i++) {
      tmp[row] -= B.mat[row][i]*T->coefB[i];
    }
  }

  if(DEBUG)
    printf("Compute a\n");

  /* compute coefA */
  for(row = 0; row < N; row++) {
    T->coefA[row] = 0.0f;
    for(i = 0; i < N; i++) {
      T->coefA[row] += Ainv.mat[row][i]*tmp[i];
    }
  }

  deleteMatrix(&A);
  deleteMatrix(&Ainv);
  deleteMatrix(&B);
  deleteMatrix(&Q);
  deleteMatrix(&Qinv);
  deleteMatrix(&P);
  free(tmp);
}

/*------------------------------------------------------------*/
float evaluateTPS(TPS T, float X, float Y) {
  int i;
  float interpVal;
  float dx,dy,Tval;

  interpVal = T.coefB[0] + T.coefB[1]*X + T.coefB[2]*Y;
  for(i = 0; i < T.N; i++) {
    dx = X - T.dataX[i];
    dy = Y - T.dataY[i];
    Tval = sqrt(dx*dx+dy*dy);
    interpVal += T.coefA[i]*solveTPS(Tval);
  }
  return interpVal;
}

/*------------------------------------------------------------*/
float solveTPS(float Tval) {
  float soln = 0.0f;
  
  if(Tval > 0) 
    soln = (Tval*Tval)*log(Tval);
  return soln;
}

/*------------------------------------------------------------*/
void printTPS(TPS T) {
  int i;
  
  for(i=0;i<T.N;i++) {
    printf("(%1.1f,%1.1f)\t",T.dataX[i],T.dataY[i]);
    printf("%1.1f\t",T.coefA[i]);
    if(i<3) 
      printf("%1.1f\n",T.coefB[i]);
    else
      printf("\n");
  }
}