/**
 * \file transform.c
 *
 * \brief This file contains routines which perform (affine?)
 * transformations from one coordinate system into another.
 *
 * The second system may be translated, stretched, and rotated relative
 * to the first. The input system is system <em>a</em> and the output
 * system is <em>b</em>.
 *
 * This program is free software under the GNU General Public License
 * (>=v2). Read the file COPYING that comes with GRASS for details.
 *
 * \author GRASS GIS Development Team
 *
 * \date 1987-2007
 */

/****************************************************************
note: uses sqrt() from math library
*****************************************************************
Points from one system may be converted into the second by
use of one of the two equation routines.

transform_a_into_b (ax,ay,bx,by)

    double ax,ay;            input point from system a
    double *bx,*by;          resultant point in system b

transform_b_into_a (bx,by,ax,ay)

    double bx,by;            input point from system b
    double *ax,*ay;          resultant point in system a
*****************************************************************
Residual analysis on the equation can be run to test how well
the equations work.  Either test how well b is predicted by a
or vice versa.

residuals_a_predicts_b (ax,ay,bx,by,use,n,residuals,rms)
residuals_b_predicts_a (ax,ay,bx,by,use,n,residuals,rms)

    double ax[], ay[];       coordinate from system a
    double bx[], by[];       coordinate from system b
    char   use[];            use point flags
    int n;                   number of points in ax,ay,bx,by
    double residual[]        residual error for each point
    double *rms;             overall root mean square error
****************************************************************/

#include <stdio.h>
#include <math.h>
#include <grass/transform.h>

/* the coefficients */
static double A0, A1, A2, A3, A4, A5;
static double B0, B1, B2, B3, B4, B5;

/* function prototypes */
static int resid(double *, double *, double *, double *, int *, int, double *,
		 double *, int);


/**
 * \fn int compute_transformation_coef (double ax[], double ay[], double bx[], double by[], char *use, int n)
 *
 * \brief The first step is to compute coefficients for a set of equations
 * which are then used to convert from the one system to the other.
 *
 * A set of x,y points from both systems is input into the equation
 * generator which determines the equation coefficients which most
 * nearly represent the original points. These coefficients are kept
 * in a static variables internal to this file.
 *
 * NOTE: use[i] must be true for ax[i],ay[i],bx[i],by[i] to be used
 * in the equation.  Also, the total number of used points must be
 * 4 or larger.
 *
 * \param[in] ax coordinate from system a
 * \param[in] ay coordinate from system a
 * \param[in] bx coordinate from system b
 * \param[in] by coordinate from system b
 * \param[in] use use point flags
 * \param[in] n number of points in ax, ay, bx, by
 * \return int 1 if successful
 * \return int -1 if could not solve equation. Points probably colinear.
 * \return int -2 if less than 4 points used
 */

int compute_transformation_coef(double ax[], double ay[], double bx[],
				double by[], int *use, int n)
{
    int i;
    int j;
    int count;
    double aa[3];
    double aar[3];
    double bb[3];
    double bbr[3];

    double cc[3][3];
    double x;

    count = 0;
    for (i = 0; i < n; i++)
	if (use[i])
	    count++;
    if (count < 4)
	return -2;		/* must have at least 4 points */

    for (i = 0; i < 3; i++) {
	aa[i] = bb[i] = 0.0;

	for (j = 0; j < 3; j++)
	    cc[i][j] = 0.0;
    }

    for (i = 0; i < n; i++) {
	if (!use[i])
	    continue;		/* skip this point */
	cc[0][0] += 1;
	cc[0][1] += bx[i];
	cc[0][2] += by[i];

	cc[1][1] += bx[i] * bx[i];
	cc[1][2] += bx[i] * by[i];
	cc[2][2] += by[i] * by[i];

	aa[0] += ay[i];
	aa[1] += ay[i] * bx[i];
	aa[2] += ay[i] * by[i];

	bb[0] += ax[i];
	bb[1] += ax[i] * bx[i];
	bb[2] += ax[i] * by[i];
    }

    cc[1][0] = cc[0][1];
    cc[2][0] = cc[0][2];
    cc[2][1] = cc[1][2];

    /* aa and bb are solved */
    if (inverse(cc) < 0)
	return (-1);
    if (m_mult(cc, aa, aar) < 0 || m_mult(cc, bb, bbr) < 0)
	return (-1);

    /* the equation coefficients */
    B0 = aar[0];
    B1 = aar[1];
    B2 = aar[2];

    B3 = bbr[0];
    B4 = bbr[1];
    B5 = bbr[2];

    /* the inverse equation */
    x = B2 * B4 - B1 * B5;

    if (!x)
	return (-1);

    A0 = (B1 * B3 - B0 * B4) / x;
    A1 = -B1 / x;
    A2 = B4 / x;
    A3 = (B0 * B5 - B2 * B3) / x;
    A4 = B2 / x;
    A5 = -B5 / x;

    return 1;
}


int transform_a_into_b(double ax, double ay, double *bx, double *by)
{
    *by = A0 + A1 * ax + A2 * ay;
    *bx = A3 + A4 * ax + A5 * ay;

    return 0;
}


int transform_b_into_a(double bx, double by, double *ax, double *ay)
{
    *ay = B0 + B1 * bx + B2 * by;
    *ax = B3 + B4 * bx + B5 * by;

    return 0;
}

/**************************************************************
These routines are internal to this source code

solve (a, b)
    double a[3][3]
    double b[3]

    equation solver used by compute_transformation_coef()
**************************************************************/

/*  #define abs(xx) (xx >= 0 ? xx : -xx)  */
/*      #define N 3  */


int residuals_a_predicts_b(double ax[], double ay[], double bx[], double by[],
			   int use[], int n, double residuals[], double *rms)
{
    resid(ax, ay, bx, by, use, n, residuals, rms, 1);

    return 0;
}


int residuals_b_predicts_a(double ax[], double ay[], double bx[], double by[],
			   int use[], int n, double residuals[], double *rms)
{
    resid(ax, ay, bx, by, use, n, residuals, rms, 0);

    return 0;
}


/**
 * \fn int print_transform_matrix (void)
 *
 * \brief Prints matrix to stdout in human readable format.
 *
 * \return int 1
 */

int print_transform_matrix(void)
{
    fprintf(stdout, "\nTransformation Matrix\n");
    fprintf(stdout, "| xoff a b |\n");
    fprintf(stdout, "| yoff d e |\n");
    fprintf(stdout, "-------------------------------------------\n");
    fprintf(stdout, "%f %f %f \n", -B3, B2, -B5);
    fprintf(stdout, "%f %f %f \n", -B0, -B1, B4);
    fprintf(stdout, "-------------------------------------------\n");

    return 1;
}


static int resid(double ax[], double ay[], double bx[], double by[],
		 int use[], int n, double residuals[], double *rms, int atob)
{
    double x, y;
    int i;
    int count;
    double sum;
    double delta;
    double dx, dy;

    count = 0;
    sum = 0.0;
    for (i = 0; i < n; i++) {
	if (!use[i])
	    continue;

	count++;
	if (atob) {
	    transform_a_into_b(ax[i], ay[i], &x, &y);
	    dx = x - bx[i];
	    dy = y - by[i];
	}
	else {
	    transform_b_into_a(bx[i], by[i], &x, &y);
	    dx = x - ax[i];
	    dy = y - ay[i];
	}

	delta = dx * dx + dy * dy;
	residuals[i] = sqrt(delta);
	sum += delta;
    }
    *rms = sqrt(sum / count);

    return 0;
}