/*
 *	From: 	  jt@avidya.ifa.hawaii.edu
 *	Subject: 	antivec.c
 *      Date: 	March 2, 2005 17:05:31 EST
 *      To: 	  wmwood-vasey@cfa.harvard.edu
 */

/* antivec.c */
/* Fits an antisymmetric matrix as a vector term difference:
 *        A[i][j] = V[i] - V[j]
 * Inputs are a difference matrix, an error matrix,
 * the number of terms, and it produces the individual values
 * The first term is set to zero.
 */
/* Rev 2.0 John Tonry 030322 - major hacks on args/coding.  Same function */
/* Rev 1.0 Megan Novicki c. Jul 2000 */

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include<string.h>
#include<strings.h>

#define MIN(a,b) (((a) < (b)) ? (a) : (b))
#define MAX(a,b) (((a) > (b)) ? (a) : (b))
#define ABS(a) (((a) > 0) ? (a) : -(a))
#define NINT(x) (x<0?(int)((x)-0.5):(int)((x)+0.5))
#define SQRT(x) ( ((x) >= 0) ? sqrt(x) : -sqrt(-(x)))

static double *cov=NULL, *y;
static int nalloc;

/* #define EXTRAWGT		/* This is to deemphasize very low error points */
                                /* but probably is better done by the caller */
/* #define SEECOV		/* Debug listing of covariance matrix? */

