#include <globals.hh>
#include <Randomize.hh>

#include <RAT/MetaNSeed.hh>
#include <RAT/Method.hh>
#include <RAT/OptimisedMethod.hh>
#include <RAT/SeededMethod.hh>
#include <RAT/OptimiserFactory.hh>
#include <RAT/DS/FitResult.hh>
using namespace RAT::DS;
using namespace RAT::Optimisers;

#include <string>
#include <utility>
using namespace std;

MetaNSeed::~MetaNSeed()
{
  delete fOptimiser;
}

void
MetaNSeed::Initialise( const std::string& param )
{
  if( param.empty() == true )
    fOptimiser = OptimiserFactory::GetInstance()->GetOptimiser( "minuit" );
  else
    fOptimiser = OptimiserFactory::GetInstance()->GetOptimiser( param );
  fNSeeds = 12;
}

void
MetaNSeed::BeginOfRun( DS::Run& run )
{
  fOptimiser->BeginOfRun( run );
}

void
MetaNSeed::EndOfRun( DS::Run& run )
{
  fOptimiser->EndOfRun( run );
}

double
MetaNSeed::Minimise()
{
  fMinFactor = 1.0;
  return Optimise();
}

double
MetaNSeed::Maximise()
{
  fMinFactor = -1.0;
  return Optimise();
}

double
MetaNSeed::Optimise()
{
  // Would like to use a TUPLE: ( param | positive errors | negative errors ) but this is only in C++11!
  // For the moment, use a pair: ( param | pair (positive errors | negative errors) ) ... i.e. pair inside a pair
  vector< pair< vector<double>, pair< vector<double>, vector<double> > > > fitResults;
  vector<double> fitValues;
  FitResult seed = fMethod->GetFitResult();
  fSeededMethod->UpdateSeed( seed );

  int iLoopCount=0; // Keeps count of the number of "valid" loops
  fOptimiser->SetComponent( fMethod );
  // Now alter the seed and optimise
  for( int iLoop = 0; iLoop < fNSeeds; iLoop++ )
    {
      // Now alter the seed, if not the first iteration
      if( iLoop != 0 )
        {
          FitResult newSeed = NewSeed( seed );
          fSeededMethod->UpdateSeed( newSeed );
        }
      // Now optimise
      if( fMinFactor < 0 )
        fitValues.push_back( fOptimiser->Maximise() );
      else
        fitValues.push_back( fOptimiser->Minimise() );
      pair< vector<double>, vector<double> > errorsPair( fOptimiser->GetPositiveErrors(), fOptimiser->GetNegativeErrors() );
      fitResults.push_back( pair< vector<double>, pair< vector<double>, vector<double> > >(fOptimiser->GetParams(), errorsPair) );

      // Remove invalid results
      TVector3 pos(fitResults[iLoopCount].first[0],fitResults[iLoopCount].first[1],fitResults[iLoopCount].first[2]);
      if( fOptimiser->GetValid() == false )
        {
          fitResults.pop_back();
          fitValues.pop_back();
          iLoopCount--;
        }

      iLoopCount++;
    }
  // Set not valid and return what ever the current values are
  if( fitValues.empty() )
    {
      fParams = fOptimiser->GetParams();
      fPositiveErrors = fOptimiser->GetPositiveErrors();
      fNegativeErrors = fOptimiser->GetNegativeErrors();
      fValid = false;
      return 0.0;
    }

  // Find the best fit, just the max or min
  fValid = true;
  int bestFit;
  if( fMinFactor < 0 )
    bestFit = max_element( fitValues.begin(), fitValues.end() ) - fitValues.begin();
  else
    bestFit = min_element( fitValues.begin(), fitValues.end() ) - fitValues.begin();

  // Now set the method result
  fParams = fitResults[bestFit].first;
  fPositiveErrors = fitResults[bestFit].second.first;
  fNegativeErrors = fitResults[bestFit].second.second;

  return fitValues[bestFit];
}

FitResult
MetaNSeed::NewSeed( const FitResult& startSeed )
{
  FitResult newSeed = startSeed;
  FitVertex vertex = newSeed.GetVertex(0);
  if( vertex.ContainsPosition() )
    {
     // Use the positive position errors to set the position ... does it matter if the negative one isn't used?
      vertex.SetPosition( TVector3( CLHEP::RandGauss::shoot( vertex.GetPosition()[0], vertex.GetPositivePositionError()[0] ),
                                    CLHEP::RandGauss::shoot( vertex.GetPosition()[1], vertex.GetPositivePositionError()[1] ),
                                    CLHEP::RandGauss::shoot( vertex.GetPosition()[2], vertex.GetPositivePositionError()[2] ) ) );
    }
  if( vertex.ContainsTime() )
    {
      vertex.SetTime( CLHEP::RandGauss::shoot( vertex.GetTime(), vertex.GetPositiveTimeError() ) );
    }
  if( vertex.ContainsDirection() )
    {
      vertex.SetDirection( TVector3( CLHEP::RandGauss::shoot( vertex.GetDirection()[0], vertex.GetPositiveDirectionError()[0] ),
                                     CLHEP::RandGauss::shoot( vertex.GetDirection()[1], vertex.GetPositiveDirectionError()[1] ),
                                     CLHEP::RandGauss::shoot( vertex.GetDirection()[2], vertex.GetPositiveDirectionError()[2] ) ) );
    }
  if( vertex.ContainsEnergy() )
    {
      vertex.SetEnergy( CLHEP::RandGauss::shoot( vertex.GetEnergy(), vertex.GetPositiveEnergyError() ) );
    }
  newSeed.SetVertex( 0, vertex );
  return newSeed;
}