#include <CLHEP/Matrix/Vector.h>

using namespace std;

#include "chisqMin.h"

chisqMin::chisqMin(residFunc *resid)
  : residFuncPtr(resid)
{
  unsigned int p = residFuncPtr->getp();
  unsigned int n = residFuncPtr->getn();
  covar = gsl_matrix_alloc(p, p); /* allocate the covariance matrix */
  T = gsl_multifit_fdfsolver_lmsder; /* choose the solver */
  s = gsl_multifit_fdfsolver_alloc (T, (size_t)n, (size_t)p); /* allocate work space,
                                                 get solver pointer */
}

chisqMin::~chisqMin() {
}

void chisqMin::setStartParams(HepVector &xIn) {
  //  cout << "setStartParams called\n";
  //  cout << "start params: " << xIn << endl;
  unsigned int n = residFuncPtr->getn();
  unsigned int p = residFuncPtr->getp();
  //  cout << "number of data points = " << n << " number of parameters = " << p << endl;
  f.f = &fGsl;
  f.df = &dfGsl;
  f.fdf = &fdfGsl;
  f.n = (size_t)n;
  f.p = (size_t)p;
  f.params = NULL; /* pointer to something that gets the data,
		      don't know if this is needed yet */
  gsl_vector *xGslPtr;
  xGslPtr = gsl_vector_alloc((size_t)p);
  for (unsigned int i = 0; i < p; i++) {
    gsl_vector_set(xGslPtr, (size_t)i, xIn(i + 1));
    //    cout << i << " " << gsl_vector_get(xGslPtr, (size_t)i) << endl; 
  }
  gsl_multifit_fdfsolver_set(s, &f, xGslPtr); /* initialize the solver */
  gsl_vector_free(xGslPtr);
}

void chisqMin::getEndParams(HepVector& xOut) {
  unsigned int p = residFuncPtr->getp();
  for (unsigned int i = 0; i < p; i++) {
    xOut(i + 1) = gsl_vector_get (s->x, (size_t)i);
  }
}

void chisqMin::fit() {
  //  cout << "fit called" << endl;
  iter = 0;
  //  print_state(iter, s);
  do {
    iter++;
    cout << "== begin iteration " << iter << " ==\n";
    status = gsl_multifit_fdfsolver_iterate(s); /* go to next point */
    printf("iter = %d, status = %d, text = %s\n", iter, status, gsl_strerror (status));
    //    print_state(iter, s);
    if (status) break;
    status = gsl_multifit_test_delta(s->dx, s->x, 1e-4, 1e-4);
  }
  while (status == GSL_CONTINUE && iter < 100);
}