/*
 *  photom: a package to perform photometric calibration
 *  Copyright (C) 2001  Michael William Richmond
 *
 *  Contact: Michael William Richmond
 *           Physics Department
 *           Rochester Institute of Technology
 *           85 Lomb Memorial Drive
 *           Rochester, NY  14623-5603
 *           E-mail: mwrsps@rit.edu
 *
 *  
 *  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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

/*
 * replaced CCMATH library routines with GSL routines.  More stable!
 *   MWR 5/6/2002
 */



#include <stdio.h>
#include <math.h>
#include "misc.h"
#include "solve_linear.h"

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

	/* 
	 * we check the singular values of the matrix;  any values which
	 * are smaller than this, we set to zero.
	 */
#define SOLVE_TINY  1.0e-12

#define DEBUG

#undef MAIN
#ifdef MAIN

typedef struct data_struct {
  double x;
  double y;
  double z;
  double weight;
} DATA;

#define NROWS 5
#define NCOLS 2 

static DATA data_array[NROWS] = { 
                             {   1.0,  1.0, -3.0,   1.0  },
                             {   2.0,  1.0,  0.0,   1.0  },
                             {   2.0,  3.0,-12.0,   1.0  },
                             {   2.0,  5.0,-24.0,   1.0  },
                             {   1.0,  5.0,-27.0,   1.0  }
                           };

int
main
	(
	int argc,
	char *argv[]
	)
{
	int i, retval;
	double *matrix_a;     /* the input matrix A */
	                      /*    will be overwritten by SVD routine */
	double *vector_b;     /* holds known values on right-hand side */
	double *vector_x;     /* holds unknown values on left-hand side */
	double *vector_sigma; /* uncertainty in unknown values */

	/* allocate for arrays, vectors */
	matrix_a = (double *) shMalloc(NROWS*NCOLS*sizeof(double));
	vector_b = (double *) shMalloc(NROWS*sizeof(double));
	vector_x = (double *) shMalloc(NCOLS*sizeof(double));
	vector_sigma = (double *) shMalloc(NCOLS*sizeof(double));

	/* fill the known matrix elements */
	for (i = 0; i < NROWS; i++) {
		matrix_a[i*NCOLS + 0] = data_array[i].x;
		matrix_a[i*NCOLS + 1] = data_array[i].y;
	}

	/* fill the known vector elements */
	for (i = 0; i < NROWS; i++) {
		vector_b[i] = data_array[i].z;
	}

	retval = solve_matrix_equation(NROWS, NCOLS, matrix_a,
				vector_b, vector_x, vector_sigma);
	printf("solve_matrix_equation returns %d \n", retval);

	/* free arrays, vector */
	shFree(matrix_a);
	shFree(vector_b);
	shFree(vector_x);
	shFree(vector_sigma);

	exit(0);
}

#endif


	/**************************************************************
	 * PROCEDURE: solve_matrix_equation
	 *
	 * DESCRIPTION: given a set of linear equations which can be
	 *              written in the form
	 *
	 *                     A * x  =  b
	 *
	 *              where
	 *                    A    is 'nrows'-by-'ncols' matrix
	 *                    x    is 'ncols'-by-1       vector (unknown)
	 *                    b    is 1-by-'nrows'       vector
	 *
	 *  this function tries to solve the matrix equation by singular
	 *  value decomposition.  It finds the values of the unknown
	 *  vector "x"; it also calculates the variance of each element
	 *  of "x", placing those variances in the vector "s".
	 *
	 *  The user must allocate space for all four of the matrix/vector
	 *  arguments.  The matrix must be stored as a 1-D array of 
	 *  values, in which
	 *
	 *           element i,j    is in   matrix[i*ncols + j]
	 *
	 *  The matrix A is not overwritten by this routine.  Only the
	 *  arguments "vector_x" and "vector_y" are modified.
	 *  
	 * RETURNS:
	 *    0         if all goes well
	 *    1         if not
	 */                

