/* nag::ad::d02ps Passive Example Program.
 */

#include <dco.hpp>
#include <iostream>
#include <nagad.h>

// Function which calls NAG AD routines.
// Solves the equation y''=-y, y0(=y(0))=0, y0'(=y'(0))=1 by solving the ODE
// system:
//    y1'=r1*y2,  y2'=-r2*y1
// over the range [0,2*pi] with initial conditions y1=0, y2=1.
template <typename T> void func(std::vector<T> &r, std::vector<T> &y);

int main()
{
  std::cout << "nag::ad::d02ps Passive Example Program Results\n\n";

  // Set problem parameters
  std::vector<double> r{1.0, 1.0};
  // Solution y
  std::vector<double> y{0.0, 1.0};

  // Call NAG Lib
  func(r, y);

  // Print outputs
  std::cout.setf(std::ios::scientific, std::ios::floatfield);
  std::cout.precision(6);
  std::cout << "\n Solution computed with required tolerance " << 1e-4
            << std::endl;
  for (std::size_t i = 0; i < y.size(); i++)
  {
    std::cout << " y" << i + 1 << " = " << y[i] << std::endl;
  }
  std::cout << std::endl;

  return 0;
}

// function which calls NAG AD Library routines
template <typename T> void func(std::vector<T> &r, std::vector<T> &y)
{
  const Integer n = y.size(), npts = 16, nwant = 1;
  const Integer liwsav = 130;
  const Integer lrwsav = 350 + 32 * n;
  const Integer lwcomm = n + 5 * nwant;

  std::vector<T> thresh(n, 1e-8), ynow(n), ypnow(n), rwsav(lrwsav),
      wcomm(lwcomm);
  std::vector<Integer> iwsav(liwsav);

  // Set initial conditions for ODE and parameters for the integrator.
  Integer method = -1;
  T       tol, hstart, tend, tstart;
  tstart = 0.0;
  tol    = 1.0e-4;
  tend   = 2.0 * nag_math_pi;
  hstart = 0.0;

  // Create AD configuration data object
  Integer           ifail = 0;
  nag::ad::handle_t ad_handle;

  // Initialize Runge-Kutta method for integrating ODE
  ifail = 0;
  nag::ad::d02pq(ad_handle, n, tstart, tend, y.data(), tol, thresh.data(),
                 method, hstart, iwsav.data(), rwsav.data(), ifail);

  auto f = [&](nag::ad::handle_t &ad_handle,
              const T &          t,
              const Integer &    n,
              const T            y[],
              T                  yp[])
            {
              yp[0] = r[0] * y[1];
              yp[1] = -r[1] * y[0];
            };

  T tnow, twant, tinc;
  tinc  = (tend - tstart) / ((double)npts);
  twant = tstart + tinc;
  tnow  = tstart;
  while (tnow < tend)
  {
    ifail = 0;
    nag::ad::d02pf(ad_handle, f, n, tnow, y.data(), ypnow.data(), iwsav.data(), rwsav.data(), ifail);
    while (twant <= tnow)
    {
      Integer ideriv = 2;
      ifail          = 0;
      nag::ad::d02ps(ad_handle, n, twant, ideriv, nwant, y.data(), ypnow.data(),
                     f, wcomm.data(), lwcomm, iwsav.data(), rwsav.data(), ifail);

      twant = twant + tinc;
    }
  }
}