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

#include <RAT/MetaDirectionSeed.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>
using namespace RAT::DS;
using namespace RAT::Optimisers;

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

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

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

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

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

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

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

double
MetaDirectionSeed::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 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
      if( fOptimiser->GetValid() == false )
        {
          fitResults.pop_back();
          fitValues.pop_back();
        }

      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;
    }

  // Return median of theta and phi as the best direction
  // This should reduce the effect of outlying points on fit
  fValid = true;
  vector<double> theta, phi;
  for(size_t p=0; p<fitValues.size(); p++)
    {
      TVector3 dirp(fitResults[p].first[0],fitResults[p].first[1],fitResults[p].first[2]);

      theta.push_back(dirp.Theta());
      phi.push_back(dirp.Phi());
    }
  sort(theta.begin(), theta.end());
  sort(phi.begin(), phi.end());
  sort(fitValues.begin(), fitValues.end());

  // Compile median direction vector
  TVector3 medianDir, errPosDir, errNegDir, errDir;
  medianDir.SetMagThetaPhi(1,theta[theta.size()/2],phi[phi.size()/2]);

  // Create an error on the direction based on the spread of the results vector
  errPosDir.SetMagThetaPhi(1,(theta[3*theta.size()/4]),(phi[3*phi.size()/4]));
  errNegDir.SetMagThetaPhi(1,(theta[theta.size()/4]),(phi[phi.size()/4]));
  errDir = errNegDir - errPosDir;

  // This is a stupid way to do it but need vector<double>
  vector<double> finalResult, finalError;
  finalResult.push_back(medianDir.Unit().X());
  finalResult.push_back(medianDir.Unit().Y());
  finalResult.push_back(medianDir.Unit().Z());
  finalError.push_back(errDir.X());
  finalError.push_back(errDir.Y());
  finalError.push_back(errDir.Z());

  // Now set the method result
  fParams = finalResult;
  fPositiveErrors = fNegativeErrors = finalError;

  return fitValues[fitValues.size()/2];
}

FitResult
MetaDirectionSeed::NewSeed( const FitResult& startSeed )
{
  FitResult newSeed = startSeed;
  FitVertex vertex = newSeed.GetVertex(0);
  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] ) ) );
    }

  newSeed.SetVertex( 0, vertex );
  return newSeed;
}