package com.dwave.math;

 /**
  * Richardson extrapolation uses the powerful idea of extrapolating a computed
  * result to the value that would have been obtained if the stepsize had been very
  * much smaller than it actually was. In particular, extrapolation to zero stepsize is
  * the desired goal. The first practical ODE integrator that implemented this idea was
  * developed by Bulirsch and Stoer, and so extrapolation methods are often call
  * Bulirsch-Stoer methods.
  * <p>
  * Translated to Java directly from Numerical Recipes in C.
  */
public final class BulirschStoer {
  private static float[] x;
  private static float[][] d;

  /**
   *  Private constructor to guard against instantiation
   */
  private BulirschStoer() {}


               //////////////////////////////
              ///// ALGORITHM ROUTINES /////
             //////////////////////////////

  /**
   *  Modified midpoint step. At <code>xs</code>, input the dependent variable vector
   *  <code>y[1..nvar]</code> and its derivative vector <code>dydx[1..nvar]</code>.
   *  Also inputt is <code>htot</code>, the total step to be made, and <code>nstep</code>, the
   *  number of substeps to be used. The output is returned as <code>yout[1..nvar]</code>, which need not
   *  be a distinct array from <code>y</code>; if it is distinct, however, then <code>y</code>
   *  and <code>dydx</code> are returned undamaged.
   */
  public static void mmid(float[] y, float[] dydx, int nvar, float xs, float htot,
                            int nstep, float[] yout, ODEInterface derivs) {
    int n, i;
    float xmid, swap, h2, h;
    float[] ym = new float[nvar],
            yn = new float[nvar];
    h = htot/nstep; // stepsize this trip

    for(i=0; i < nvar; i++) {
      ym[i] = y[i];
      yn[i] = y[i] + h*dydx[i]; // first step
    }
    xmid = xs+h;
    derivs.deriv(xmid, yn, yout); // will use yout for temporary storage of derivatives
    h2 = 2f * h;
    for(n=2; n <= nstep; n++) { // general step
      for(i=0; i < nvar; i++) {
        swap = ym[i] + h2*yout[i];
        ym[i] = yn[i];
        yn[i] = swap;
      }
      xmid += h;
      derivs.deriv(xmid, yn, yout);
    }
    for(i=0; i < nvar; i++)  // last step
      yout[i] = 0.5f * (ym[i] +yn[i] + h*yout[i]);
  }


              /////////////////////////////////
             ///// EXTRAPOLATION METHODS /////
            /////////////////////////////////


  /**
   *  Use polynomial extrapolation to evaluate <code>nv</code> functions at <i>x=0</i> by fitting
   *  a polynomial to a sequence of estimates with progressively smaller values <i>x=</i><code>xest</code>,
   *  and corresponding function vectors <code>yest[1..nv]</code>. This call is number <code>iest</code>
   *  in the sequence of calls. Extrapolated function values are output as <code>yz[1..nv]</code>,
   *  and their estimated eror is output as <code>dy[1..nv]</code>.
   */
  public static void pzextr(int iest, float xest, float[] yest, float[] yz, float[] dy, int nv) {
    int k1, j;
    float q, f2, f1, delta;
    float[] c = new float[nv];

    x[iest] = xest; // save current independent variable

    for(j=0; j < nv; j++)
      dy[j] = yz[j] = yest[j];
    if(iest == 1) // Store first estimate in first column
      for(j = 0; j < nv; j++)
        d[j][0] = yest[j];
    else {
      for(j=0; j < nv; j++)
        c[j] = yest[j];
      for(k1=0; k1< iest;k1++) {
        delta = 1.0f/(x[iest-k1] - xest);
        f1 = xest * delta;
        f2 = x[iest-k1] * delta;
        for(j=0; j<nv; j++) { // propagate tableau 1 diagonal more
          q = d[j][k1];
          d[j][k1] = dy[j];
          delta = c[j] - q;
          dy[j] = f1*delta;
          c[j] = f2*delta;
          yz[j] += dy[j];
        }
      }
      for(j=0; j<nv; j++)
        d[j][iest] = dy[j];
    }
  }

