/***************************************************************************/
/***************************************************************************/
/*
    CRS.C - Center for Remote Sensing rectification routines

    Written By: Brian J. Buckley

            At: The Center for Remote Sensing
                Michigan State University
                302 Berkey Hall
                East Lansing, MI  48824
                (517)353-7195

    Written: 12/19/91

    Last Update: 12/26/91 Brian J. Buckley
    Last Update:  1/24/92 Brian J. Buckley
      Added printout of trnfile. Triggered by BDEBUG.
    Last Update:  1/27/92 Brian J. Buckley
      Fixed bug so that only the active control points were used.
*/
/***************************************************************************/
/***************************************************************************/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#include <grass/gis.h>

/*
#define MSDOS 1
*/

/*
#define BDEBUG
*/
#ifdef BDEBUG
  FILE *fp;
#endif

#ifdef MSDOS

#include "stdlib.h"
#include "dummy.h"

typedef double DOUBLE;

#else

#include <grass/imagery.h>

typedef double DOUBLE;

#endif

#include "crs.h"

/* STRUCTURE FOR USE INTERNALLY WITH THESE FUNCTIONS.  THESE FUNCTIONS EXPECT
   SQUARE MATRICES SO ONLY ONE VARIABLE IS GIVEN (N) FOR THE MATRIX SIZE */

struct MATRIX
  {
  int n;     /* SIZE OF THIS MATRIX (N x N) */
  DOUBLE *v;
  };

/* CALCULATE OFFSET INTO ARRAY BASED ON R/C */

#define M(row,col) m->v[(((row)-1)*(m->n))+(col)-1]

/***************************************************************************/
/*
*/
/***************************************************************************/

#define MSUCCESS     1 /* SUCCESS */
#define MNPTERR      0 /* NOT ENOUGH POINTS */
#define MUNSOLVABLE -1 /* NOT SOLVABLE */
#define MMEMERR     -2 /* NOT ENOUGH MEMORY */
#define MPARMERR    -3 /* PARAMETER ERROR */
#define MINTERR     -4 /* INTERNAL ERROR */

/***************************************************************************/
/*
    FUNCTION PROTOTYPES FOR STATIC (INTERNAL) FUNCTIONS
*/
/***************************************************************************/

#ifdef MSDOS

static int calccoef(struct Control_Points *,double *,double *,int);
static int calcls(struct Control_Points *,struct MATRIX *,
                  DOUBLE *,DOUBLE *,double *,double *);
static int exactdet(struct Control_Points *,struct MATRIX *,
                    DOUBLE *,DOUBLE *,double *,double *);
static int solvemat(struct MATRIX *,DOUBLE *,DOUBLE *,double *,double *);
static DOUBLE term(int,double,double);

#ifdef BDEBUG
static int checkgeoref(struct Control_Points *,double *,double *,int,int,
                       FILE *fp);
#endif

#else

static int calccoef();
static int calcls();
static int exactdet();
static int solvemat();
static DOUBLE term();
#ifdef BDEBUG
static int checkgeoref();
#endif

#endif

/***************************************************************************/
/*
    USE THIS TRANSFORMATION FUNCTION IF YOU WANT TO DO ARRAYS.
*/
/***************************************************************************/

#ifdef BETTERGEOREF

extern void CRS_georef(int,DOUBLE *,DOUBLE *,DOUBLE *,DOUBLE *,int);

