/*---------------------------------------------------------------------------

  File name    : isaac_cal.c
  Authors      : Yves Jung
  Created on   : 17 Dec. 1998
  Description  : Wavelength calibration for ISAAC
  Notice       :

  		1d polynomial fitting routines have been added to this code.
		They really belong to another module, but to keep this source a
		stand-alone utility, they have been included directly, together
		with matrix computation code. Please do not try to separate
		them from the main source!
  
  About this program
  compile with:
  cc -o isaac_cal isaac_cal.c -lm
   
  *--------------------------------------------------------------------------*/

/*
  
  $Id: isaac_cal.c,v 1.7 2001/06/27 07:52:19 ndevilla Exp $
  $Author: ndevilla $
  $Date: 2001/06/27 07:52:19 $
  $Revision: 1.7 $
    
  */

/*---------------------------------------------------------------------------
  Includes
  ---------------------------------------------------------------------------*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <math.h>


/*---------------------------------------------------------------------------
  Defines
  ---------------------------------------------------------------------------*/

#define NAME_SZ     1024
#define null_mx(m) ((m) == NULL)


/*----------------------------------------------------------------------------
 * 								New Types
 *--------------------------------------------------------------------------*/


typedef struct _MATRIX_ {
	    double *m;
	    int nr;
	    int nc;
} matrix, *Matrix;

typedef struct _DPOINT_ {
	double	x ;
	double	y ;
} dpoint ;


/*---------------------------------------------------------------------------
  Function prototypes
  ---------------------------------------------------------------------------*/

static void usage(char * pname) ;
static char * strlwc(char * s) ;
static double *
isaac_physical_model(
    double      lambda_c,
    char    *   objective,
    char    *   resolution,
    int         nbpix
) ;

static void output_disp_rel(
    double  *   list,
    int         np,
    char    *   outname
) ;

static double *
fit_1d_poly(
	int			poly_deg,
	dpoint	*	list,
	int			np,
	double	*	mean_squared_error
);

static double ipow(double x, int i);
static Matrix create_mx(int nr, int nc) ;
static Matrix copy_mx(Matrix a) ;
static void close_mx(Matrix a) ;
static Matrix mul_mx(Matrix a, Matrix b) ;
static Matrix invert_mx(Matrix aa) ;
static Matrix transp_mx(Matrix a) ;
static int gauss_pivot(double *ptra, double *ptrc, int n) ;
static Matrix least_sq_mx(Matrix A, Matrix B) ;


/*---------------------------------------------------------------------------
  Global variables (flags)
  ---------------------------------------------------------------------------*/

static char prog_desc[] = "physical model of ISAAC dispersion relation" ;
static int  debug_flag      = 0 ;
static int  verbose_flag    = 0 ;

static char rcsId[] = "$Id: isaac_cal.c,v 1.7 2001/06/27 07:52:19 ndevilla Exp $" ;


/*---------------------------------------------------------------------------
  main()  
  ---------------------------------------------------------------------------*/