antivec(int n, 			/* Number of terms */
	double *a, 		/* Antisymmetric nxn matrix */
	double *e, 		/* Uncertainties of terms in matrix */
	double *v, 		/* Resulting vector */
	double *dvext, 		/* External uncertainty in vector */
	double *dvint)		/* Internal uncertainty in vector */
{
   int i, j, k, stat;
   double err, wgt, det, chi, dof, wgtave;

   dof = (n*(n-1))/2 - (n-1);

/* Estimate an average weight */
   for(j=0, wgtave=0.0; j<n; j++) {
      for(i=0; i<j; i++) wgtave += 1/(e[i+n*j]*e[i+n*j]);
   }
   wgtave /= ((n*(n-1))/2);

/* Allocate some space for a matrix workspace */
   if(cov == NULL || n > nalloc) {
      if(cov != NULL) {
	 free(cov);
	 free(y);
      }
      cov = (double *)calloc(n*n, sizeof(double));
      y = (double *)calloc(n, sizeof(double));
      nalloc = n;
   }

#ifdef SEECOV
   printf("Incoming error matrix:\n");
   for(j=0; j<n; j++)
      for(i=0; i<n; i++) printf("%7.3f%c", e[i+n*j], i==n-1?'\n':' ');
#endif

/* Evaluate the uncertainties for V from supplied matrix of uncertainties */
/* cov[j][i] = 1 off diagonal and N-1 on diag;  y[i] = Sum(j) e[j][i]^2) */
   for(j=0; j<n; j++) {
      for(i=0, y[j]=cov[j+n*j]=0.0; i<n; i++) {
// Old version without weights
//	 cov[i+n*j] = (i==j) ? n-1 : 1;	/* Megan has N, which is a bug (?) */
#ifdef  EXTRAWGT
/* This inclusion of wgtave is completely ad hoc.  I want to deemphasize
 * negative fluctuations in the error estimates */
	 wgt = (e[i+n*j] != 0 && i !=j ) ? 1/(e[i+n*j]*e[i+n*j]+1/wgtave) : 0;
#else
/* This is what the math calls for */
	 wgt = (e[i+n*j] != 0 && i != j) ? 1/(e[i+n*j]*e[i+n*j]) : 0;
#endif

	 cov[j+n*j] += wgt*wgt;
	 if(i != j) {
	    cov[i+n*j] = wgt*wgt;
	    y[j] += wgt;
	 }
      }
   }

#ifdef SEECOV
   printf("External weight matrix:\n");
   for(j=0; j<n; j++)
      for(i=0; i<n; i++) printf("%7.3f%c", cov[i+n*j], i==n-1?'\n':' ');
#endif

   if( (stat=invert(n, cov, &det) != 0)) {
      fprintf(stderr, "Error at DVEXT eval: invert returned %d\n", stat);
      return(-1);
   }
   for(j=0; j<n; j++) {
      for(i=0, dvext[j] = 0.0; i<n; i++) dvext[j] += y[i] * cov[i+n*j];
#ifdef  EXTRAWGT
/* This removal of wgtave is completely ad hoc */
      dvext[j] = SQRT(dvext[j]-0.5/wgtave);
#else
/* This is what the math calls for */
      dvext[j] = SQRT(dvext[j]);
#endif
   }

/* Calculate the best-fit vector to the antisymmetric matrix */
/* cov[j][i] = 1/e[j+1][i+1]^2 off diagonal, and Sum(i!=j) -1/(e*e) on diag */
/* y[i] = Sum(j) a[j][i+1]/e[j][i+1]^2) */
/* Add in a factor of (Sum(j) v(j))^2 to chi^2 to keep it non-singular (and
 * force the Sum(j) v(j) = 0)
 */
   for(i=0; i<n*n; i++) cov[i] = wgtave;
   for(i=0; i<n*n; i++) cov[i] = 1.0;
   for(j=0; j<n; j++) {
      for(i=0, y[j]=0.0; i<n; i++) {
	 wgt = (e[i+n*j] != 0 && i != j) ? 1/(e[i+n*j]*e[i+n*j]) : 0;
	 cov[i+n*j] += wgt;
	 cov[j+n*j] -= wgt;	/* Add and sub cancel when i=j */
	 y[j] += a[j+n*i] * wgt;
      }
   }

/* Solve and multiply out to evaluate v */
#ifdef SEECOV
   printf("Hessian matrix: <w> = %.3f\n", wgtave);
   for(j=0; j<n; j++)
      for(i=0; i<n; i++) printf("%7.3f%c", cov[i+n*j], i==n-1?'\n':' ');
#endif

   if( (stat=invert(n, cov, &det) != 0)) {
      fprintf(stderr, "Error at V eval: invert returned %d\n", stat);
      return(-1);
   }

   for(j=0; j<n; j++) {
      for(i=0, v[j]=0.0; i<n; i++) v[j] += y[i] * cov[i+n*j];
   }

/* Evaluate the consistency for V from the covariance matrix now in cov */
   for(j=0; j<n; j++) {
      dvint[j] = cov[j+n*j] = SQRT(-cov[j+n*j]);
      for(i=0; i<j; i++) {
	 cov[i+n*j] = cov[j+n*i] = -cov[i+n*j] /(cov[i+n*i] * cov[j+n*j]);
      }
   }

/* Collect chi^2 from the residual matrix */
   for(j=0, chi=0.0; j<n; j++) {
      for(i=j+1; i<n; i++) {
	 chi += (a[i+n*j] - (v[j]-v[i])) * (a[i+n*j] - (v[j]-v[i])) /
	    (e[i+n*j] * e[i+n*j]);
      }
   }

/* Adjust the balance between external and internal error */
/* We assume that some fraction of the error matrix comes from "external 
 * error", i.e. errors in the vector values which are consistent between
 * all the difference measurements, hence invisible to the antisymmetric
 * matrix, and the rest comes from "internal error" which arose from the
 * process of getting the terms of the antisymmetric matrix.  The former
 * errors should be analyzed using the formalism for the "dvext" and the
 * latter using the "dvint" formalism.  Roughly speaking the dvext errors
 * are sqrt(2) smaller than the e terms and the dvint errors are sqrt(1/N)
 * smaller than the e terms.  We adjust the fraction so that chi/N = 1.
 * We expect normally that chi/N < 1, i.e. the difference vector matches
 * the antisymmetric matrix quite well, and the uncertainties in the e 
 * vector are mainly arising from external uncertainties in v.  However,
 * there are at least two ways to partition things.
 *
 * Case 1: we take the e matrix at face value as telling us about the 
 * external errors, use the full thing for the dvext, and scale it as much
 * as necessary for dvint:
 *
 *			dvext -> dvext
 *			dvint -> dvint * sqrt(chi/N)
 *
 * Case 2: we partition the e matrix into an internal piece and an external
 * piece, with the internal piece adjusted to make chi/N = 1 and the
 * external piece having a floor of zero.
 *
 *			dvext -> dvext * MAX(0, 1-sqrt(chi/N))
 *			dvint -> dvint * sqrt(chi/N)
 *
 * I'm dubious about Case 2, so I'm just going to report things as Case 1.
 * Users who may have error matrices which include a large piece of 
 * internal matrix error (instead of external vector error) may want to
 * rescale the dvext and dvint before adding in quadrature to get a final
 * uncertainty.
 */

   for(j=0; j<n; j++) dvint[j] *= sqrt(chi/dof);

#ifdef SEECOV
   printf("Error matrix:\n");
   for(j=0; j<n; j++)
      for(i=0; i<n; i++) printf("%7.3f%c", cov[i+n*j], i==n-1?'\n':' ');
#endif

   return(0);
}

/* Wrapper for fortran */
antivec_(int *n, 			/* Number of terms */
	double *a, 		/* Antisymmetric nxn matrix */
	double *e, 		/* Uncertainties of terms in matrix */
	double *v, 		/* Resulting vector */
	double *dvext, 		/* External uncertainty in vector */
	double *dvint)		/* Internal uncertainty in vector */
{
   return( antivec(*n, a, e, v, dvext, dvint) );
}