/* Configuration_and_Procrustus.c
 *
 * Copyright (C) 1993-2002 David Weenink
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*
 djmw 20020424 GPL
 djmw 20020704 Changed header NUMclasses.h to SVD.h
*/

#include "Configuration_and_Procrustus.h"
#include "SVD.h"

static int procrustus (double **x, double **y, long nPoints, 
	long nDimensions, double **t, double *v, double *s)
{
	SVD svd = NULL; 
	long i, j, k;
	double **c, **xc = NULL, **yc = NULL, **yt = NULL;
	double trace, traceXtJYT, traceYtJY;

	Melder_assert (t != NULL && v != NULL && s != NULL);
		
	c = NUMdmatrix (1, nDimensions, 1, nDimensions);
	if (c == NULL) return 0;
	yc = NUMdmatrix_copy (y, 1, nPoints, 1, nDimensions);
	if (yc == NULL) goto end;
	
	/*
		Reference: Borg & Groenen (1997), Modern multidimensional scaling, 
		Springer
		1. Calculate C = X'JY (page 346).
		JY amounts to centering the columns of Y.
	*/
	
	NUMcentreColumns_d (yc, 1, nPoints, 1, nDimensions, NULL);
	for (i = 1; i <= nDimensions; i++)
	{
		for (j = 1; j <= nDimensions; j++)
		{
			for (k = 1; k <= nPoints; k++)
			{
				c[i][j] += x[k][i] * yc[k][j];
			}
		}
	}
	
	/*
		2. Decompose C by SVD:  C = PDQ' (SVD attribute is Q instead of Q'!)
	*/
	
	if (! (svd = SVD_create_d (c, nDimensions, nDimensions))) goto end;
	
	for (trace = 0, i = 1; i <= nDimensions; i++)
	{
		trace += svd -> d[i];
	}
	
	if (trace == 0)
	{
		(void) Melder_error ("NUMprocrustus: degenerate configuration(s).");
		goto end;
	}
	
	/*
		3. T = QP'
	*/
		
	for (i = 1; i <= nDimensions; i++)
	{
		for (j = 1; j <= nDimensions; j++)
		{
			for (t[i][j] = 0, k = 1; k <= nDimensions; k++)
			{
				t[i][j] += svd -> v[i][k] * svd -> u[j][k];
			}
		}
	}
	
	
	if (! (xc = NUMdmatrix_copy (x, 1, nPoints, 1, nDimensions)) ||
		! (yt = NUMdmatrix (1, nPoints, 1, nDimensions))) goto end;
	
	/*
		4. Dilation factor s = (tr X'JYT) / (tr Y'JY)
		   First we need YT.
	*/
	
	for (i = 1; i <= nPoints; i++)
	{
		for (j = 1; j <= nDimensions; j++)
		{
			for (k = 1; k <= nDimensions; k++)
			{
				yt[i][j] += y[i][k] * t[k][j];
			}
		}
	}
	
	/*
		X'J amount to centering the columns of X
	*/
	
	NUMcentreColumns_d (xc, 1, nPoints, 1, nDimensions, NULL);
	
	/*
		tr X'J YT == tr xc' yt
	*/
	
	for (traceXtJYT = 0, i = 1; i <= nDimensions; i++)
	{
		for (j = 1; j <= nPoints; j++)
		{
			traceXtJYT += xc[j][i] * yt[j][i];
		}
	}
	
	for (traceYtJY = 0, i = 1; i <= nDimensions; i++)
	{
		for (j = 1; j <= nPoints; j++)
		{
			traceYtJY += y[j][i] * yc[j][i];
		}
	}
	
	*s = traceXtJYT / traceYtJY;
		
	/*
		5. Translation vector tr = (X - sYT)'1 / nPoints
	*/

	for (i = 1; i <= nDimensions; i++)
	{
		for (j = 1; j <= nPoints; j++)
		{
			v[i] += x[j][i] - *s * yt[j][i];
		}
		v[i] /= nPoints;
	}
	
end:

	forget (svd);
	NUMdmatrix_free (xc, 1, 1);
	NUMdmatrix_free (yc, 1, 1);
	NUMdmatrix_free (c, 1, 1);
	NUMdmatrix_free (yt, 1, 1);
	return ! Melder_hasError ();
}

Procrustus Configurations_to_Procrustus (Configuration me,
	Configuration thee)
{
	Procrustus p;
	if (my numberOfRows != thy numberOfRows || 
		my numberOfColumns != thy numberOfColumns) return Melder_errorp 
		("Configuration_procrustus: Configurations must have the "
			"same number of points and the same dimension.");

	p = Procrustus_create (my numberOfColumns);
	if (p == NULL) return NULL;
	if (! procrustus (my data, thy data, my numberOfRows, my numberOfColumns,
		p -> r, p -> t, &(p -> s))) forget (p);

	return p;
}

/* End of file Configuration_and_Procrustus.c */