int main(int argc, char *argv[])
{
	double    lambda_c;
	char      objective[3];
	char      resolution[3];
	char   *  outname ;
	int       nbpix ;
	int		  i ;

	double *  disp_rel ;
	double *  coeffs ;
	dpoint *  plist ;
	double	  mse ;
	int		  poly_deg ;

	lambda_c = 15000.0 ;
	strcpy(objective, "l1") ;
	strcpy(resolution, "lr") ;
	outname = NULL ;
	nbpix = 1024 ;
	poly_deg = -1 ;

	if (argc<2) {
		usage(argv[0]);
	}

	/* Handle command-line options */
	i=1 ;
	while (i<argc) {
		/* --version: print version and exit */
		if (!strcmp(argv[i], "--version")) {
			fprintf(stderr, "version Id:\n") ;
			fprintf(stderr, "%s\n", rcsId) ;
			return 0 ;
		} else if (!strcmp(argv[i], "-d")) {
			debug_flag=1 ;
		} else if (!strcmp(argv[i], "-v")) {
			verbose_flag=1 ;
		} else if (!strcmp(argv[i], "-h")) {
			usage(argv[0]) ;
		} else if (!strcmp(argv[i], "-l")) {
			i++ ;
			if (i>=argc) {
				fprintf(stderr, "error: option -l requires an argument\n");
				return -1 ;
			}
			sscanf(argv[i], "%lg", &lambda_c) ;
			if (lambda_c < 0 || lambda_c > 100000) {
				fprintf(stderr, "illegal value for lambda_c: %g\n", lambda_c) ;
				return -1 ;
			}
		} else if (!strcmp(argv[i], "-n")) {
			i++ ;
			if (i>=argc) {
				fprintf(stderr, "error: option -i requires an argument\n");
				return -1 ;
			}
			nbpix = (int)atoi(argv[i]) ;
			if (nbpix<1) {
				fprintf(stderr, "illegal value for nbpix: %d\n", nbpix);
				return -1 ;
			}
		} else if (!strcmp(argv[i], "-o")) {
			i++ ;
			if (i>=argc) {
				fprintf(stderr, "error: -o option requires an argument\n");
				return -1 ;
			}
			strncpy(objective, argv[i], 2) ;
			strcpy(objective, strlwc(objective)) ;
			if (objective[0]!='l' && objective[0]!='s') {
				fprintf(stderr, "illegal name for objective: %s\n", objective) ;
				return -1 ;
			}
			if (objective[1]!='1' && objective[1]!='2' && objective[1]!='3') {
				fprintf(stderr, "illegal name for objective: %s\n", objective) ;
				return -1 ;
			}
		} else if (!strcmp(argv[i], "-p")) {
			i++ ;
			if (i>=argc) {
				fprintf(stderr, "error: -p option requires an argument\n");
				return -1 ;
			}
			poly_deg = (int)atoi(argv[i]) ;
			if (poly_deg<1 || (poly_deg>100)) {
				fprintf(stderr, "wrong polynomial degree: %d\n", poly_deg) ;
				return -1 ;
			}
		} else if (!strcmp(argv[i], "-r")) {
			i++ ;
			if (i>=argc) {
				fprintf(stderr, "error: -r option requires an argument\n");
				return -1 ;
			}
			strncpy(resolution, argv[i], 2) ;
			strcpy(resolution, strlwc(resolution)) ;
			if (resolution[0]!='l' && resolution[0]!='m') {
				fprintf(stderr, "illegal resolution: %s\n", resolution) ;
				return -1 ;
			}
		} else if (argv[i][0]=='-') {
			fprintf(stderr, "illegal option: %s\n", argv[i]) ;
			usage(argv[0]) ;
		} else {
			outname = argv[i] ;
		}
		i++ ;
    }
    disp_rel = isaac_physical_model( lambda_c,
                                     objective,
                                     resolution,
                                     nbpix);
	if (poly_deg<0) {
		if (disp_rel == NULL) {
			fprintf(stderr, "cannot compute dispersion relation: aborting\n");
			return -1 ;
		}
		output_disp_rel(disp_rel, nbpix, outname) ;
		free(disp_rel) ;
	} else {
		/*
		 * build up list of points and pass it to fit_1d_poly()
		 */
		plist = malloc(nbpix * sizeof(dpoint)) ;
		for (i=0 ; i<nbpix ; i++) {
			plist[i].x = (double)i ;
			plist[i].y = disp_rel[i] ;
		}
		free(disp_rel) ;
		coeffs = fit_1d_poly(poly_deg, plist, nbpix, &mse) ;
		free(plist) ;
		printf("%d ", poly_deg) ;
		for (i=0 ; i<poly_deg+1 ; i++) {
			printf("%g ", coeffs[i]) ;
		}
		printf("\n") ;
		free(coeffs) ;
	}

    return 0 ;
}


