//
// ********************************************************************
// * License and Disclaimer                                           *
// *                                                                  *
// * The  Geant4 software  is  copyright of the Copyright Holders  of *
// * the Geant4 Collaboration.  It is provided  under  the terms  and *
// * conditions of the Geant4 Software License,  included in the file *
// * LICENSE and available at  http://cern.ch/geant4/license .  These *
// * include a list of copyright holders.                             *
// *                                                                  *
// * Neither the authors of this software system, nor their employing *
// * institutes,nor the agencies providing financial support for this *
// * work  make  any representation or  warranty, express or implied, *
// * regarding  this  software system or assume any liability for its *
// * use.  Please see the license in the file  LICENSE  and URL above *
// * for the full disclaimer and the limitation of liability.         *
// *                                                                  *
// * This  code  implementation is the result of  the  scientific and *
// * technical work of the GEANT4 collaboration.                      *
// * By using,  copying,  modifying or  distributing the software (or *
// * any work based  on the software)  you  agree  to acknowledge its *
// * use  in  resulting  scientific  publications,  and indicate your *
// * acceptance of all terms of the Geant4 Software license.          *
// ********************************************************************
//
// G4FSALIntegrationDriver inline implementation
//
// Author: Dmitry Sorokin (CERN, Google Summer of Code 2017), 20.10.2017
// --------------------------------------------------------------------

#include "G4FieldUtils.hh"

template <class T>
G4FSALIntegrationDriver<T>::
G4FSALIntegrationDriver ( G4double hminimum, T* pStepper,
                          G4int numComponents, G4int statisticsVerbose )
 : Base(pStepper),
   fMinimumStep(hminimum),
   fVerboseLevel(statisticsVerbose)
{
  if (numComponents != Base::GetStepper()->GetNumberOfVariables())
  {
    std::ostringstream message;
    message << "Driver's number of integrated components "
            << numComponents
            << " != Stepper's number of components "
            << pStepper->GetNumberOfVariables();
    G4Exception("G4FSALIntegrationDriver","GeomField0002",
                FatalException, message);
  }
}

template <class T>
G4FSALIntegrationDriver<T>::~G4FSALIntegrationDriver()
{
#ifdef G4VERBOSE
  if( fVerboseLevel > 0 )
  {
    G4cout << "G4FSALIntegration Driver Stats: "
           << "#QuickAdvance " << fNoQuickAvanceCalls
           << " - #AccurateAdvance " << fNoAccurateAdvanceCalls << G4endl
           << "#good steps " << fNoAccurateAdvanceGoodSteps << " "
           << "#bad steps " << fNoAccurateAdvanceBadSteps << G4endl;
  }
#endif
}

// Runge-Kutta driver with adaptive stepsize control. Integrate starting
// values at y_current over hstep x2 with accuracy eps.
// On output ystart is replaced by values at the end of the integration
// interval. RightHandSide is the right-hand side of ODE system.
// The source is similar to odeint routine from NRC p.721-722 .
//
template <class T>
G4bool G4FSALIntegrationDriver<T>::
AccurateAdvance( G4FieldTrack& track, G4double hstep,
                 G4double eps, G4double hinitial )
{
  ++fNoAccurateAdvanceCalls;

  if (hstep < GetMinimumStep())
  {
    G4double dchord_step = 0.0, dyerr = 0.0;
    G4double dydx[G4FieldTrack::ncompSVEC];
    Base::GetDerivatives(track, dydx);
    return QuickAdvance(track, dydx, hstep, dchord_step, dyerr);
  }

  G4bool succeeded = false;

  G4double hnext, hdid;

  G4double y[G4FieldTrack::ncompSVEC], dydx[G4FieldTrack::ncompSVEC];

  track.DumpToArray(y);

  // hstep somtimes is too small. No need to add large curveLength.
  G4double curveLength = 0.0;
  G4double endCurveLength = hstep;

  G4double h = hstep;
  if (hinitial > CLHEP::perMillion * hstep && hinitial < hstep)
  {
    h = hinitial;
  }

  Base::GetStepper()->RightHandSide(y, dydx);

  for (G4int iter = 0; iter < Base::GetMaxNoSteps(); ++iter)
  {
    const G4ThreeVector StartPos =
          field_utils::makeVector(y, field_utils::Value3D::Position);

    OneGoodStep(y, dydx, curveLength, h, eps, hdid, hnext);

    const G4ThreeVector EndPos =
          field_utils::makeVector(y, field_utils::Value3D::Position);

    CheckStep(EndPos, StartPos, hdid);

    G4double restCurveLength = endCurveLength - curveLength;
    if (restCurveLength < GetSmallestFraction() * hstep)
    {
      succeeded = true;
      break;
    }
    h = std::min(hnext, restCurveLength);
  }


  if (succeeded)
  {
    track.LoadFromArray(y, Base::GetStepper()->GetNumberOfVariables());
    track.SetCurveLength(track.GetCurveLength() + curveLength);
  }

  return succeeded;
}

