//
// ********************************************************************
// * 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.          *
// ********************************************************************
//
// G4BulirschStoer driver inline methods implementation

// Author: Dmitry Sorokin (CERN, Google Summer of Code 2016), 13.02.2018
// Supervision: John Apostolakis (CERN)
// --------------------------------------------------------------------

#include <cassert>

#include "G4LineSection.hh"
#include "G4FieldUtils.hh"

G4IntegrationDriver<G4BulirschStoer>::
G4IntegrationDriver( G4double hminimum, G4BulirschStoer* stepper,
                     G4int numberOfComponents, G4int statisticsVerbosity )
  : fMinimumStep(hminimum),
    fVerbosity(statisticsVerbosity),
    fMidpointMethod(stepper->GetEquationOfMotion(),
                    stepper->GetNumberOfVariables()),
    bulirschStoer(stepper),
    interval_sequence{2,4}
{
    assert(stepper->GetNumberOfVariables() == numberOfComponents);
    
    if(stepper->GetNumberOfVariables() != numberOfComponents)
    {
      std::ostringstream msg;
      msg << "Disagreement in number of variables = "
          << stepper->GetNumberOfVariables()
          << " vs no of components = " << numberOfComponents;
      G4Exception("G4IntegrationDriver<G4BulirschStoer> Constructor:",
                  "GeomField1001", FatalException, msg);       
    }
}

G4bool G4IntegrationDriver<G4BulirschStoer>::
AccurateAdvance( G4FieldTrack& track, G4double hstep,
                 G4double eps, G4double hinitial)
{
    // G4int fNoTotalSteps = 0;
    G4int fMaxNoSteps = 10000;
    // G4double fNoBadSteps = 0.0;
    G4double fSmallestFraction = 1.0e-12;

    // 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 .

    //  Ensure that hstep > 0
    if(hstep == 0)
    {
      std::ostringstream message;
      message << "Proposed step is zero; hstep = " << hstep << " !";
      G4Exception("G4IntegrationDriver<G4BulirschStoer>::AccurateAdvance()",
                  "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("G4IntegrationDriver<G4BulirschStoer>::AccurateAdvance()",
                    "GeomField0003", EventMustBeAborted, message);

        return false;
    }

    // init first step size
    //
    G4double h;
    if ( (hinitial > 0) && (hinitial < hstep)
      && (hinitial > perMillion * hstep) )
    {
       h = hinitial;
    }
    else  //  Initial Step size "h" defaults to the full interval
    {
       h = hstep;
    }

    // integration variables
    //
    track.DumpToArray(yCurrent);

    // copy non-integration variables to out array
    //
    std::memcpy(yOut + GetNumberOfVarialbles(),
                yCurrent + GetNumberOfVarialbles(),
         sizeof(G4double)*(G4FieldTrack::ncompSVEC-GetNumberOfVarialbles()));

    G4double startCurveLength = track.GetCurveLength();
    G4double curveLength = startCurveLength;
    G4double endCurveLength = startCurveLength + hstep;

    // loop variables
    //
    G4int nstp = 1; //, no_warnings = 0;
    G4double hnext, hdid;

    G4bool succeeded = true, lastStepSucceeded;

    G4int  noFullIntegr = 0, noSmallIntegr = 0 ;
    // static G4ThreadLocal G4int noGoodSteps = 0 ;  // Bad = chord > curve-len

    G4bool lastStep = false;

    // BulirschStoer->reset();

    G4FieldTrack yFldTrk(track);

    do
    {
        G4ThreeVector StartPos(yCurrent[0], yCurrent[1], yCurrent[2]);
        GetEquationOfMotion()->RightHandSide(yCurrent, dydxCurrent);
        // fNoTotalSteps++;

        // Perform the Integration
        //
        if(h == 0)
        {
          G4Exception("G4IntegrationDriver<G4BulirschStoer>::AccurateAdvance()",
                      "GeomField0003", FatalException,
                      "Integration Step became Zero!");
        }
        else if(h > fMinimumStep)
        {
            // step size if Ok
            //
            OneGoodStep(yCurrent,dydxCurrent,curveLength,h,eps,hdid,hnext);
            lastStepSucceeded = (hdid == h);
        }
        else  // for small steps call QuickAdvance for speed
        {
            G4double dchord_step, dyerr, dyerr_len;  // What to do with these ?
            yFldTrk.LoadFromArray(yCurrent, G4FieldTrack::ncompSVEC);
            yFldTrk.SetCurveLength(curveLength);

            QuickAdvance(yFldTrk, dydxCurrent, h, dchord_step, dyerr_len);

            yFldTrk.DumpToArray(yCurrent);


            dyerr = dyerr_len / h;
            hdid = h;
            curveLength += hdid;

            // Compute suggested new step
            // hnext = ComputeNewStepSize(dyerr/eps, h);
            hnext = h;

            // hnext= ComputeNewStepSize_WithinLimits( dyerr/eps, h);
            lastStepSucceeded = (dyerr <= eps);
        }

        lastStepSucceeded ? ++noFullIntegr : ++noSmallIntegr;

        G4ThreeVector EndPos(yCurrent[0], yCurrent[1], yCurrent[2]);
/*
        // Check the endpoint
        //
        G4double endPointDist = (EndPos - StartPos).mag();
        if (endPointDist >= hdid*(1. + perMillion))
        {
            ++fNoBadSteps;

            // Issue a warning only for gross differences -
            // we understand how small difference occur.
            //
            if (endPointDist >= hdid*(1.+perThousand))
            {
                ++no_warnings;
            }
        }
        else
        {
            ++noGoodSteps;
        }
*/
        //  Avoid numerous small last steps
        //
        if((h < eps * hstep) || (h < fSmallestFraction * startCurveLength))
        {
            // No more integration -- the next step will not happen
            //
            lastStep = true;
        }
        else
        {
            // Check the proposed next stepsize
            //
            if(std::fabs(hnext) < fMinimumStep)
            {
              // Make sure that the next step is at least Hmin
              //
              h = fMinimumStep;
            }
            else
            {
              h = hnext;
            }

            //  Ensure that the next step does not overshoot
            //
            if (curveLength + h > endCurveLength)
            {
              h = endCurveLength - curveLength;
            }

            if (h == 0)
            {
              // Cannot progress - accept this as last step - by default
              //
              lastStep = true;
            }
        }
    } while (((nstp++) <= fMaxNoSteps)
           && (curveLength < endCurveLength) && (!lastStep));
       //
       // Have we reached the end ?
       // --> a better test might be x-x2 > an_epsilon

    succeeded = (curveLength >= endCurveLength);
      // If it was a "forced" last step

    // Copy integrated vars to output array
    //
    std::memcpy(yOut, yCurrent, sizeof(G4double) * GetNumberOfVarialbles());

    // upload new state
    track.LoadFromArray(yOut, G4FieldTrack::ncompSVEC);
    track.SetCurveLength(curveLength);

    if(nstp > fMaxNoSteps)
    {
      // ++no_warnings;
      succeeded = false;
    }

    return succeeded;
}