void 
CRS_georef2 (
    int order,  /* ORDER OF TRANSFORMATION TO BE PERFORMED, MUST MATCH THE
               ORDER USED TO CALCULATE THE COEFFICIENTS */
    double E[], /* EASTING COEFFICIENTS */
    double N[], /* NORTHING COEFFICIENTS */
    double e[], /* EASTINGS TO BE TRANSFORMED */
    double n[], /* NORTHINGS TO BE TRANSFORMED */
    int numpts /* NUMBER OF POINTS TO BE TRANSFORMED */
)
  {
  DOUBLE e3, e2n, e2, en2, en, e1, n3, n2, n1;
  int i;

  switch(order)
    {
    case 3:

      for(i = 0 ; i < numpts ; i++)
        {
        e1  = e[i];
        n1  = n[i];
        e2  = e1 * e1;
        en  = e1 * n1;
        n2  = n1 * n1;
        e3  = e1 * e2;
        e2n = e2 * n1;
        en2 = e1 * n2;
        n3  = n1 * n2;

        e[i] = E[0]      +
               E[1] * e1 + E[2] * n1  +
               E[3] * e2 + E[4] * en  + E[5] * n2  +
               E[6] * e3 + E[7] * e2n + E[8] * en2 + E[9] * n3;
        n[i] = N[0]      +
               N[1] * e1 + N[2] * n1  +
               N[3] * e2 + N[4] * en  + N[5] * n2  +
               N[6] * e3 + N[7] * e2n + N[8] * en2 + N[9] * n3;
        }
      break;

    case 2:

      for(i = 0 ; i < numpts ; i++)
        {
        e1 = e[i];
        n1 = n[i];
        e2 = e1 * e1;
        n2 = n1 * n1;
        en = e1 * n1;

        e[i] = E[0]      + E[1] * e1 + E[2] * n1 +
               E[3] * e2 + E[4] * en + E[5] * n2;
        n[i] = N[0]      + N[1] * e1 + N[2] * n1 +
               N[3] * e2 + N[4] * en + N[5] * n2;
        }
      break;

    case 1:

      for(i = 0 ; i < numpts ; i++)
        {
        e1 = e[i];
        n1 = n[i];
        e[i] = E[0] + E[1] * e1 + E[2] * n1;
        n[i] = N[0] + N[1] * e1 + N[2] * n1;
        }
      break;
    }
  }

#endif

/***************************************************************************/
/*
    TRANSFORM A SINGLE COORDINATE PAIR.
*/
/***************************************************************************/

int 
CRS_georef (
    double e1,  /* EASTINGS TO BE TRANSFORMED */
    double n1,  /* NORTHINGS TO BE TRANSFORMED */
    double *e,  /* EASTINGS TO BE TRANSFORMED */
    double *n,  /* NORTHINGS TO BE TRANSFORMED */
    double E[], /* EASTING COEFFICIENTS */
    double N[], /* NORTHING COEFFICIENTS */
    int order  /* ORDER OF TRANSFORMATION TO BE PERFORMED, MUST MATCH THE
               ORDER USED TO CALCULATE THE COEFFICIENTS */
)
  {
  DOUBLE e3, e2n, en2, n3, e2, en, n2;

  switch(order)
    {
    case 1:

      *e = E[0] + E[1] * e1 + E[2] * n1;
      *n = N[0] + N[1] * e1 + N[2] * n1;
      break;

    case 2:

      e2 = e1 * e1;
      n2 = n1 * n1;
      en = e1 * n1;

      *e = E[0]      + E[1] * e1 + E[2] * n1 +
           E[3] * e2 + E[4] * en + E[5] * n2;
      *n = N[0]      + N[1] * e1 + N[2] * n1 +
           N[3] * e2 + N[4] * en + N[5] * n2;
      break;

    case 3:

      e2  = e1 * e1;
      en  = e1 * n1;
      n2  = n1 * n1;
      e3  = e1 * e2;
      e2n = e2 * n1;
      en2 = e1 * n2;
      n3  = n1 * n2;

      *e = E[0]      +
           E[1] * e1 + E[2] * n1  +
           E[3] * e2 + E[4] * en  + E[5] * n2  +
           E[6] * e3 + E[7] * e2n + E[8] * en2 + E[9] * n3;
      *n = N[0]      +
           N[1] * e1 + N[2] * n1  +
           N[3] * e2 + N[4] * en  + N[5] * n2  +
           N[6] * e3 + N[7] * e2n + N[8] * en2 + N[9] * n3;
      break;

    default:

      return(MPARMERR);
      break;
    }

  return(MSUCCESS);
  }

/***************************************************************************/
/*
    COMPUTE THE GEOREFFERENCING COEFFICIENTS BASED ON A SET OF CONTROL POINTS
*/
/***************************************************************************/

int 
CRS_compute_georef_equations (struct Control_Points *cp, double E12[], double N12[], double E21[], double N21[], int order)
  {
  double *tempptr;
  int status;

  if(order < 1 || order > MAXORDER)
    return(MPARMERR);

#ifdef BDEBUG
  fp = fopen("error.dat","w");
  if(fp == NULL)
    return(-1);
#endif

  /* CALCULATE THE FORWARD TRANSFORMATION COEFFICIENTS */

  status = calccoef(cp,E12,N12,order);
  if(status != MSUCCESS)
    return(status);

#ifdef BDEBUG
  checkgeoref(cp,E12,N12,order,1,fp);
#endif

  /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS */

  tempptr = cp->e1;
  cp->e1 = cp->e2;
  cp->e2 = tempptr;
  tempptr = cp->n1;
  cp->n1 = cp->n2;
  cp->n2 = tempptr;

  /* CALCULATE THE BACKWARD TRANSFORMATION COEFFICIENTS */

  status = calccoef(cp,E21,N21,order);

#ifdef BDEBUG
  checkgeoref(cp,E21,N21,order,0,fp);
  fclose(fp);
#endif

  /* SWITCH THE 1 AND 2 EASTING AND NORTHING ARRAYS BACK */

  tempptr = cp->e1;
  cp->e1 = cp->e2;
  cp->e2 = tempptr;
  tempptr = cp->n1;
  cp->n1 = cp->n2;
  cp->n2 = tempptr;

  return(status);
  }