/*---------------------------------------------------------------------------
  Function codes
 ---------------------------------------------------------------------------*/

  

/*

 Physical model of ISAAC dispersion block. This module return the
 wavelength for all detector pixels given the instrument configuration.
 All optical configuration values are hardcoded here (!) but the module
 is stand alone.
       
       QucContinuousFunction1D IsaacSkyProjection(
       double lambda_c,  /central wavelength in angstroms 
       RWCString res,         / resolution (low, medium)
       RWCString objective,     / objective (S1,S2,L1,L2,L3)
       double *wlow,     / Low wavelength
       double *wupp,     / High wavelength
       double *skyp,     / Sky projection
       int    *pref_order
       )
       {
*/


/*
 * ISAAC configuration
 */

#define ISAAC_LR_DIR                 7.0    /* angle in degrees */
#define ISAAC_MR_DIR                31.5    /* angle in degrees */

#define ISAAC_LR_GRATING            40.0    /* grooves/mm */
#define ISAAC_MR_GRATING            210.0   /* grooves/mm */

#define ISAAC_FOCAL_LENGTH_MM       175.0   /* mm */

#define ISAAC_PIXEL_SIZE_S          18.5    /* microns */
#define ISAAC_PIXEL_SIZE_M          27.0    /* microns */

#define ISAAC_BEAM_DIFF             2.72    /* angle in degrees */
#define ISAAC_PUPIL_SIZE_MM         100.0   /* mm */

#define ISAAC_FLGTH_S1              1.75
#define ISAAC_FLGTH_S2              3.25
#define ISAAC_FLGTH_L1              1.56
#define ISAAC_FLGTH_L2              4.77
#define ISAAC_FLGTH_L3              9.88

#define ANGLE_IN_DEFAULT			0.00
#define ANGLE_OUT_DEFAULT			0.00

