// @(#)root/minuit2:$Id: FumiliStandardChi2FCN.cxx 20880 2007-11-19 11:23:41Z rdm $
// Authors: M. Winkler, F. James, L. Moneta, A. Zsenei   2003-2005  

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2005 LCG ROOT Math team,  CERN/PH-SFT                *
 *                                                                    *
 **********************************************************************/


#include "Minuit2/FumiliStandardChi2FCN.h"

#include <vector>
#include <cmath>

namespace ROOT {

   namespace Minuit2 {


//#include <iostream>

std::vector<double> FumiliStandardChi2FCN::Elements(const std::vector<double>& par) const {
   // Calculate the f(i) contribution to the Chi2. Chi2 = Sum[f(i)**2]
   
   std::vector<double> result;
   double tmp1 = 0.0;
   unsigned int fPositionsSize = fPositions.size();
   
   
   for(unsigned int i=0; i < fPositionsSize; i++) {
      
      const std::vector<double> & currentPosition = fPositions[i];
      
      // The commented line is the object-oriented way to do it
      // but it is faster to do a single function call...
      //(*(this->getModelFunction())).SetParameters(par);
      tmp1 = (*(this->ModelFunction()))(par, currentPosition)- fMeasurements[i];
      
      result.push_back(tmp1*fInvErrors[i] );
      
      //std::cout << "element " << i << "  " << (*(this->getModelFunction()))(par, currentPosition) << "  " <<  fMeasurements[i] << "  " << result[i] << std::endl; 
   }
   
   
   
   return result;
   
}



const std::vector<double> & FumiliStandardChi2FCN::GetMeasurement(int index) const {
   // Return the coordinate (position) values.
   return fPositions[index];
   
}


int FumiliStandardChi2FCN::GetNumberOfMeasurements() const {
   // Return size
   return fPositions.size();
   
}



void  FumiliStandardChi2FCN::EvaluateAll( const std::vector<double> & par) { 
   // Evaluate chi2 value, gradient and hessian all in a single
   // loop on the measurements 
   
   int nmeas = GetNumberOfMeasurements(); 
   std::vector<double> & grad = Gradient();
   std::vector<double> & h = Hessian();
   int npar = par.size();
   double chi2 = 0; 
   grad.resize(npar);
   h.resize( static_cast<unsigned int>(0.5 * npar* (npar + 1) ) );
   // reset Elements
   grad.assign(npar, 0.0);
   h.assign(static_cast<unsigned int>(0.5 * npar* (npar + 1) ) , 0.0);
   
   
   const ParametricFunction & modelFunc = *ModelFunction();
   
   for (int i = 0; i < nmeas; ++i) { 
      
      // work for multi-dimensional points
      const std::vector<double> & currentPosition = fPositions[i];
      modelFunc.SetParameters( currentPosition );
      double invError = fInvErrors[i];
      double fval = modelFunc(par); 
      
      double element = ( fval - fMeasurements[i] )*invError;
      chi2 += element*element;
      
      // calc derivatives 
      
      // this method should return a reference
      std::vector<double> mfg = modelFunc.GetGradient(par);
      
      // grad is derivative of chi2 w.r.t to parameters
      for (int j = 0; j < npar ; ++j) { 
         double dfj = invError * mfg[j]; 
         grad[j] += 2.0 * element * dfj; 
         
         // in second derivative use Fumili approximation neglecting the term containing the 
         // second derivatives of the model function
         for (int k = j; k < npar; ++ k) { 
            int idx =  j + k*(k+1)/2; 
            h[idx] += 2.0 * dfj * invError * mfg[k]; 
         }
         
      } // end param loop  
      
   } // end points loop 
   
   // set Value in base class
   SetFCNValue( chi2); 
   
}

   }  // namespace Minuit2

}  // namespace ROOT