// Driver for one Runge-Kutta Step with monitoring of local truncation error
// to ensure accuracy and adjust stepsize. Input are dependent variable
// array y[0,...,5] and its derivative dydx[0,...,5] at the
// starting value of the independent variable x . Also input are stepsize
// to be attempted htry, and the required accuracy eps. On output y and x
// are replaced by their new values, hdid is the stepsize that was actually
// accomplished, and hnext is the estimated next stepsize.
// This is similar to the function rkqs from the book:
// Numerical Recipes in C: The Art of Scientific Computing (NRC), Second
// Edition, by William H. Press, Saul A. Teukolsky, William T.
// Vetterling, and Brian P. Flannery (Cambridge University Press 1992),
// 16.2 Adaptive StepSize Control for Runge-Kutta, p. 719
//
template <class T>
void G4FSALIntegrationDriver<T>::
OneGoodStep(G4double y[], 
            G4double dydx[],
            G4double& curveLength,   // InOut
            G4double htry, 
            G4double eps_rel_max,
            G4double& hdid,         // Out
            G4double& hnext)        // Out
{
  G4double error2 = DBL_MAX;

  G4double yError[G4FieldTrack::ncompSVEC],
           yOut[G4FieldTrack::ncompSVEC],
           dydxOut[G4FieldTrack::ncompSVEC];

  // Set stepsize to the initial trial value
  G4double hstep = htry;

  const G4int max_trials = 100;

  for (G4int iter = 0; iter < max_trials; ++iter)
  {
    Base::GetStepper()->Stepper(y, dydx, hstep, yOut, yError, dydxOut);
    error2 = field_utils::relativeError2(y, yError, hstep, eps_rel_max);

    // Step succeeded.
    if (error2 <= 1) {  break; }

    hstep = Base::ShrinkStepSize2(hstep, error2);
  }

  hnext = Base::GrowStepSize2(hstep, error2);
  curveLength += (hdid = hstep);

  for(G4int k = 0; k < Base::GetStepper()->GetNumberOfVariables(); ++k)
  {
    y[k] = yOut[k];
    dydx[k] = dydxOut[k];
  }
}

template <class T>
G4bool G4FSALIntegrationDriver<T>::
QuickAdvance( G4FieldTrack& fieldTrack, const G4double dydxIn[],
              G4double hstep,
              G4double& dchord_step, G4double& dyerr )
{
  ++fNoQuickAvanceCalls;

  if (hstep == 0)
  {
    std::ostringstream message;
    message << "Proposed step is zero; hstep = " << hstep << " !";
    G4Exception("G4FSALIntegrationDriver ::QuickAdvance()",
                "GeomField1001", JustWarning, message);
    return true;
  }
  if (hstep < 0)
  {
    std::ostringstream message;
    message << "Invalid run condition." << G4endl
            << "Proposed step is negative; hstep = "
            << hstep << "." << G4endl
            << "Requested step cannot be negative! Aborting event.";
    G4Exception("G4FSALIntegrationDriver ::QuickAdvance()",
                "GeomField0003", EventMustBeAborted, message);
    return false;
  }

  G4double yError[G4FieldTrack::ncompSVEC],
           yIn[G4FieldTrack::ncompSVEC],
           yOut[G4FieldTrack::ncompSVEC],
           dydxOut[G4FieldTrack::ncompSVEC];

  fieldTrack.DumpToArray(yIn);

  Base::GetStepper()->Stepper(yIn, dydxIn, hstep, yOut, yError, dydxOut);
  dchord_step = Base::GetStepper()->DistChord();

  fieldTrack.LoadFromArray(yOut, Base::GetStepper()->GetNumberOfVariables());
  fieldTrack.SetCurveLength(fieldTrack.GetCurveLength() + hstep);

  dyerr = field_utils::absoluteError(yOut, yError, hstep);

  return true;
}