static double *
isaac_physical_model(
    double      lambda_c,
    char    *   objective,
    char    *   resolution,
    int         nbpix
)
{

  double beam_diff ;
  double focal_length, pixel_size, pupil;
  double gr; 
  double gr_dir;
  double isaacLRdir = ISAAC_LR_DIR ;
  double isaacMRdir = ISAAC_MR_DIR ; 
  
  double isaacLRGrating = ISAAC_LR_GRATING ; 
  double isaacMRGrating = ISAAC_MR_GRATING ;
  int order;
  double * disp_rel;
  double x;
  double angle_in;
  double angle_out;
  double wsl, wsu, wsm;
  int i;

  double a,b,det;


  /* This module determines the dispersion relation of ISAAC for the
     different configurations of objectives, gratings, detectors, and 
     central wavelengths
   */ 
  
  /* Now reading configuration, low- or medium-resolution, 
     and central wavelength, to determine, order number, grating setting,
     wavelength range.
     */

    
  /* assumed Optical configuration:
     
     Focal lens of objectives for short- and long-wavelength objective:
     
     (Values at 77 Kelvin).
     S1=f/1.75 S2=f/3.25 L1=f/1.56 L2=f/4.77 L3=f/9.98
     Pupil size: 100 mm
     Pixel size: 18.5 microns (SW), 27 microns (LW)
     Gratings: low resolutions, 40 gr/mm, entering at about 5 degrees
     medium resol.,  210 gr/mm, entering at about 23 degrees
     Beam difference: 2.72 degrees
     
     Test: L2, medium res., wave=21200 A, pixel 27 microns,  
     The module computes: 20574.2,  21824.5,  1.22103, order 2
     Lab measure: 1.28 A/pixel measured with pixels 30 microns.
     
     Test: S1, low res., wave=12500 A, pixel 18.5 microns
     The module computes: 9131.32,  15864.9,  6.57576, order 4
       */
  
  /* Syntax: test <central_wave_angst> <low/medium> <s1/s2/l1/l2/l3> */
  
  focal_length = ISAAC_FOCAL_LENGTH_MM ;
  pixel_size   = ISAAC_PIXEL_SIZE_S ;
  beam_diff    = ISAAC_BEAM_DIFF ; 
  pupil        = ISAAC_PUPIL_SIZE_MM ; 

  disp_rel = malloc(nbpix * sizeof(double)) ;
  if (disp_rel == NULL) {
      fprintf(stderr, "memory allocation failure: aborting\n") ;
      return NULL ;
  }
  
  if (objective[0] == 's') {
    if (objective[1] == '1') focal_length = pupil*ISAAC_FLGTH_S1 ;
    if (objective[1] == '2') focal_length = pupil*ISAAC_FLGTH_S2 ;
    pixel_size = ISAAC_PIXEL_SIZE_S ; 
  }
    
  if (objective[0] == 'l') {
    if (objective[1] == '1') focal_length = pupil*ISAAC_FLGTH_L1;
    if (objective[1] == '2') focal_length = pupil*ISAAC_FLGTH_L2;
    if (objective[1] == '3') focal_length = pupil*ISAAC_FLGTH_L3;
    pixel_size = ISAAC_PIXEL_SIZE_M ;
  }

  focal_length *= 1e-3; /* Convert mm to m. */
  
  /* Display configuration */
  
  if (verbose_flag) {
      fprintf(stderr, "configuration: \n") ;
      fprintf(stderr, "resolution   : ") ;
      if (resolution[0]=='l') fprintf(stderr, "low\n") ;
      if (resolution[0]=='m') fprintf(stderr, "medium\n") ;
      fprintf(stderr, "lambda_c     : %g\n", lambda_c) ;
      fprintf(stderr, "objective    : %s\n", objective) ;
      fprintf(stderr, "focal length : %g\n", focal_length) ;
  }


  /* Will be eventually loaded from the IDF */
  
  gr=0 ;
  if (resolution[0] == 'l') {
	  gr = isaacLRGrating; gr_dir = isaacLRdir;
  } else if (resolution[0] == 'm') {
	  gr = isaacMRGrating; gr_dir = isaacMRdir;
  } else {
	  fprintf(stderr, "wrong grating!\n") ;
	  return NULL ;
  }
  
  gr *= 1e-6; /* To have gr in groove/nm. */
  beam_diff *= M_PI/180.0; /* Convert beam_diff in radians. */
  
  
	/*
	 * 40 gr/mm --> 5. degrees input
	 * 210 gr/mm -->  23. degrees 
	 */

  /* lambda_c A -> nm */
  lambda_c /= 10 ;

  
    if ( lambda_c >= 890 && lambda_c < 8000) {
        if ( lambda_c >= 890 && lambda_c < 990 ) {
            order = 6;
        } else if ( lambda_c >= 990 && lambda_c < 1100 ) {
            order = 5;
        } else if ( lambda_c >= 1100 && lambda_c < 1400 ) {
            order = 4;
        } else if ( lambda_c >= 1400 && lambda_c < 1850 ) {
            order = 3;
        } else if ( lambda_c >= 1850 && lambda_c < 2500 ) {
            order = 2;
        } else if ( lambda_c >= 2500 && lambda_c < 5500 ) {
            order = 1;
        } else {
            order = 1;
        }
        if (debug_flag) {
            fprintf(stderr, "chosen order : %d\n", order) ;
        }
    } else {
		angle_in = ANGLE_IN_DEFAULT ;
		angle_out = ANGLE_OUT_DEFAULT ;
        order = (int)(0.5+(sin(angle_in)+sin(angle_out))/(lambda_c*gr));
        if (debug_flag) {
            fprintf(stderr, "computed order : %d\n", order) ;
        }
    }
  
    if (verbose_flag) {
      fprintf(stderr, "order    : %d\n", order) ;
    }


    /* The following is the solution of the set of equations :
         (1)  sin(angle_in) + sin(angle_out) = order*gr*lambda_c 
         (2)  angle_out - angle_in = beam_diff
	   for the two angles angle_in and angle_out. The system has two
	   solutions, only one is retained.
    */

  det = 2*sin(beam_diff);
  a   = order*gr*lambda_c*sin(beam_diff);
  b   = sqrt(order*gr*lambda_c*order*gr*lambda_c*
             (sin(beam_diff)*sin(beam_diff)+2*cos(beam_diff)-2) 
             + 2*sin(beam_diff)*sin(beam_diff)*(1-cos(beam_diff)));

  angle_in = asin((a-b)/det);
  angle_out = angle_in + beam_diff;

  /* 
    fprintf(stderr, "angles: %g %g\n",in*180./M_PI,out*180./M_PI);
    fprintf(stderr, "test central wav.: %g\n",(sin(in)+sin(out))/order/gr);
  */

  for (i=0; i<nbpix;i++) {
    x = (double)(i-(nbpix/2.)) * pixel_size * 1e-6; /* x is in meters */
	disp_rel[i] =
		(sin(angle_in)+sin(angle_out+atan(x/focal_length)))/(order*gr);
  
  	/* Convert disprel from nm to angstroms */
  	disp_rel[i] *= 10 ;
  
  }
  
  wsl = disp_rel[0];
  wsu = disp_rel[nbpix-1];
  wsm = (wsl+wsu)/2.;
  
  if (verbose_flag) {
      fprintf(stderr, "central wavelength: %g A\n", wsm) ;
      fprintf(stderr, "wave band: [%g %g] (angstroms)\n", wsl, wsu) ;
      fprintf(stderr, "average dispersion: %g A/pixel\n",
			  (wsu-wsl)/(double)nbpix) ;
  }

  return disp_rel ;
}


