#include "machdefs.h"
#include "peida.h"
#include "linfit.h"
#include "matrix.h"
#include "cvector.h"

double LinFit(const OVector& x, const OVector& y, int nf, double (*f)(int, double), 
	   OVector& c)
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  
  OMatrix fx(nf, n);
  for (int i=0; i<nf; i++)
    for (int j=0; j<n; j++)
      fx(i,j) = f(i,x(j));

  return LinFit(fx,y,c);
}



double LinFit(const OMatrix& fx, const OVector& y, OVector& c)
{
  int n = y.NElts();
  if ( n != fx.NCol()) THROW(sizeMismatchErr);

  int nf = fx.NRows();

  OMatrix a(nf,nf);

  for (int j=0; j<nf; j++)
    for (int k=j; k<nf; k++)
      a(j,k) = a(k,j) = fx.Row(j) * fx.Row(k);
   
  c = fx * y;

  if (OMatrix::GausPiv(a,c) == 0) THROW(singMatxErr);

  double xi2 = 0;

  for (int k=0; k<n; k++) {
    double x = 0;
    for (int i=0; i<nf; i++)
      x += c(i) * fx(i,k);
    x -= y(k);
    xi2 += x*x;
  }
  return xi2;
}



double LinFit(const OVector& x, const OVector& y, const OVector& errY2, int nf,
	   double (*f)(int, double), OVector& c, OVector& errC)
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  
  OMatrix fx(nf, n);
  for (int i=0; i<nf; i++)
    for (int j=0; j<n; j++)
      fx(i,j) = f(i,x(j));

  return LinFit(fx,y,errY2,c,errC);
}


double LinFit(const OMatrix& fx, const OVector& y, const OVector& errY2,
	   OVector& c, OVector& errC)
{
  int n = y.NElts();
  if ( n != errY2.NElts()) THROW(sizeMismatchErr);
  if ( n != fx.NCol()) THROW(sizeMismatchErr);

  int nf = fx.NRows();

  OMatrix a(nf,nf);

  c.Realloc(nf);
  errC.Realloc(nf);

  for (int j=0; j<nf; j++)
    for (int k=j; k<nf; k++) {
      double x=0;
      for (int l=0; l<n; l++)
	x += fx(j,l) * fx(k,l) / errY2(l);        // Matrice a inverser
      a(j,k) = a(k,j) = x;
    }
 
  OMatrix d(nf,nf+1);
  for (int k=0; k<nf; k++) {
    double x=0;
    for (int l=0; l<n; l++)
      x += fx(k,l) * y(l) / errY2(l);             // Second membre 1ere colonne
    d(k,0) = x;
    for (int m=1; m<=nf; m++)
      d(k,m) = (k==m) ? 1 : 0;                // Reste second membre = Id.
  }

  if (OMatrix::GausPiv(a,d) == 0) THROW(singMatxErr);

  for (int l=0; l<nf; l++) {
    c(l) = d(l,0);                            // Parametres = 1ere colonne
    errC(l) = d(l,l+1);                        // Erreurs = diag inverse.
  }

  double xi2 = 0;

  for (int jj=0; jj<n; jj++) {
    double x = 0;
    for (int ii=0; ii<nf; ii++)
      x += c(ii) * fx(ii,jj);
    x -= y(jj);
    xi2 += x*x/errY2(jj);
  }
  return xi2;
}