template <class T>
void G4FSALIntegrationDriver<T>::SetSmallestFraction(G4double newFraction)
{
  if( newFraction > 1.e-16 && newFraction < 1e-8 )
  {
    fSmallestFraction = newFraction;
  }
  else
  {
    std::ostringstream message;
    message << "Smallest Fraction not changed. " << G4endl
            << "  Proposed value was " << newFraction << G4endl
            << "  Value must be between 1.e-8 and 1.e-16";
    G4Exception("G4FSALIntegrationDriver::SetSmallestFraction()",
                "GeomField1001", JustWarning, message);
  }
}

template <class T>
void G4FSALIntegrationDriver<T>::CheckStep(
    const G4ThreeVector& posIn, const G4ThreeVector& posOut, G4double hdid)
{
  const G4double endPointDist = (posOut - posIn).mag();
  if (endPointDist >= hdid * (1. + CLHEP::perMillion))
  {
    ++fNoAccurateAdvanceBadSteps;
#ifdef G4DEBUG_FIELD
    // Issue a warning only for gross differences -
    // we understand how small difference occur.
    if (endPointDist >= hdid * (1. + perThousand))
    {
      G4Exception("G4FSALIntegrationDriver::CheckStep()",
                  "GeomField1002", JustWarning, "endPointDist >= hdid!");
    }
#endif
  }
  else
  {
    ++fNoAccurateAdvanceGoodSteps;
  }
}

template <class T>
inline G4double G4FSALIntegrationDriver<T>::GetMinimumStep() const
{
  return fMinimumStep;
}

template <class T>
void G4FSALIntegrationDriver<T>::SetMinimumStep(G4double minimumStepLength)
{
  fMinimumStep = minimumStepLength;
}

template <class T>
G4int G4FSALIntegrationDriver<T>::GetVerboseLevel() const
{
  return fVerboseLevel;
} 

template <class T>
void G4FSALIntegrationDriver<T>::SetVerboseLevel(G4int newLevel)
{
  fVerboseLevel = newLevel;
}

template <class T>
G4double G4FSALIntegrationDriver<T>::GetSmallestFraction() const
{
  return fSmallestFraction;
}

template <class T>
void G4FSALIntegrationDriver<T>::IncrementQuickAdvanceCalls()
{
  ++fNoQuickAvanceCalls;
}

template <class T>
G4double
G4FSALIntegrationDriver<T>::AdvanceChordLimited(G4FieldTrack& track,
                                             G4double hstep,
                                             G4double eps,
                                             G4double chordDistance)
{
  return ChordFinderDelegate::AdvanceChordLimitedImpl(track, hstep,
                                                      eps, chordDistance);
}

template <class T>
void G4FSALIntegrationDriver<T>::OnStartTracking()
{
  ChordFinderDelegate::ResetStepEstimate();
}

template <class T>
void G4FSALIntegrationDriver<T>::OnComputeStep(const G4FieldTrack*)
{
}

template <class T>
G4bool G4FSALIntegrationDriver<T>::DoesReIntegrate() const
{
  return true;
} 

template <class T>
void G4FSALIntegrationDriver<T>::StreamInfo( std::ostream& os ) const
{
  // Write out the parameters / state of the driver

  os << "State of G4IntegrationDriver: " << std::endl;
  os << "--Base state (G4RKIntegrationDriver): " << std::endl;
  Base::StreamInfo( os );
  os << "--Own  state (G4IntegrationDriver<>): " << std::endl;  
  os << "    fMinimumStep =      " << fMinimumStep  << std::endl;
  os << "    Smallest Fraction = " << fSmallestFraction << std::endl;
  
  os << "    verbose level     = " << fVerboseLevel << std::endl;    
  os << "    Reintegrates      = " << DoesReIntegrate() << std::endl;
  os << "--Chord Finder Delegate state: " << std::endl;
  ChordFinderDelegate::StreamDelegateInfo( os );
}