/* nag::ad::d02pe Tangent over Tangent Example Program.
 */

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

// Function which calls NAG AD routines.
template <typename T> void func(std::vector<T> &y0, std::vector<T> &y);

// Driver with the tangent calls.
// Solves the equation y''=-y, y0(=y(0))=0, y0'(=y'(0))=1 by solving the ODE
// system:
//    y1'=y2,  y2'=-y1
// over the range [0,2*pi] with initial conditions y1=0, y2=1.
// Also, computes the sum of all Hessian elements d2y/dy02.
void driver(const std::vector<double> &y0v,
            std::vector<double> &      yv,
            double &                   d2y_dy02);

int main()
{
  std::cout << " nag::ad::d02pe Tangent over Tangent Example Program Results\n";

  // Set initial conditions
  std::vector<double> y0v{0.0, 1.0};
  // Solution y
  std::vector<double> yv;
  // Sum of Hessian elements
  double d2y_dy02;

  // Call driver
  driver(y0v, yv, d2y_dy02);

  // Print outputs
  std::cout << "\n Derivatives calculated: Second order tangents\n";
  std::cout << " Computational mode    : algorithmic\n";

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

  // Print derivatives
  std::cout
      << "\n Sum of all Hessian elements of solution vector y w.r.t. initial values:\n";
  std::cout << " sum_ijk [d^2 y/dy0^2]_ijk = " << d2y_dy02 << std::endl;

  return 0;
}

// Driver with the tangent calls.
// Solves the equation y''=-y, y0(=y(0))=0, y0'(=y'(0))=1 by solving the ODE
// system:
//    y1'=y2,  y2'=-y1
// over the range [0,2*pi] with initial conditions y1=0, y2=1.
// Also, computes the sum of all Hessian elements d2y/dy02.
void driver(const std::vector<double> &y0v,
            std::vector<double> &      yv,
            double &                   d2y_dy02)
{
  using mode = dco::gt1s<dco::gt1s<double>::type>;
  using T    = mode::type;

  // Size of system of ODEs
  Integer n = y0v.size();
  // y0 stores the initial conditions of the problem
  std::vector<T> y0(begin(y0v), end(y0v));
  dco::derivative(dco::value(y0)) = std::vector<double>(n, 1.0);
  dco::value(dco::derivative(y0)) = std::vector<double>(n, 1.0);

  // Solution y, variable to differentiate
  std::vector<T> y(n);

  // Call the NAG AD Lib functions
  func(y0, y);

  // Extract the computed solutions
  yv = dco::passive_value(y);

  // Sum the tangents of y
  d2y_dy02 = 0.0;
  for (T const &yi : y)
  {
    d2y_dy02 += dco::derivative(dco::derivative(yi));
  }
}

// function which calls NAG AD Library routines
template <typename T> void func(std::vector<T> &y0, std::vector<T> &y)
{
  // Active variables
  const Integer        n    = y0.size();
  const Integer        npts = 8, liwsav = 130, lrwsav = 350 + 32 * n;
  std::vector<T>       thresh(n, 1e-8), ypgot(n), ymax(n), rwsav(lrwsav);
  std::vector<Integer> iwsav(liwsav);

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

  // Set control for output
  double tinc = 2.0 * nag_math_pi / (double)(npts);
  // 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, y0.data(), tol, thresh.data(),
                 method, hstart, iwsav.data(), rwsav.data(), ifail);

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

  for (int j = 0; j < npts; ++j)
  {
    twant = twant + tinc;
    ifail = 0;
    // Solve an initial value problem for a 1st order system of ODEs
    nag::ad::d02pe(ad_handle, f, n, twant, tgot, y.data(), ypgot.data(),
                   ymax.data(), iwsav.data(), rwsav.data(), ifail);
  }
}