/*---------------------------------------------------------------------------
  Function :    output_disp_rel()
  In       :    double array, # of elements in array, output name
  Out      :    list of doubles to ASCII file
  Job      :    output a list of doubles to an ASCII file
  Notice   :    if outname is NULL, output is done to stdout
  ---------------------------------------------------------------------------*/

static
void output_disp_rel(
    double  *   list,
    int         np,
    char    *   outname
)
{
    FILE *  out ;
    int     i ;

	out = NULL ;
    if (outname!=NULL) {
        out = fopen(outname, "w") ;
	}
	if (out == NULL) {
		out = stdout ;
	}
    for (i=0 ; i<np ; i++) {
        fprintf(out, "%d\t%g\n", i, list[i]) ;
    }

    if (out!=stdout) {
        fclose(out) ;
    }
    return ;
}


/*---------------------------------------------------------------------------
   Function :   usage()
   In       :   string containing program name
   Out      :   message to stdout
   Job      :   printout usage message
   Notice   :
 ---------------------------------------------------------------------------*/

static void usage(char * pname)
{
    printf("\n\n") ;
    printf("*** %s\n", prog_desc) ;
    printf("\n") ;
    printf("use: %s [parameters] [options] [output name]\n", pname) ;
    printf("parameters are:\n") ;
	printf("\tcentral wavelength in nanometers\n") ;
	printf("\t  default is 1500 nm\n") ;
	printf("\t  change it with -l <lambda>\n") ;
	printf("\tobjective\n");
	printf("\t  supported are L1 L2 L3 S1 S2\n") ;
	printf("\t  default is 'L1'\n") ;
	printf("\t  change it with -o 'objective'\n") ;
	printf("\tresolution\n") ;
	printf("\t  supported are: LR MR\n") ;
	printf("\t  default is 'LR'\n") ;
	printf("\t  change it with -r 'resolution'\n") ;
	printf("\tnumber of (linear) pixels on the detector\n") ;
	printf("\t  default is 1024\n") ;
	printf("\t  change it with -n <nbpix>\n") ;
	printf("\n") ;
	printf("\t-p <degree> fits a 2d polynomial of given degree\n") ;
	printf("\tand returns (only) the coefficients as:\n") ;
	printf("\t-> degree c[0] c[1] ... c[degree]\n") ;
    printf("\n") ;
    printf("options are:\n") ;
	printf("\t-version to get version number\n") ;
    printf("\t-h for this help\n") ;
    printf("\t-v for verbose mode\n") ;
    printf("\t-d for debug mode\n") ;
    printf("\n\n") ;
    exit(1) ;
}