G4bool G4IntegrationDriver<G4BulirschStoer>::
QuickAdvance( G4FieldTrack& track, const G4double dydx[],
              G4double hstep, G4double& missDist, G4double& dyerr)
{
    const auto nvar = fMidpointMethod.GetNumberOfVariables();

    track.DumpToArray(yIn);
    const G4double curveLength = track.GetCurveLength();

    fMidpointMethod.SetSteps(interval_sequence[0]);
    fMidpointMethod.DoStep(yIn, dydx, yOut, hstep, yMid, derivs[0]);

    fMidpointMethod.SetSteps(interval_sequence[1]);
    fMidpointMethod.DoStep(yIn, dydx, yOut2, hstep, yMid2, derivs[1]);

    // extrapolation
    //
    static const G4double coeff =
        1. / (sqr(static_cast<G4double>(interval_sequence[1]) /
                     static_cast<G4double>(interval_sequence[0])) - 1.);

    for (G4int i = 0; i < nvar; ++i)
    {
        yOut[i] = yOut2[i] + (yOut2[i] - yOut[i]) * coeff;
        yMid[i] = yMid2[i] + (yMid2[i] - yMid[i]) * coeff;
    }

    // calculate chord length
    //
    const auto mid = field_utils::makeVector(yMid,
                     field_utils::Value3D::Position);
    const auto in  = field_utils::makeVector(yIn,
                     field_utils::Value3D::Position);
    const auto out = field_utils::makeVector(yOut,
                     field_utils::Value3D::Position);

    missDist = G4LineSection::Distline(mid, in, out);

    // calculate error
    //
    for (G4int i = 0; i < nvar; ++i)
    {
        yError[i] = yOut[i] - yOut2[i];
    }

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

    // copy non-integrated variables to output array
    //
    std::memcpy(yOut + nvar, yIn + nvar,
                sizeof(G4double) * (G4FieldTrack::ncompSVEC - nvar));

    // set new state
    //
    track.LoadFromArray(yOut, G4FieldTrack::ncompSVEC);
    track.SetCurveLength(curveLength + hstep);

    return true;
}