int
solve_matrix_equation
	(
	int nrows,           /* I: number of rows in matrix A, vector b */
	int ncols,           /* I: number of cols in matrix A, vectors x, s */
	double *matrix_a,    /* I: known matrix A, stored as 1-D array */
	double *vector_b,    /* I: known vector b */
	double *vector_x,    /* O: unknown vector x, for which we solve */
	double *vector_s     /* O: sqrt(variance) in vector x values */
	)
{
	int retval, i, j;
	double value, xx, yy;
	double variance, stdev;
	gsl_matrix *matrix_a1;    /* a copy of the input matrix A */
	                          /*    will be overwritten by SVD routine */
	gsl_matrix *matrix_v;     /* "V" matrix returned by SVD routine */
	gsl_matrix *matrix_X;     /* temp ncols-by-ncols matrix, needed by SVD  */
	double *vector_check;     /* after finding "x", we calc A*x and put */
	                          /*    results in this vector for checking */
	gsl_vector *gsl_vector_x;  /* solution vector "x" in GSL format */
	gsl_vector *gsl_vector_b;  /* solution vector "b" in GSL format */
	gsl_vector *gsl_vector_s;  /* "S" vector 1-by-ncols in GSL format */
	gsl_vector *gsl_vector_work;  /* temp vector 1-by-ncols in GSL format */

	/* allocate space for non-GSL vectors */
   vector_check = (double *) shMalloc(nrows*sizeof(double));
	
	/* allocate space for GSL vectors and matrices */
	matrix_a1 = gsl_matrix_alloc(nrows, ncols);
	matrix_v = gsl_matrix_alloc(ncols, ncols);
	matrix_X = gsl_matrix_alloc(ncols, ncols);

	gsl_vector_x = gsl_vector_alloc(ncols);
	gsl_vector_b = gsl_vector_alloc(nrows);
	gsl_vector_work = gsl_vector_alloc(ncols);
	gsl_vector_s = gsl_vector_alloc(ncols);


	/* make a copy of matrix_a, which we'll pass to SVD routine */
	for (i = 0; i < nrows; i++) {
		for (j = 0; j < ncols; j++) {
			gsl_matrix_set(matrix_a1, i, j, matrix_a[i*ncols + j]);
		}
	}
	/* and make a copy of the input vector b, in GSL format */
	for (i = 0; i < nrows; i++) {
		gsl_vector_set(gsl_vector_b, i, vector_b[i]);
	}

#ifdef DEBUG
	printf("  matrix_a has %6d rows %6d cols \n", nrows, ncols);
	printf("  this is matrix_a, in initial form \n");
	for (i = 0; i < nrows; i++) {
		for (j = 0; j < ncols; j++) {
			printf(" %8.4f ", matrix_a[i*ncols + j]);
		}
		printf(" \n");
	}
	printf("  this is vector_b, in initial form \n");
	for (i = 0; i < nrows; i++) {
		printf(" %8.4f \n", vector_b[i]);
	}
	printf(" \n");
#endif

	/* call the SVD routine */
	retval = gsl_linalg_SV_decomp_mod(matrix_a1, matrix_X, 
						matrix_v, gsl_vector_s, gsl_vector_work);
#ifdef DEBUG
	printf ("gsl_linalg_SV_decom_mod returns  %d \n", retval);
#endif

	/* 
	 * check the returned singular values, which are in the 
	 *   GSL vector "gsl_vector_s".  If the inverse of a value
	 *   is too small, place ZERO into the matrix in its place
	 */
	for (i = 0; i < ncols; i++) {
		value = gsl_vector_get(gsl_vector_s, i);
		if (value > 1.0/SOLVE_TINY) {
			value = 0.0;
		} 
		gsl_vector_set(gsl_vector_s, i, value);
#ifdef DEBUG
		printf(" vector_s value %3d is %10.5e \n", i, value);
#endif
	}

	/*
	 * Now, we can use the decomposed matrices to solve the vector equation
	 */
	retval = gsl_linalg_SV_solve(matrix_a1, matrix_v, gsl_vector_s, 
						gsl_vector_b, gsl_vector_x);

	/* 
	 * place the solution values into the output vector "x" 
	 */
	for (i = 0; i < ncols; i++) {
		vector_x[i] = gsl_vector_get(gsl_vector_x, i);
	}

#ifdef DEBUG
	printf("  this is vector_x, the solution vector \n");
	for (i = 0; i < 1; i++) {
		for (j = 0; j < ncols; j++) {
			printf(" %8.4f ", vector_x[i*ncols + j]);
		}
		printf(" \n");
	}
#endif

	/*
	 * check by multiplying original matrix A by solution vector X
	 *   and see if we get the vector B
	 */
#if 0
#ifdef DEBUG
	rmmult(vector_check, matrix_a, vector_x, nrows, ncols, 1);
	printf("  this is vector_check, which should match vector_b \n");
	for (i = 0; i < nrows; i++) {
		printf(" %8.4f \n", vector_check[i]);
	}
	printf(" \n");
#endif
#endif

	/*
	 * calculate the uncertainty in each of the solution vector elements 
	 */
	for (i = 0; i < ncols; i++) {
		variance = 0;
		for (j = 0; j < ncols; j++) {
			xx = gsl_matrix_get(matrix_v, j, i);
			yy = gsl_vector_get(gsl_vector_s, j);
			variance += (xx*xx)/(yy*yy);
		}
		stdev = sqrt(variance);
		vector_s[i] = stdev;
	}


	/* free all arrays and vectors */
	shFree(vector_check);
	gsl_vector_free(gsl_vector_x);
	gsl_vector_free(gsl_vector_b);
	gsl_vector_free(gsl_vector_s);
	gsl_vector_free(gsl_vector_work);
	gsl_matrix_free(matrix_a1);
	gsl_matrix_free(matrix_v);
	gsl_matrix_free(matrix_X);

	return(0);

}