/*
  Function: strlwc
  Synopsis: Converts all alphabetic characters in string to lowercase,
  stopping at the final null character.  Returns string.  If string is
  null, returns null.  We do not call this function strlwr because that
  is already provided by some systems (but not by ANSI C).
  */ 

static char * strlwc (char *string)
{
  char *scan;
  
  if (string) {
    scan = string;
    while (*scan) {
      *scan = (char) tolower (*scan);
      scan++;
    }
  }
  return string ;
}


/*---------------------------------------------------------------------------
   Function	:	fit_1d_poly()
   In 		:	requested polynomial degree
   				a list of pixel positions + number of pixels in the list
				(out) mean squared error, set to NULL if you do not want
				to compute it.
   Out 		:	newly allocated array containing fit coefficients
   Job		:	fit a polynomial to a list of pixel positions
   Notice	:
   				The fitted polynomial is such that:
				y = c[0] + c[1].x + c[2].x^2 + ... + c[n].x^n
				So requesting a polynomial of degree n will return n+1
				coefficients. Beware that with such polynomials, two
				input points shall never be on the same vertical!
 ---------------------------------------------------------------------------*/

static double *
fit_1d_poly(
	int			poly_deg,
	dpoint	*	list,
	int			np,
	double	*	mean_squared_error
)
{
	int			i, k ;
	Matrix		mA, mB, mX ;
	double	*	c ;
	double		err ;
	double		xp, y ;

	if (np<poly_deg+1) {
		fprintf(stderr,"not enough points") ;
		fprintf(stderr,"cannot fit %dth degree polynomial with %d points",
				poly_deg, np);
		return NULL;
	}

	mA = create_mx(poly_deg+1, np) ;
	mB = create_mx(1, np) ;

	for (i=0 ; i<np ; i++) {
		mA->m[i] = 1.0 ;
		for (k=1 ; k<=poly_deg ; k++) {
			mA->m[i+k*np] = ipow(list[i].x, k) ;
		}
		mB->m[i] = list[i].y ;
	}

	/*
	 * Solve XA=B by a least-square solution (aka pseudo-inverse).
	 */
	mX = least_sq_mx(mA,mB) ;
	/*
	 * Delete input matrices
	 */
	close_mx(mA) ;
	close_mx(mB) ;
	/*
	 * Examine result
	 */
	if (null_mx(mX)) {
		fprintf(stderr,"cannot fit: non-invertible matrix") ;
		return NULL ;
	}
	c = malloc((poly_deg+1)*sizeof(double)) ;
	for (i=0 ; i<(poly_deg+1) ; i++) {
		c[i] = mX->m[i] ;
	}
	close_mx(mX) ;

	/*
	 * If requested, compute mean squared error
	 */
	if (mean_squared_error != NULL) {
		err = 0.00 ;
		for (i=0 ; i<np ; i++) {
			y = c[0] ;
			/*
			 * Compute the value obtained through the fit
			 */
			for (k=1 ; k<=poly_deg ; k++) {
				xp = ipow(list[i].x, k) ;
				y += c[k] * xp ; 
			}
			/*
			 * Subtract from the true value, square, accumulate
			 */
			xp   = ipow(list[i].y - y, 2) ;
			err += xp ; 
		}
		/* Average the error term */
		err /= (double)np ;
		*mean_squared_error = err ;
	}
	return c ;
}