/***************************************************************************/
/*
    COMPUTE THE GEOREFFERENCING COEFFICIENTS BASED ON A SET OF CONTROL POINTS
*/
/***************************************************************************/

static int 
calccoef (struct Control_Points *cp, double E[], double N[], int order)
  {
  struct MATRIX m;
  DOUBLE *a;
  DOUBLE *b;
  int numactive;   /* NUMBER OF ACTIVE CONTROL POINTS */
  int status, i;

  /* CALCULATE THE NUMBER OF VALID CONTROL POINTS */

  for(i = numactive = 0 ; i < cp->count ; i++)
    {
    if(cp->status[i] > 0)
      numactive++;
    }

  /* CALCULATE THE MINIMUM NUMBER OF CONTROL POINTS NEEDED TO DETERMINE
     A TRANSFORMATION OF THIS ORDER */

  m.n = ((order + 1) * (order + 2)) / 2;

  if(numactive < m.n)
    return(MNPTERR);

  /* INITIALIZE MATRIX */

  m.v = (DOUBLE *)G_calloc(m.n*m.n,sizeof(DOUBLE));
  if(m.v == NULL)
    {
    return(MMEMERR);
    }
  a = (DOUBLE *)G_calloc(m.n,sizeof(DOUBLE));
  if(a == NULL)
    {
    G_free((char *)m.v);
    return(MMEMERR);
    }
  b = (DOUBLE *)G_calloc(m.n,sizeof(DOUBLE));
  if(b == NULL)
    {
    G_free((char *)m.v);
    G_free((char *)a);
    return(MMEMERR);
    }

  if(numactive == m.n)
    status = exactdet(cp,&m,a,b,E,N);
  else
    status = calcls(cp,&m,a,b,E,N);

  G_free((char *)m.v);
  G_free((char *)a);
  G_free((char *)b);

  return(status);
  }

/***************************************************************************/
/*
    CALCULATE THE TRANSFORMATION COEFFICIENTS WITH EXACTLY THE MINIMUM
    NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.
*/
/***************************************************************************/

static int exactdet (
    struct Control_Points *cp,
    struct MATRIX *m,
    DOUBLE a[],
    DOUBLE b[],
    double E[],     /* EASTING COEFFICIENTS */
    double N[]     /* NORTHING COEFFICIENTS */
)
  {
  int pntnow, currow, j;

  currow = 1;
  for(pntnow = 0 ; pntnow < cp->count ; pntnow++)
    {
    if(cp->status[pntnow] > 0)
      {
      /* POPULATE MATRIX M */

#ifdef BDEBUG
      fprintf(fp,"%2d ",pntnow+1);
#endif

      for(j = 1 ; j <= m->n ; j++)
        {
        M(currow,j) = term(j,cp->e1[pntnow],cp->n1[pntnow]);
#ifdef BDEBUG
        fprintf(fp,"%+14.7le ",M(currow,j));
        if(j == 5)
          fprintf(fp,"\n   ");
#endif
        }
#ifdef BDEBUG
      fprintf(fp,"\n");
#endif

      /* POPULATE MATRIX A AND B */

      a[currow-1] = cp->e2[pntnow];
      b[currow-1] = cp->n2[pntnow];
#ifdef BDEBUG
      fprintf(fp,"   %+14.7le ",a[currow-1]);
      fprintf(fp,"%+14.7le\n",b[currow-1]);
#endif

      currow++;
      }
#ifdef BDEBUG
    else
      {
      fprintf(fp,"%2d UNUSED\n",pntnow+1);
      }
#endif
    }

  if(currow - 1 != m->n)
    return(MINTERR);

  return(solvemat(m,a,b,E,N));
  }

