#include "ShowerFit.hh"
#include "Math/Minimizer.h"
#include "Math/Factory.h"
#include "Math/Functor.h"
#include "TRandom2.h"
#include "TError.h"
#include "TStopwatch.h"
#include "Fit/FitConfig.h"

Trk ShowerFit::fit( Trk& start_track , 
		    Det& det, 
		    vector<Hit>& data, 
		    int do_init_pdf /*=1*/ )
{ 
  TStopwatch W;

  path_.clear();

  if ( do_init_pdf )
    {
      if (max_dist == 0 )
	{
	  pdf -> init_event( det, data );
	}
      else
	{
	  pdf -> init_event( det, data, start_track.pos, max_dist );
	} 
    }
  
  ROOT::Math::Minimizer* M = 
    ROOT::Math::Factory::CreateMinimizer( minimizer_name.c_str(), minimizer_algo.c_str());

  if (!M)
    {
      fatal("failed to create minimizer", minimizer_name.c_str(), minimizer_algo.c_str() );

    }

  
  M->SetMaxFunctionCalls(10000);   // for Minuit/Minuit2 
  M->SetMaxIterations(100);        // for GSL
  M->SetTolerance(0.001);
  M->SetPrintLevel( verb - 2 );    // -1 seems to be the lowser setting in Gslminimizer

  ROOT::Math::Functor f( this , &ShowerFit::score_function, 7 );
  M->SetFunction( f );

  //Variables are: [posx, posy, posz, theta, phi, time, energy]
  //See showrec_util.hh -> to_trk
  double vstart[7];
  to_pars( start_track, vstart );
  vstart[5]+=0.1; // starting at 0 seems to fix parameter
  vector<double> step = pack(1.0, 1.0, 1.0, 0.01, 0.01, 1.0, 0.1 );

  if (verb) print (verb, "fixing ", fixed_vars , verb );

  for (int i=0; i<7; i++ )
    {
      if ( contains( fixed_vars, i ) )
	{
	  M -> SetFixedVariable( i, parnames()[i] ,vstart[i] );
	}
      else
	{
	  M -> SetVariable( i, parnames()[i] ,vstart[i], step[i] );
	}
     } 
  


  // ----------------------------------------
  //  do the fit
  // ----------------------------------------

  ErrorHandlerFunc_t oldhandler = SetErrorHandler( fit_errorhandler );
  
  M -> Minimize(); // for great justice!
  
  SetErrorHandler( oldhandler );

  Trk r;
  to_trk( M -> X(), r );
  r.lik = M -> MinValue();

  r.fitinf.resize(1);
  r.fitinf[0] = W.RealTime();

  total_time += r.fitinf[0];
  nfits++;
  
  // the error matrix computation in root is not implemented (at least for gslminimizer)
  // r.error_matrix.resize(49);
  // cout << "-----------------calling getCov" << minimizer_name.c_str() << endl;
  // M -> GetCovMatrix( &r.error_matrix[0] );
  
  delete M;
  return r; 
}




Trk ShowerFit::fit2( Trk& start_track , 
		     Det& det, 
		     vector<Hit>& data, 
		     int do_init_pdf /*=1*/ )
{ 
  TStopwatch W;

  cout << " fit2 " << endl;

  path_.clear();

  if ( do_init_pdf )
    {
      if (max_dist == 0 )
	{
	  pdf -> init_event( det, data );
	}
      else
	{
	  // the following will precompute the z-histograms
	  pdf -> init_event( det, data, start_track.pos, max_dist );
	} 
    }

  ROOT::Math::Minimizer* M = 
    ROOT::Math::Factory::CreateMinimizer( minimizer_name.c_str(), minimizer_algo.c_str());
  
  M->SetMaxFunctionCalls(10000);   // for Minuit/Minuit2 
  M->SetMaxIterations(100);        // for GSL 
  M->SetTolerance(0.001);
  M->SetPrintLevel( verb - 2 );    // -1 seems to be the lowser setting in Gslminimizer

  GradFunc gf;
  gf.pdf = (DigitalShowerPdf*) pdf;

  //ROOT::Math::Functor f( this , &ShowerFit::score_function, 7 );
  M->SetFunction( gf );
  


  double v[7];
  to_pars( start_track, v );
  
  M -> SetVariable(0, "theta", v[3] , 0.01);
  M -> SetVariable(1, "phi",   v[4] , 0.01);
  M -> SetVariable(2, "logE",  v[6] , 1.0 );
    
  // ----------------------------------------
  //  do the fit
  // ----------------------------------------

  ErrorHandlerFunc_t oldhandler = SetErrorHandler( fit_errorhandler );
  
  M -> Minimize(); 
  
  SetErrorHandler( oldhandler );

  const double* x =  M -> X();
  v[3] = x[0];
  v[4] = x[1];
  v[6] = x[2];

  print ("e = ", v[6] );

  Trk r;
  to_trk(v , r );
  r.lik = M -> MinValue();
  r.fitinf.resize(1);
  r.fitinf[0] = W.RealTime();
  total_time += r.fitinf[0];
  nfits++;
  r.error_matrix.resize(49);
  M -> GetCovMatrix( &r.error_matrix[0] );
  
  delete M;
  return r; 
}


//Using only eval (not gradient)
Trk ShowerFit::fit3( Trk& start_track , 
		    Det& det, 
		    vector<Hit>& data, 
		    int do_init_pdf /*=1*/ )
{ 
  TStopwatch W;

  path_.clear();

  if ( do_init_pdf )
    {
      if (max_dist == 0 )
	{
	  pdf -> init_event( det, data );
	}
      else
	{
	  pdf -> init_event( det, data, start_track.pos, max_dist );
	} 
    }
  
  ROOT::Math::Minimizer* M = 
    ROOT::Math::Factory::CreateMinimizer( minimizer_name.c_str(), "NMSIMPLEX2");
  
  M->SetMaxFunctionCalls(10000);   // for Minuit/Minuit2 
  M->SetMaxIterations(100);        // for GSL 
  M->SetTolerance(0.001);
  M->SetPrintLevel( verb - 2 );    // -1 seems to be the lowser setting in Gslminimizer

  ROOT::Math::Functor f( this , &ShowerFit::score_function, 7 );
  M->SetFunction( f );

  double vstart[7];
  to_pars( start_track, vstart );
  vstart[5]+=0.1; // starting at 0 seems to fix parameter
  vector<double> step = pack(1.0 ,1.0 ,1.0 , 0.01, 0.01, 1.0, 0.1 );

  if (verb) print (verb, "fixing ", fixed_vars , verb );

  for (int i=0; i<7; i++ )
    {
      if ( contains( fixed_vars, i ) )
	{
	  M -> SetFixedVariable( i, parnames()[i] ,vstart[i] );
	}
      else
	{
	  M -> SetVariable( i, parnames()[i] ,vstart[i], step[i] );
	}
     } 



  // ----------------------------------------
  //  do the fit
  // ----------------------------------------

  ErrorHandlerFunc_t oldhandler = SetErrorHandler( fit_errorhandler );
  
  M -> Minimize(); // for great justice!
  
  SetErrorHandler( oldhandler );

  Trk r;
  to_trk( M -> X(), r );
  r.lik = M -> MinValue();

  r.fitinf.resize(1);
  r.fitinf[0] = W.RealTime();

  total_time += r.fitinf[0];
  nfits++;
  
  r.error_matrix.resize(49);
  M -> GetCovMatrix( &r.error_matrix[0] );
  
  delete M;

  return r; 
}