  /**
   *  Exact substitute for <code>pzextr</code>, but uses diagonal rational function
   *  extrapolation instead of polynomial extrapolation
   
  public static void rxextr(int iest, float xest, float[] yest, float[] yz, float[] dy, int nv) {
    int k, j;
    float yy, v, ddy, c, b1, b;
    float[] fx = new float[iest];
    x[iest] = xest; // save current independent variable

    if(iest == 1)
      for(j=1; j <= nv; j++) {
        yz[j] = yest[j];
        d[j][1] = yest[j];
        dy[j] = yest[j];
      }
    else {
      for(k=1; k <iest; k++)
        fx[k+1] = x[iest-k]/xest;
      for(j=1; j<= nv; j++) {// Evaluate next diagonal in tableau
        v = d[j][1];
        d[j][1] = yy = c = yest[j];
        for(k=2; k <=iest; k++) {
          b1 = fx[k]*v;
          b=b1 - c;
          if(b != 0) {
            b = (c-v) / b;
            ddy = c*b;
            c = b1*b;
          }
          else // care needed to avoid division by 0
            ddy = v;
          if(k != iest)
            v = d[j][k];
            d[j][k] = ddy;
            yy += ddy;
        }
      }
    }
  }  */


               ////////////////////
              ///// STEPPERS /////
             ////////////////////