/***************************************************************************/
/*
    CALCULATE THE TRANSFORMATION COEFFICIENTS WITH MORE THAN THE MINIMUM
    NUMBER OF CONTROL POINTS REQUIRED FOR THIS TRANSFORMATION.  THIS
    ROUTINE USES THE LEAST SQUARES METHOD TO COMPUTE THE COEFFICIENTS.
*/
/***************************************************************************/

static int calcls (
    struct Control_Points *cp,
    struct MATRIX *m,
    DOUBLE a[],
    DOUBLE b[],
    double E[],     /* EASTING COEFFICIENTS */
    double N[]     /* NORTHING COEFFICIENTS */
)
  {
  int i, j, n, numactive = 0;

  /* INITIALIZE THE UPPER HALF OF THE MATRIX AND THE TWO COLUMN VECTORS */

  for(i = 1 ; i <= m->n ; i++)
    {
    for(j = i ; j <= m->n ; j++)
      M(i,j) = 0.0;
    a[i-1] = b[i-1] = 0.0;
    }

  /* SUM THE UPPER HALF OF THE MATRIX AND THE COLUMN VECTORS ACCORDING TO
     THE LEAST SQUARES METHOD OF SOLVING OVER DETERMINED SYSTEMS */

  for(n = 0 ; n < cp->count ; n++)
    {
    if(cp->status[n] > 0)
      {
      numactive++;
      for(i = 1 ; i <= m->n ; i++)
        {
        for(j = i ; j <= m->n ; j++)
          M(i,j) += term(i,cp->e1[n],cp->n1[n]) * term(j,cp->e1[n],cp->n1[n]);

        a[i-1] += cp->e2[n] * term(i,cp->e1[n],cp->n1[n]);
        b[i-1] += cp->n2[n] * term(i,cp->e1[n],cp->n1[n]);
        }
      }
    }

  if(numactive <= m->n)
    return(MINTERR);

  /* TRANSPOSE VALUES IN UPPER HALF OF M TO OTHER HALF */

  for(i = 2 ; i <= m->n ; i++)
    {
    for(j = 1 ; j < i ; j++)
      M(i,j) = M(j,i);
    }

  return(solvemat(m,a,b,E,N));
  }

/***************************************************************************/
/*
    CALCULATE THE X/Y TERM BASED ON THE TERM NUMBER

ORDER\TERM   1    2    3    4    5    6    7    8    9   10
  1        e0n0 e1n0 e0n1
  2        e0n0 e1n0 e0n1 e2n0 e1n1 e0n2
  3        e0n0 e1n0 e0n1 e2n0 e1n1 e0n2 e3n0 e2n1 e1n2 e0n3
*/
/***************************************************************************/

static DOUBLE term (int term, double e, double n)
  {
  switch(term)
    {
    case  1: return((DOUBLE)1.0);
    case  2: return((DOUBLE)e);
    case  3: return((DOUBLE)n);
    case  4: return((DOUBLE)(e*e));
    case  5: return((DOUBLE)(e*n));
    case  6: return((DOUBLE)(n*n));
    case  7: return((DOUBLE)(e*e*e));
    case  8: return((DOUBLE)(e*e*n));
    case  9: return((DOUBLE)(e*n*n));
    case 10: return((DOUBLE)(n*n*n));
    }
  return((DOUBLE)0.0);
  }

/***************************************************************************/
/*
    SOLVE FOR THE 'E' AND 'N' COEFFICIENTS BY USING A SOMEWHAT MODIFIED
    GAUSSIAN ELIMINATION METHOD.

    | M11 M12 ... M1n | | E0   |   | a0   |
    | M21 M22 ... M2n | | E1   | = | a1   |
    |  .   .   .   .  | | .    |   | .    |
    | Mn1 Mn2 ... Mnn | | En-1 |   | an-1 |

    and

    | M11 M12 ... M1n | | N0   |   | b0   |
    | M21 M22 ... M2n | | N1   | = | b1   |
    |  .   .   .   .  | | .    |   | .    |
    | Mn1 Mn2 ... Mnn | | Nn-1 |   | bn-1 |
*/
/***************************************************************************/