/*---------------------------------------------------------------------------
   Function	:	ipow()
   In 		:	double, int
   Out 		:	double
   Job		:	same as pow(x,y) but for integer values of y
   Notice	:	much faster than the math function due to the integer.
   				On some compilers, this optimization is already done in
				the pow() function, we do not rely on that fact.
 ---------------------------------------------------------------------------*/


static double ipow(double x, int i)
{
	double ret ;

	if (i==0) {
		ret = 1.00 ;
	} else if (i==1) {
		ret = x ;
	} else {
		ret = x ;
		while (--i) {
			ret *= x;
		}
	}
	return ret;
}


/*
 * Matrix code: left undocumented
 * litteral copy of eclipse/matrix.c and matrix.h
 */

#define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)


static Matrix
create_mx(int nr, int nc)
{
	Matrix b;
	b = (Matrix)calloc(1,sizeof(matrix));
	b->m = (double*)calloc(nr*nc,sizeof(double));
	b->nr= nr;
	b->nc= nc;
	return b;
}

static void
close_mx(Matrix a)
{
	if(null_mx(a)) return;
	if(a->m != NULL)
		free(a->m);
	free(a);
	return;
}

static Matrix
copy_mx(Matrix a)
{
    Matrix b = create_mx(a->nr,a->nc);
    if (null_mx(b)) return b; {
        register int s = a->nr*a->nc;
        register double *mm = b->m+s;
        register double *am = a->m+s;
        while (s--) *--mm = *--am;
    }
    return b;
}


static Matrix
mul_mx(Matrix a, Matrix b)
{
	Matrix c, d;
	int n1=a->nr, n2=a->nc, n3=b->nc;
	register double *a0;
	register double *c0;
	register double *d0;
	register int i,j,k;

	if(n2!=b->nr) return NULL;
	c = create_mx(n1,n3);
	d = transp_mx(b);

	for (i=0,c0=c->m;i<n1;i++)
		for (j=0,d0=d->m;j<n3;j++,c0++)
			for (k=0,*c0=0,a0=a->m+i*n2;k<n2;k++)
				*c0 += *a0++ * *d0++;
	close_mx(d);
	return c;
}

static Matrix
invert_mx(Matrix aa)
{
	Matrix bb;
	int test=1;

	if(aa->nr!=aa->nc) return NULL;
	bb = create_mx(aa->nr,aa->nc);

	if(aa->nr==1)
	{
		double det;
		register double ted;
		det= *(aa->m);
		if(dtiny(det)) test=0;
		ted=1./det;
		*(bb->m)=ted;
	}
	else if(aa->nr==2)
	{
		double det;
		register double ted;
		register double *mm=aa->m;
		double a= *(mm++),b= *(mm++);
		double c= *(mm++),d= *(mm);
		det=a*d-b*c;
		if(dtiny(det)) test=0;
		ted=1./det;
		mm=bb->m;
		*(mm++)= d*ted,*(mm++)= -b*ted;
		*(mm++)= -c*ted,*(mm)= a*ted;
	}
	else if(aa->nr==3)
	{
		double det;
		register double ted;
		register double *mm=aa->m;
		double a= *(mm++),b= *(mm++),c= *(mm++);
		double d= *(mm++),e= *(mm++),f= *(mm++);
		double g= *(mm++),h= *(mm++),i= *(mm);
		det=a*e*i-a*h*f-b*d*i+b*g*f+c*d*h-c*g*e;
		if(dtiny(det)) test=0;
		ted=1./det;
		mm=bb->m;
		*(mm++)=(e*i-f*h)*ted,*(mm++)=(c*h-b*i)*ted,*(mm++)=(b*f-e*c)*ted;
		*(mm++)=(f*g-d*i)*ted,*(mm++)=(a*i-g*c)*ted,*(mm++)=(d*c-a*f)*ted;
		*(mm++)=(d*h-g*e)*ted,*(mm++)=(g*b-a*h)*ted,*(mm)=(a*e-d*b)*ted;
	}
	else
	{
		Matrix temp=copy_mx(aa);
		if(gauss_pivot(temp->m,bb->m,aa->nr)==0) test=0;
		close_mx(temp);
	}
	if(test==0)
	{
		fprintf(stderr,"matrix::invert: not invertible, aborting inversion");
		return NULL;
	}
	return bb;
}