G4double G4IntegrationDriver<G4BulirschStoer>::
AdvanceChordLimited(G4FieldTrack& track, G4double hstep, G4double eps,
                    G4double chordDistance)
{
    return ChordFinderDelegate::
           AdvanceChordLimitedImpl(track, hstep, eps, chordDistance);
}

void G4IntegrationDriver<G4BulirschStoer>::
OneGoodStep( G4double y[], const G4double dydx[], G4double& curveLength,
             G4double htry, G4double eps, G4double& hdid, G4double& hnext)
{
    hnext = htry;
    G4double curveLengthBegin = curveLength;

    // set maximum allowed error
    //
    bulirschStoer->set_max_relative_error(eps);

    while (true)
    {
        auto res = bulirschStoer->try_step(y, dydx, curveLength, yOut, hnext);
        if (res == G4BulirschStoer::step_result::success)
        {
            break;
        }
    }

    std::memcpy(y, yOut, sizeof(G4double) * GetNumberOfVarialbles());
    hdid = curveLength - curveLengthBegin;
}

void G4IntegrationDriver<G4BulirschStoer>::
OnStartTracking()
{
  ChordFinderDelegate::ResetStepEstimate();
}

void G4IntegrationDriver<G4BulirschStoer>::
OnComputeStep(const G4FieldTrack* /*track*/)
{
}

G4bool G4IntegrationDriver<G4BulirschStoer>::
DoesReIntegrate() const
{
  return false;
}

void G4IntegrationDriver<G4BulirschStoer>::
GetDerivatives( const G4FieldTrack& track, G4double dydx[]) const
{
    G4double y[G4FieldTrack::ncompSVEC];
    track.DumpToArray(y);
    GetEquationOfMotion()->RightHandSide(y, dydx);
}

void G4IntegrationDriver<G4BulirschStoer>::
GetDerivatives( const G4FieldTrack& track, G4double dydx[],
                G4double field[]) const
{
    G4double y[G4FieldTrack::ncompSVEC];
    track.DumpToArray(y);
    GetEquationOfMotion()->EvaluateRhsReturnB(y, dydx, field);  
}

void G4IntegrationDriver<G4BulirschStoer>::SetVerboseLevel(G4int level)
{
    fVerbosity = level;
}

G4int G4IntegrationDriver<G4BulirschStoer>::GetVerboseLevel() const
{
    return fVerbosity;
}

G4double G4IntegrationDriver<G4BulirschStoer>::
ComputeNewStepSize( G4double /* errMaxNorm*/, G4double  hstepCurrent)
{
    return hstepCurrent;
}

G4EquationOfMotion* G4IntegrationDriver<G4BulirschStoer>::GetEquationOfMotion()
{
    assert(bulirschStoer->GetEquationOfMotion() ==
           fMidpointMethod.GetEquationOfMotion());

    return bulirschStoer->GetEquationOfMotion();
}

const G4EquationOfMotion*
G4IntegrationDriver<G4BulirschStoer>::GetEquationOfMotion() const
{
    return const_cast<G4IntegrationDriver<G4BulirschStoer>*>(this)->
            GetEquationOfMotion();
}

void G4IntegrationDriver<G4BulirschStoer>::
SetEquationOfMotion( G4EquationOfMotion* equation )
{
    bulirschStoer->SetEquationOfMotion(equation);
    fMidpointMethod.SetEquationOfMotion(equation);
}

G4int G4IntegrationDriver<G4BulirschStoer>::GetNumberOfVarialbles() const
{
    assert(bulirschStoer->GetNumberOfVariables() ==
           fMidpointMethod.GetNumberOfVariables());

    return bulirschStoer->GetNumberOfVariables();
}

const G4MagIntegratorStepper*
G4IntegrationDriver<G4BulirschStoer>::GetStepper() const
{
    return nullptr;
}

G4MagIntegratorStepper*
G4IntegrationDriver<G4BulirschStoer>::GetStepper()
{
    return nullptr;
}

void
G4IntegrationDriver<G4BulirschStoer>::StreamInfo( std::ostream& os ) const
{
  os << "State of G4IntegrationDriver<G4BulirschStoer>: " << std::endl;
  os << "   Method is implemented, but gives no information. " << std::endl;
}