static int solvemat (struct MATRIX *m,
  DOUBLE a[], DOUBLE b[], double E[], double N[])
{
  int i, j, i2, j2, imark;
  DOUBLE factor, temp;
  DOUBLE pivot;  /* ACTUAL VALUE OF THE LARGEST PIVOT CANDIDATE */

  for(i = 1 ; i <= m->n ; i++)
    {
    j = i;

    /* find row with largest magnitude value for pivot value */

    pivot = M(i,j);
    imark = i;
    for(i2 = i + 1 ; i2 <= m->n ; i2++)
      {
      temp = fabs(M(i2,j));
      if(temp > fabs(pivot))
        {
        pivot = M(i2,j);
        imark = i2;
        }
      }

    /* if the pivot is very small then the points are nearly co-linear */
    /* co-linear points result in an undefined matrix, and nearly */
    /* co-linear points results in a solution with rounding error */

    if(pivot == 0.0)
      return(MUNSOLVABLE);

    /* if row with highest pivot is not the current row, switch them */
 
    if(imark != i)
      {
      for(j2 = 1 ; j2 <= m->n ; j2++)
        {
        temp = M(imark,j2);
        M(imark,j2) = M(i,j2);
        M(i,j2) = temp;
        }

      temp = a[imark-1];
      a[imark-1] = a[i-1];
      a[i-1] = temp;

      temp = b[imark-1];
      b[imark-1] = b[i-1];
      b[i-1] = temp;
      }

    /* compute zeros above and below the pivot, and compute
       values for the rest of the row as well */

    for(i2 = 1 ; i2 <= m->n ; i2++)
      {
      if(i2 != i)
        {
        factor = M(i2,j) / pivot;
        for(j2 = j ; j2 <= m->n ; j2++)
          M(i2,j2) -= factor * M(i,j2);
        a[i2-1] -= factor * a[i-1];
        b[i2-1] -= factor * b[i-1];
        }
      }
    }

  /* SINCE ALL OTHER VALUES IN THE MATRIX ARE ZERO NOW, CALCULATE THE
     COEFFICIENTS BY DIVIDING THE COLUMN VECTORS BY THE DIAGONAL VALUES. */

  for(i = 1 ; i <= m->n ; i++)
    {
    E[i-1] = a[i-1] / M(i,i);
    N[i-1] = b[i-1] / M(i,i);
    }

  return(MSUCCESS);
  }

/***************************************************************************/
/*
*/
/***************************************************************************/

#ifdef BDEBUG

static int checkgeoref (struct Control_Points *cp,
  double E[], double N[], int order, int forward, FILE *fp)
{
  DOUBLE xrms, yrms, dx, dy, dx2, dy2, totaldist, dist;
  double tempx, tempy;
  int i, n, numactive;

  n = ((order + 1) * (order + 2)) / 2;

  if(forward)
    fprintf(fp,"FORWARD:\n");
  else
    fprintf(fp,"BACKWARD:\n");

  fprintf(fp,"%d order\n",order);
  for(i = 0 ; i < n ; i++)
    fprintf(fp,"%+.17E %+.17E\n",E[i],N[i]);

  xrms = yrms = dx2 = dy2 = totaldist = 0.0;
  numactive = 0;
  for(i = 0 ; i < cp->count ; i++)
    {
    fprintf(fp,"\nCONTROL POINT: %d\n",i+1);

    fprintf(fp,"%20s: %+.20lE %+.20lE\n","ORIGINAL POINT",
            cp->e1[i],cp->n1[i]);
    fprintf(fp,"%20s: %+.20lE %+.20lE\n","DESIRED POINT",
            cp->e2[i],cp->n2[i]);

    if(cp->status[i] > 0)
      {
      numactive++;
      CRS_georef(cp->e1[i],cp->n1[i],&tempx,&tempy,E,N,order);

      fprintf(fp,"%20s: %+.20lE %+.20lE\n","CALCULATED POINT",tempx,tempy);
      dx = tempx - cp->e2[i];
      dy = tempy - cp->n2[i];
      fprintf(fp,"%20s: %+.20lE %+.20lE\n","RESIDUAL ERROR",dx,dy);
      dx2 = dx * dx;
      dy2 = dy * dy;
      dist = sqrt(dx2 + dy2);
      fprintf(fp,"%20s: %+.20lE\n","DISTANCE (RMS) ERROR",dist);

      xrms += dx2;
      yrms += dy2;

      totaldist += dist;
      }
    else
      fprintf(fp,"NOT USED\n");
    }
  xrms = sqrt(xrms / (DOUBLE)numactive);
  yrms = sqrt(yrms / (DOUBLE)numactive);

  fprintf(fp,"\n%20s: %+.20lE %+.20lE\n","RMS ERROR",xrms,yrms);

  fprintf(fp,"\n%20s: %+.20lE\n","TOTAL RMS ERROR",sqrt(xrms*xrms+yrms*yrms));

  fprintf(fp,"\n%20s: %+.20lE\n","AVG. DISTANCE ERROR",totaldist/numactive);

  return(0);
  }

#endif