  /**
   *  Bulirsch-Stoer step with monitoring of local truncation error to ensure accuracy and adjust
   *  stepsize. Input are the dependennt variable vector <code>y[1..nv]</code> and its derivative
   *  <code>dydx[1..nv]</code> at the starting value of the independent variable <code>x</code>.
   *  Also input are the stepsize to be attempted <code>htry</code>, the required accuracy
   *  <code>eps</code>, and the vector <code>yscal[1..nv]</code> against which the errror is
   *  scaled. On output, <code>y</code> and <code>x</code> are replaced by their new values,
   *  <code>hdid</code> is the stepsize that was actually accomplishhed, and <code>hnext</code>
   *  is the estimated next stepsize. <code>derivs</code> is the user-supplied routine that
   *  computes the right-hand side derivatives. Be sure to set <code>htry</code> on successive steps
   *  to the value of <code>hnext</code> returned from the previous step, as is the case
   *  if the routine is called by <code>odeint</code>.
   */
  public static void bsstep(float[] y, float[] dydx, int nv, float[] xx, float htry,
                              float eps, float yscal[], float[] hdid, float[] hnext, ODEInterface derivs) {
    int KMAXX = 8;          // Max row number used
    int IMAXX = KMAXX + 1;  //    in the extrapolation
    float SAFE1 = 0.25F,    // Safety factors
          SAFE2 = 0.7F,
          REDMAX = 1.0e-5F, // Maximum factor for stepsize reduction
          REDMIN = 0.7F,    // Minimum factor for stepsize reduction
          TINY = 1.0E-30F,  // Prevents division by zero
          SCALMX = 0.1F;    // 1/SCALMX is the max factor by which a stepsize can be increased

    int i, iq, k, kk, km=0;
    int kmax=0, kopt=0;
    float epsold = -1.0f;
    float eps1, errmax=0f, fact, h, red=0f, scale=0f, work, wrkmin, xest, xnew=0f;
    float[] err, yerr, ysav, yseq;
    float[] a = new float[IMAXX+1];
    float[][] alf = new float[KMAXX+1][KMAXX+1];
    int nseq[] = {0, 2, 4, 6, 8, 10, 12, 14, 16, 18};
    boolean reduct = false;
    boolean exitflag = false;
    boolean first = true;

    d = new float[nv][KMAXX];
    err=new float[KMAXX];
    x=new float[KMAXX];
    yerr=new float[nv];
    ysav=new float[nv];
    yseq=new float[nv];

    if (eps != epsold) { // A new tolerance,  so reinitialize.
      hnext[0] =-1.0e29f; // "Impossible" values.
      xnew = -1.0e29f; // "Impossible" values.

      eps1 = SAFE1*eps;
      a[0] = nseq[0]+1; //Compute work coeffcients A_k

      for (k = 0;k < KMAXX;k++)
        a[k+1] = a[k]+nseq[k+1];
      for (iq = 1; iq < KMAXX; iq++) { // Compute (k, q).
        for (k = 0; k < iq; k++)
          alf[k][iq] = (float) Math.pow(eps1, (a[k+1]-a[iq+1]) / ((a[iq+1]-a[0]+1.0)*(2*k+1)));
      }
      epsold = eps;
      for (kopt = 1; kopt < KMAXX-1; kopt++) //Determine optimal row number for convergence.
        if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt])
          break;
      kmax = kopt;
    }
    h = htry;
    for (i = 0;i < nv;i++)
      ysav[i] = y[i]; // Save the starting values.
    if (xx[0] != xnew || h != hnext[0]) { //A new stepsize or a new integration: re-establish the order window.
      first = true;
      kopt = kmax;
    }
    reduct = false;

    for (;;) {
      for (k = 0;k < kmax;k++) { // Evaluate the sequence of modified midpoint integrations.
        xnew = xx[0] + h;
        if (xnew  ==  xx[0]) {
          System.out.println("step size underflow in bsstep");
          return;
        }

        mmid(ysav, dydx, nv, xx[0], h, nseq[k], yseq, derivs);

        xest = (h/nseq[k])*(h/nseq[k]); // Squared,  since error series is even.

        pzextr(k, xest, yseq, y, yerr, nv); // Perform extrapolation.

        if (k !=  0) { // Compute normalized error estimate (k).
          errmax = TINY;
          for (i = 0;i < nv;i++)
            errmax = Math.max(errmax, Math.abs(yerr[i]/yscal[i]));
          errmax /=  eps; // Scale error relative to tolerance.
          km = k-1;
          err[km] = (float)Math.pow(errmax/SAFE1, 1.0/(2*km+1));
        }
        if (k !=  0 && (k >=  kopt-1 || first)) { // In order window.
          if (errmax < 1.0) { // Converged.
            exitflag = true;
            break;
          }
          if (k  ==  kmax || k  ==  kopt+1) { // Check for possible stepsize reduction.
            red = SAFE2/err[km];
            break;
          }
          else if (k  ==  kopt && alf[kopt-1][kopt] < err[km]) {
            red = 1.0f/err[km];
            break;
          }
          else if (kopt  ==  kmax && alf[km][kmax-1] < err[km]) {
            red = alf[km][kmax-1]*SAFE2/err[km];
            break;
          }
          else if (alf[km][kopt] < err[km]) {
            red = alf[km][kopt-1]/err[km];
            break;
          }
        }
      }
      if (exitflag)
        break;
      red = Math.min(red, REDMIN); // Reduce stepsize by at least REDMIN and at most REDMAX.
      red = Math.max(red, REDMAX);
      h *=  red;
      reduct = true;
    } // Try again.
    xx[0] = xnew; // Successful step taken.
    hdid[0] = h;
    first = false;
    wrkmin = 1.0e35f; // Compute optimal row for convergence and corresponding stepsize.

    for (kk = 0;kk < km; kk++) {
      fact = Math.max(err[kk], SCALMX);
      work = fact*a[kk+1];
      if (work < wrkmin) {
        scale = fact;
        wrkmin = work;
        kopt = kk+1;
      }
    }
    hnext[0] = h/scale;
    if (kopt >=  k && kopt  ==  kmax && !reduct) { //Check for possible order increase,  but not if stepsize was just reduced.
      fact = Math.max(scale/alf[kopt-1][kopt], SCALMX);
      if (a[kopt+1]*fact <=  wrkmin) {
        hnext[0] = h/fact;
        kopt++;
      }
    }
  }

               ///////////////////
              ///// DRIVERS /////
             ///////////////////
}








  /**
   *  Stoermer's rule for integrating <i>y'' = f(x, y)</i> for a system of <i>n = </i><code>nv</code><i>/2</i>
   *  equations. On input <code>y[1..nv]</code> contains <i>y</i> in its first <i>n</i> elements and
   *  <i>y'</i> in its second <i>n</i> elements, all evaluated at <code>xs. d2y[1..nv]</code>
   *  contains the right-hand side function <i>f</i> (also evaluated at <code>xs</code>)
   *  in its first <i>n</i> elements. Its second <i>n</i> elements are not referneced.
   *  Also input is <code>htot</code>, the total step to be taken, and <code>nstep</code>,
   *  the number of substeps to be used. The output is returned as <code>yout[1..nv]</code>
   *  with the same storage arrangements as <code>y. derivs</code> is the user-supplied
   *  routine that calculates <i>f</i>.

  public static void stoerm(float[] y, float[] d2y, int nv, float xs, float htot, int nstep,
                              float[] yout, ODEInterface derivs) {
    int i, n, neqns, nn;
    float h, h2, halfh, xstoerm;
    float[] ytemp = new float[nv];
    h = htot/nstep; // stepsize this trip
    halfh= 0.5f*h;
    neqns=nv/2; // number of equations

    for(i=1;i<=neqns;i++) { // first step
      n=neqns+i;
      ytemp[n] = h*(y[n] + halfh*d2y[i]);
      ytemp[i] = y[i]+ytemp[n];
    }
    xstoerm = xs+h;
    derivs.deriv(xstoerm, ytemp, yout); // use yout for temporary storage of derivatives
    h2=h*h;

    for(nn=2; nn<=nstep; nn++) { // general step
      for(i=1; i <=neqns; i++) {
        n=neqns+1;
        ytemp[n] += h2*yout[i];
        ytemp[i] += ytemp[n];
      }
      xstoerm += h;
      derivs.deriv(xstoerm, ytemp, yout);
    }
    for(i=1; i <=neqns;i++) { // last step
      n=neqns+1;
      yout[n] = ytemp[n]/h + halfh*yout[i];
      yout[i] = ytemp[i];
    }
  }   */