/**********************************************************************
 *
 * File: lfit.c (22-Apr-2008)
 *
 * Lineares Ausgleichsproblem mit Fitfunktion der Form
 *
 *            _m_
 *           \
 *    y(x) = /___ c(k)*phi_k(x)
 *            k=1
 *
 * auf dem Intervall [0,1], wo
 *
 *   phi_k(x) = x*(1-x)*(2*x-1)^(k-1)
 *
 * Loesung des linearen Gleichungssystems fuer Fitparameter mit
 * Gauss'scher Elimination und partieller Pivotierung
 *
 * m..............Anzahl der Fitparameter c_k
 * lfit_in.dat....Eingabefile mit zu fittenden Daten (enthaelt in erster
 *                Zeile Anzahl der Datenpunkte n)
 *
 * nx.............Anzahl der Datenpunkte fuer Darstellung der gefitteten
 *                Funktion im Intervall [0,1]
 * lfit_out.dat...Ausgabefile mit gefitteter Funktion
 *
 **********************************************************************/

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

#define EPS 1.0e-08

/**********************************************************************/

double phi(int k,double x) {

/**********************************************************************/

  /* Basisfunktionen fuer Fit */

  if(k==0) {
    return x*(1.0-x);
  } else {
    return x*(1.0-x)*pow(2.0*x-1.0,k);
  }

}

/**********************************************************************/

void gaussp(double **a_in,double *b,double *x,int n) {

/**********************************************************************/

  int i,itemp,j,k,m;
  int *iperm;
  double pivot,pmax;
  double **a;

  /* Kopie der Koeffizientenmatrix a_in und der rechten Seite b
     in EINE n*(n+1) Matrix a (d.h. a_in wird nicht ueberschrieben) */

  a=(double**)malloc(n*sizeof(double*));

  for(i=0;i<=n-1;i++) {
    a[i]=(double*)malloc((n+1)*sizeof(double));
  }

  for(i=0;i<=n-1;i++) {
    for(j=0;j<=n-1;j++) {
      a[i][j]=a_in[i][j];
    }
    a[i][n]=b[i];
  }

  /* Permutationsvektor initialisieren */

  iperm=(int*)malloc(n*sizeof(int));

  for(i=0;i<=n-1;i++) {
    iperm[i]=i;
  }

  for(i=0;i<n-1;i++) {

    /* Maximales Pivotelement in Spalte i suchen */

    m=0;
    pmax=0.0;
    for(j=i;j<=n-1;j++) {
      if(fabs(a[iperm[j]][i])>pmax) {
	m=j;
	pmax=fabs(a[iperm[j]][i]);
      }
    }
    if(pmax<EPS) {
      printf(" pmax<eps\n");
      exit(1);
    }

    /* Zeilen (nur Indizes!) vertauschen */

    if(m!=i) {
      itemp=iperm[i];
      iperm[i]=iperm[m];
      iperm[m]=itemp;
    }

    /* Elimination */

    pivot=a[iperm[i]][i];
    for(k=i+1;k<=n-1;k++) {
      for(j=i+1;j<=n;j++) {
	a[iperm[k]][j]=a[iperm[k]][j]-a[iperm[i]][j]*a[iperm[k]][i]/pivot;
      }
    }

  }

  /* Ruecksubstitution */
  
  if(fabs(a[iperm[n-1]][n-1])<EPS) {
    printf(" |a[n-1][n-1]|<eps\n");
    exit(1);
  }
  
  x[n-1]=a[iperm[n-1]][n]/a[iperm[n-1]][n-1];
  for(i=n-2;i>=0;i--) {
    x[i]=a[iperm[i]][n];
    for(j=i+1;j<=n-1;j++) {
      x[i]=x[i]-a[iperm[i]][j]*x[j];
    }
    x[i]=x[i]/a[iperm[i]][i];
  }

  free(a);
  free(iperm);

  return;

}

/**********************************************************************/

int main() {

/**********************************************************************/

  FILE *fpi,*fpo;
  int i,j,k,m,n,nx;
  double chi2,dx,xfit,yfit;
  double *b,*c,*sig,*x,*y;
  double **a;

  /* Anzahl Fitparameter & Punkte Ausgabefile */

  printf(" m,nx=");
  scanf("%d,%d",&m,&nx);

  /* Daten einlesen */

  fpi=fopen("lfit_in.dat","r");

  fscanf(fpi,"%d",&n);

  a=(double**)malloc(m*sizeof(double*));
  for(i=0;i<=m-1;i++) {
    a[i]=(double*)malloc(m*sizeof(double));
  }

  b=(double*)malloc(m*sizeof(double));
  c=(double*)malloc(m*sizeof(double));
  sig=(double*)malloc(n*sizeof(double));
  x=(double*)malloc(n*sizeof(double));
  y=(double*)malloc(n*sizeof(double));

  for(i=0;i<=n-1;i++) {
    fscanf(fpi,"%lf %lf %lf",&x[i],&y[i],&sig[i]);
  }

  fclose(fpi);

  /* Koeffizienten des linearen Gleichungssystems berechnen */

  for(i=0;i<=m-1;i++) {
    b[i]=0.0;
    for(k=0;k<=n-1;k++) {
      b[i]=b[i]+phi(i,x[k])*y[k]/(sig[k]*sig[k]);
    }
    for(j=0;j<=m-1;j++) {
      a[i][j]=0.0;
      for(k=0;k<=n-1;k++) {
	a[i][j]=a[i][j]+phi(i,x[k])*phi(j,x[k])/(sig[k]*sig[k]);
      }
    }
  }

  /* Gleichungssystem loesen */

  gaussp(a,b,c,m);

  /* Werte der Fitparamter */

  for(k=1;k<=m;k++) {
    printf(" c_k, k=%3d: %12.5e\n",k,c[k-1]);
  }

  /* Reduziertes chi^2 */

  chi2=0.0;
  for(i=0;i<=n-1;i++) {
    yfit=0.0;
    for(k=1;k<=m;k++) {
      yfit=yfit+c[k-1]*phi(k-1,x[i]);
    }
    chi2=chi2+((yfit-y[i])/sig[i])*((yfit-y[i])/sig[i]);
  }

  printf(" chi2_red=%12.5e\n",chi2/(n-m));

  /* Fit auf File schreiben */

  fpo=fopen("lfit_out.dat","w");

  dx=1.0/nx;

  for(i=0;i<=nx;i++) {

    xfit=i*dx;
    yfit=0.0;
    for(k=1;k<=m;k++) {
      yfit=yfit+c[k-1]*phi(k-1,xfit);
    }

    fprintf(fpo," %7.5lf %12.5e\n",xfit,yfit);

  }

  fclose(fpo);

  return 0;

}

/**********************************************************************/