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

#include <RAT/MetaAreaSeed.hh>
#include <RAT/Method.hh>
#include <RAT/OptimisedMethod.hh>
#include <RAT/SeededMethod.hh>
#include <RAT/OptimiserFactory.hh>
#include <RAT/DS/FitResult.hh>
#include <RAT/DB.hh>
#include <TH1D.h>
using namespace RAT::DS;
using namespace RAT::Optimisers;

#include <string>
#include <CLHEP/Units/PhysicalConstants.h>
#include <utility>
using namespace CLHEP;
using namespace std;

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

void
MetaAreaSeed::Initialise( const std::string& param )
{
  if( param.empty() == true )
    fOptimiser = OptimiserFactory::GetInstance()->GetOptimiser( "minuit" );
  else
    fOptimiser = OptimiserFactory::GetInstance()->GetOptimiser( param );
  fNSeeds = 100;  // Make up to 100 attempts
  fNSuccess = 5;  // To get at least 5 successes
}

void
MetaAreaSeed::BeginOfRun( DS::Run& run )
{
  DB* db = DB::Get();
  fAVRadius = db->GetLink("SOLID", "acrylic_vessel_outer")->GetD("r_sphere");
  fLimit = 1000;  // Success is defined as events within 1m of each other
  fOptimiser->BeginOfRun( run );
}

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

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

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

double
MetaAreaSeed::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 );

  TVector3 bestPos(0,0,0);
  double bestValue=0;
  int count=0, bestLoop=-1;
  int iLoopCount=0;      // Keep a count of number of "valid" loops

  vector<TVector3> vPos; // Vector of fitted positions

  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 currentPos(fitResults[iLoopCount].first[0],fitResults[iLoopCount].first[1],fitResults[iLoopCount].first[2]);
      if( fOptimiser->GetValid() == false )
        {
          fitResults.pop_back();
          fitValues.pop_back();
          iLoopCount--;
        }
      else if(currentPos.Mag()>fAVRadius)
        {
          // Check result is within a sensible physical radius
          // e.g, within the AV/PSUP
          fitResults.pop_back();
          fitValues.pop_back();
          iLoopCount--;
        }
      else
        {
          // load new vertex into vector
          vPos.push_back(currentPos);

          // check to see if we have group of events within the same region
          // if yes, return the event within the highest likelihood value
          for(size_t i=0; i<vPos.size(); i++)
            {
              count=0;
              for(size_t j=0; j<vPos.size(); j++)
                {
                  if(i==j) continue;
                  if((vPos[i]-vPos[j]).Mag()<fLimit) count++;
                }
              if(count>fNSuccess && fitValues[i]>bestValue)
                {
                  bestValue=fitValues[i];
                  bestLoop=i;
                }
            }

          // If we have a result, finish here
          if(bestLoop>=0) iLoop=fNSeeds+1;
        }
      iLoopCount++;
    }
  // Set not valid and return what ever the current values are
  if( fitValues.empty() || bestLoop == - 1 )
    {
      fParams = fOptimiser->GetParams();
      fPositiveErrors = fOptimiser->GetPositiveErrors();
      fNegativeErrors = fOptimiser->GetNegativeErrors();
      fValid = false;
      return 0.0;
    }

  fValid = true;

  fParams = fitResults[bestLoop].first;
  fPositiveErrors = fitResults[bestLoop].second.first;
  fNegativeErrors = fitResults[bestLoop].second.second;

  return fitValues[bestLoop];
}

FitResult
MetaAreaSeed::NewSeed( const FitResult& startSeed )
{
  FitResult newSeed = startSeed;
  FitVertex vertex = newSeed.GetVertex(0);
  if( vertex.ContainsPosition() )
    {
      const double cosTheta = -1.0 + 2.0 * G4UniformRand();
      const double phi = twopi * G4UniformRand();
      const double r = pow( G4UniformRand(), 1.0 / 3.0 ) * fAVRadius;
      vertex.SetPosition( TVector3( r * sin( acos( cosTheta ) ) * cos( phi ),
                                    r * sin( acos( cosTheta ) ) * sin( phi ),
                                    r * cosTheta ) );
      vertex.SetPositionErrors( ROOT::Math::XYZVectorF(fAVRadius, fAVRadius, fAVRadius) );
    }
  if( vertex.ContainsTime() )
    {
      vertex.SetTime( 240.0 + G4UniformRand() * 20.0 );
      vertex.SetTimeErrors( 20.0 );
    }
  if( vertex.ContainsDirection() )
    {
      const double cosTheta = -1.0 + 2.0 * G4UniformRand();
      const double phi = twopi * G4UniformRand();
      vertex.SetDirection( TVector3( sin( acos( cosTheta ) ) * cos( phi ),
                                     sin( acos( cosTheta ) ) * sin( phi ),
                                     cosTheta ) );
      vertex.SetDirectionErrors( TVector3( 1.0, 1.0, 1.0 ) );
    }
  if( vertex.ContainsEnergy() )
    {
      // Random numbers
      vertex.SetEnergy( 6.0 * G4UniformRand() );
      vertex.SetEnergyErrors( 12.0 );
    }
  newSeed.SetVertex( 0, vertex );
  return newSeed;
}