//
// ********************************************************************
// * 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.          *
// ********************************************************************
//
// G4MagErrorStepper implementation
//
// Author: W.Wander (MIT), 09.12.1997
// --------------------------------------------------------------------

#include "G4MagErrorStepper.hh"
#include "G4LineSection.hh"

G4MagErrorStepper::~G4MagErrorStepper()
{
   delete [] yMiddle;
   delete [] dydxMid;
   delete [] yInitial;
   delete [] yOneStep;
}

void G4MagErrorStepper::Stepper( const G4double yInput[],
                                 const G4double dydx[],
                                       G4double hstep,
                                       G4double yOutput[],
                                       G4double yError [] )
{  
   const G4int nvar = GetNumberOfVariables();
   const G4int maxvar = GetNumberOfStateVariables();

   // correction for Richardson Extrapolation.
   //
   G4double  correction = 1. / ( (1 << IntegratorOrder()) -1 );
   
   //  Saving yInput because yInput and yOutput can be aliases for same array
   //
   for(G4int i=0; i<nvar; ++i)
   {
     yInitial[i]=yInput[i];
   }
   yInitial[7] = yInput[7]; // Copy the time in case...even if not really needed
   yMiddle[7] = yInput[7];  // Copy the time from initial value 
   yOneStep[7] = yInput[7]; // As it contributes to final value of yOutput ?
   // yOutput[7] = yInput[7];  // -> dumb stepper does it too for RK4

   for(G4int i=nvar; i<maxvar; ++i)
   {
     yOutput[i]=yInput[i];
   }
   // yError[7] = 0.0;         

   G4double halfStep = hstep * 0.5; 

   // Do two half steps
   //
   DumbStepper  (yInitial,  dydx,   halfStep, yMiddle);
   RightHandSide(yMiddle, dydxMid);    
   DumbStepper  (yMiddle, dydxMid, halfStep, yOutput); 

   // Store midpoint, chord calculation
   //
   fMidPoint = G4ThreeVector( yMiddle[0],  yMiddle[1],  yMiddle[2]); 

   // Do a full Step
   //
   DumbStepper(yInitial, dydx, hstep, yOneStep);
   for(G4int i=0; i<nvar; ++i)
   {
      yError [i] = yOutput[i] - yOneStep[i] ;
      yOutput[i] += yError[i]*correction ;
        // Provides accuracy increased by 1 order via Richardson Extrapolation  
   }

   fInitialPoint = G4ThreeVector( yInitial[0], yInitial[1], yInitial[2]); 
   fFinalPoint   = G4ThreeVector( yOutput[0],  yOutput[1],  yOutput[2]); 

   return;
}

G4double G4MagErrorStepper::DistChord() const 
{
  // Estimate the maximum distance from the curve to the chord
  //
  // We estimate this using the distance of the midpoint to 
  // chord (the line between 
  // 
  // Method below is good only for angle deviations < 2 pi, 
  // This restriction should not a problem for the Runge cutta methods, 
  // which generally cannot integrate accurately for large angle deviations.

  G4double distLine, distChord; 

  if (fInitialPoint != fFinalPoint)
  {
     distLine = G4LineSection::Distline(fMidPoint, fInitialPoint, fFinalPoint);
       // This is a class method that gives distance of Mid 
       // from the Chord between the Initial and Final points.

     distChord = distLine;
  }
  else
  {
     distChord = (fMidPoint-fInitialPoint).mag();
  }

  return distChord;
}