static Matrix
transp_mx(Matrix a)
{
	register int nc=a->nc, nr=a->nr;
	register double *a0;
	register double *b0;
	register int i,j;
	Matrix b = create_mx(nc,nr);

	if (b == (Matrix)NULL) return b ;
	for (i=0,b0=b->m;i<nc;i++)
		for (j=0,a0=a->m+i;j<nr;j++,a0+=nc,b0++)
			*b0 = *a0;
	return b;
}


static int
gauss_pivot(double *ptra, double *ptrc, int n)
/* c(n,n) = a(n,n)^-1 */
{
#define ABS(a) (((a) > 0) ? (a) : -(a))

	register int i,j,k,l;
	int maj;
	double max,r,t;
	double *ptrb;

	ptrb=(double *)calloc(n*n,sizeof(double));
	for(i=0;i<n;i++)
		ptrb[i*n+i]= 1.0;

	for (i=1;i <= n;i++)
	{
		/* Search max in current column  */
		max = ABS(*(ptra + n*i-n));
		maj = i;
		for (j = i;j <= n;j++)
			if (ABS(*(ptra+n*j+i-n-1)) > max)
			{
				maj = j;
				max = ABS(*(ptra+n*j+i-n-1));
			}

		/* swap lines i and maj */
		if (maj != i)
		{
			for (j = i;j <= n;j++)
			{
				r = *(ptra+n*maj+j-n-1);
				*(ptra+n*maj+j-n-1) = *(ptra+n*i+j-n-1);
				*(ptra+n*i+j-n-1) = r;
			}
			for(l=0;l<n;l++)
			{
				r = *(ptrb+l*n+maj-1);
				*(ptrb+l*n+maj-1) = *(ptrb+l*n+i-1);
				*(ptrb+l*n+i-1) = r;
			}
		}

		/* Subtract line by line */
		for (j = i + 1;j <= n;j++)
		{
			t = (*(ptra+(n+1)*i-n-1));
			if(dtiny(t)) return(0);
			r = (*(ptra+n*j+i-n-1)) / t;
			for(l=0;l<n;l++)
				*(ptrb+l*n+j-1) -= r * (*(ptrb+l*n+i-1));
			for (k = i;k <= n;k++)
				*(ptra+n*j+k-n-1) -= r * (*(ptra+n*i+k-n-1));
		}
	}

	/* Triangular system resolution	*/
	for(l=0;l<n;l++)
		for (i = n;i >= 1;i--)
		{
			t = (*(ptra+(n+1)*i-n-1));
			if(dtiny(t)) return(0);
			*(ptrc+l+(i-1)*n) = (*(ptrb+l*n+i-1)) / t;
			if (i > 1)
				for (j = i - 1;j > 0;j--)
					*(ptrb+l*n+j-1) -= (*(ptra+n*j+i-n-1)) * (*(ptrc+l+(i-1)*n));
		}
	free(ptrb);
	return(1);
}


static Matrix least_sq_mx(
	Matrix	A,
	Matrix	B
)
{
	Matrix	m1,
			m2,
			m3,
			m4,
			m5 ;



	m1 = transp_mx(A) ;
	m2 = mul_mx(A, m1) ;
	m3 = invert_mx(m2) ;
	m4 = mul_mx(B, m1) ;
	m5 = mul_mx(m4, m3) ;

	close_mx(m1) ;
	close_mx(m2) ;
	close_mx(m3) ;
	close_mx(m4) ;

	return m5 ;
}