#include <RAT/SeededMethod.hh>
#include <RAT/DS/FitVertex.hh>
using namespace RAT;
using namespace RAT::Methods;
using namespace RAT::DS;


void SeededMethod::SetSeed( const DS::FitResult& seed )
{

  if( seed.GetVertexCount() == 0 )
    // Nothing to seed from
    return;

  const DS::FitVertex& seedVertex = seed.GetVertex( 0 );
  DS::FitVertex vertex = fSeedResult.GetVertex( 0 );

  // Set errors as having the same fixed/unfixed as the main parameter (even if no errors are set, they should be associated with the main parameter)
  // Set errors as having their own validity (i.e. if a parameter set without errors by a method then the parameter may be valid but the errors are not!)

  if( seedVertex.ContainsPosition() && seedVertex.ValidPosition() )
    if( !vertex.ContainsPosition() || vertex.FixedPosition() || !seedVertex.FixedPosition() )
      {
        vertex.SetPosition( seedVertex.GetPosition(), seedVertex.ValidPosition(), seedVertex.FixedPosition() );
        if( seedVertex.ContainsPositivePositionError() )
          vertex.SetPositivePositionError( ROOT::Math::XYZVectorF(seedVertex.GetPositivePositionError().x(), seedVertex.GetPositivePositionError().y(), seedVertex.GetPositivePositionError().z()), seedVertex.ValidPositivePositionError(), seedVertex.FixedPosition() );
        if( seedVertex.ContainsNegativePositionError() )
          vertex.SetNegativePositionError( ROOT::Math::XYZVectorF(seedVertex.GetNegativePositionError().x(), seedVertex.GetNegativePositionError().y(), seedVertex.GetNegativePositionError().z()), seedVertex.ValidNegativePositionError(), seedVertex.FixedPosition() );
      }

  if( seedVertex.ContainsTime() && seedVertex.ValidTime() )
    if( !vertex.ContainsTime() || vertex.FixedTime() || !seedVertex.FixedTime() )
      {
        vertex.SetTime( seedVertex.GetTime(), seedVertex.ValidTime(), seedVertex.FixedTime() );
        if( seedVertex.ContainsPositiveTimeError() )
          vertex.SetPositiveTimeError( seedVertex.GetPositiveTimeError(), seedVertex.ValidPositiveTimeError(), seedVertex.FixedTime() );
        if( seedVertex.ContainsNegativeTimeError() )
          vertex.SetNegativeTimeError( seedVertex.GetNegativeTimeError(), seedVertex.ValidNegativeTimeError(), seedVertex.FixedTime() );
      }

  if( seedVertex.ContainsDirection() && seedVertex.ValidDirection() )
    if( !vertex.ContainsDirection() || vertex.FixedDirection() || !seedVertex.FixedDirection() )
      {
        vertex.SetDirection( seedVertex.GetDirection(), seedVertex.ValidDirection(), seedVertex.FixedDirection() );
        if( seedVertex.ContainsPositiveDirectionError() )
          vertex.SetPositiveDirectionError( seedVertex.GetPositiveDirectionError(), seedVertex.ValidPositiveDirectionError(), seedVertex.FixedDirection() );
        if( seedVertex.ContainsNegativeDirectionError() )
          vertex.SetNegativeDirectionError( seedVertex.GetNegativeDirectionError(), seedVertex.ValidNegativeDirectionError(), seedVertex.FixedDirection() );
      }

  if( seedVertex.ContainsEnergy() && seedVertex.ValidEnergy() )
    if( !vertex.ContainsEnergy() || vertex.FixedEnergy() || !seedVertex.FixedEnergy() )
      {
        vertex.SetEnergy( seedVertex.GetEnergy(), seedVertex.ValidEnergy(), seedVertex.FixedEnergy() );
        if( seedVertex.ContainsPositiveEnergyError() )
          vertex.SetPositiveEnergyError( seedVertex.GetPositiveEnergyError(), seedVertex.ValidPositiveEnergyError(), seedVertex.FixedEnergy() );
        if( seedVertex.ContainsNegativeEnergyError() )
          vertex.SetNegativeEnergyError( seedVertex.GetNegativeEnergyError(), seedVertex.ValidNegativeEnergyError(), seedVertex.FixedEnergy() );
      }

  fSeedResult.SetVertex( 0, vertex );

}

void SeededMethod::CopySeedToResult( )
{

  const DS::FitVertex& seedVertex = fSeedResult.GetVertex( 0 );
  DS::FitVertex resultVertex;

  if( seedVertex.ContainsPosition() )
    resultVertex.SetPosition( seedVertex.GetPosition(), seedVertex.ValidPosition(), true );
  if( seedVertex.ContainsPositivePositionError() )
    resultVertex.SetPositivePositionError( ROOT::Math::XYZVectorF(seedVertex.GetPositivePositionError().x(), seedVertex.GetPositivePositionError().y(), seedVertex.GetPositivePositionError().z()), seedVertex.ValidPositivePositionError(), true );
  if( seedVertex.ContainsNegativePositionError() )
    resultVertex.SetNegativePositionError( ROOT::Math::XYZVectorF(seedVertex.GetNegativePositionError().x(), seedVertex.GetNegativePositionError().y(), seedVertex.GetNegativePositionError().z()), seedVertex.ValidNegativePositionError(), true );

  if( seedVertex.ContainsTime() )
    resultVertex.SetTime( seedVertex.GetTime(), seedVertex.ValidTime(), true );
  if( seedVertex.ContainsPositiveTimeError() )
    resultVertex.SetPositiveTimeError( seedVertex.GetPositiveTimeError(), true );
  if( seedVertex.ContainsNegativeTimeError() )
    resultVertex.SetNegativeTimeError( seedVertex.GetNegativeTimeError(), true );

  if( seedVertex.ContainsDirection() )
    resultVertex.SetDirection( seedVertex.GetDirection(), seedVertex.ValidDirection(), true );
  if( seedVertex.ContainsPositiveDirectionError() )
    resultVertex.SetPositiveDirectionError( seedVertex.GetPositiveDirectionError(), true );
  if( seedVertex.ContainsNegativeDirectionError() )
    resultVertex.SetNegativeDirectionError( seedVertex.GetNegativeDirectionError(), true );

  if( seedVertex.ContainsEnergy() )
    resultVertex.SetEnergy( seedVertex.GetEnergy(), seedVertex.ValidEnergy(), true );
  if( seedVertex.ContainsPositiveEnergyError() )
    resultVertex.SetPositiveEnergyError( seedVertex.GetPositiveEnergyError(), true );
  if( seedVertex.ContainsNegativeEnergyError() )
    resultVertex.SetNegativeEnergyError( seedVertex.GetNegativeEnergyError(), true );

  fFitResult.SetVertex( 0, resultVertex );

}