/* F11JB_T1W_F C++ Header Example Program.
 *
 * Copyright 2022 Numerical Algorithms Group.
 * Mark 28.5, 2022.
 */
#include <dco.hpp>
#include <iostream>
#include <nag.h>
#include <nagad.h>
#include <nagx04.h>
#include <stdio.h>
using namespace std;

extern "C"
{
  static void NAG_CALL do_rcm(nag::ad::handle_t &ad_handle,
                              Integer &          n,
                              Integer &          nnz,
                              Integer            irow[],
                              Integer            icol[],
                              nagad_t1w_w_rtype  a[],
                              nagad_t1w_w_rtype  y[],
                              Integer            istr[],
                              Integer            perm_fwd[],
                              Integer            perm_inv[],
                              Integer            iwork[]);
}

int main()
{
  int               exit_status = 0;
  nag::ad::handle_t ad_handle;
  Integer           ifail = 0;

  cout << "F11JB_T1W_F C++ Header Example Program Results\n\n";
  // Skip heading in data file
  string mystr;
  getline(cin, mystr);

  // Read order of matrix and number of nonzero entries
  Integer n, nnz;
  cin >> n;
  cin >> nnz;

  Integer            la     = 3 * nnz;
  Integer            liwork = 2 * la + 7 * n + 1;
  nagad_t1w_w_rtype *a = 0, *x = 0, *y = 0;
  double *           ar = 0, *yr = 0, *dxdy = 0;
  Integer *          icol = 0, *ipiv = 0, *irow = 0, *istr = 0, *iwork = 0;
  Integer *          perm_fwd = 0, *perm_inv = 0;

  a        = new nagad_t1w_w_rtype[la];
  x        = new nagad_t1w_w_rtype[n];
  y        = new nagad_t1w_w_rtype[n];
  icol     = new Integer[la];
  ipiv     = new Integer[n];
  irow     = new Integer[la];
  istr     = new Integer[n + 1];
  iwork    = new Integer[liwork];
  perm_fwd = new Integer[n];
  perm_inv = new Integer[n];
  ar       = new double[la];
  yr       = new double[n];
  dxdy     = new double[n * n];

  // Read the matrix A

  for (int i = 0; i < nnz; i++)
  {
    cin >> ar[i] >> irow[i] >> icol[i];
    a[i] = ar[i];
  }

  // Read the vector y
  for (int i = 0; i < n; i++)
  {
    cin >> yr[i];
    y[i] = yr[i];
  }

  // Create AD configuration data object
  ifail = 0;

  // Calculate Cholesky factorization
  Integer           lfill = -1;
  Integer           nnzc, npivm;
  nagad_t1w_w_rtype dscale, dtol;
  dtol   = 0.0;
  dscale = 0.0;

  // Compute reverse Cuthill-McKee permutation for bandwidth reduction
  do_rcm(ad_handle, n, nnz, irow, icol, a, y, istr, perm_fwd, perm_inv, iwork);

  double inc = 1.0, zero = 0.0;
  for (int i = 0; i < n; i++)
  {
    dco::derivative(y[perm_inv[i]]) = inc;

    ifail = 0;
    nag::ad::f11ja(ad_handle, n, nnz, a, la, irow, icol, lfill, dtol, "N",
                   dscale, "M", ipiv, istr, nnzc, npivm, iwork, liwork, ifail);

    // Check the output value of NPIVM
    if (npivm > 0)
    {
      cout << " Factorization is not complete" << endl;
      goto END;
    }

    // Solve P L D L^T P^T x = y
    ifail = 0;
    nag::ad::f11jb(ad_handle, n, a, la, irow, icol, ipiv, istr, "C", y, x,
                   ifail);

    dco::derivative(y[perm_inv[i]]) = zero;

    for (int j = 0; j < n; j++)
    {
      Integer k = i * n + j;
      dxdy[k]   = dco::derivative(x[perm_inv[j]]);
    }
  }

  // Output results
  cout.setf(ios::scientific, ios::floatfield);
  cout.precision(4);
  cout << "  Solution vector" << endl;
  for (int i = 0; i < n; ++i)
  {
    cout.width(12);
    cout << dco::value(x[perm_inv[i]]) << endl;
  }

  cout << "\n Derivatives calculated: First order tangents\n";
  cout << " Computational mode    : algorithmic\n";
  cout << "\n Derivatives of solution X w.r.t RHS Y:\n";

  // Print derivatives
  cout << endl;
  NagError fail;
  INIT_FAIL(fail);
  x04cac(Nag_ColMajor, Nag_GeneralMatrix, Nag_NonUnitDiag, n, n, dxdy, n,
         "       dx_i/dy_j", 0, &fail);

END:

  ifail = 0;

  delete[] a;
  delete[] x;
  delete[] y;
  delete[] icol;
  delete[] ipiv;
  delete[] irow;
  delete[] istr;
  delete[] iwork;
  delete[] ar;
  delete[] yr;
  delete[] dxdy;
  delete[] perm_fwd;
  delete[] perm_inv;

  return exit_status;
}

static void NAG_CALL do_rcm(nag::ad::handle_t &ad_handle,
                            Integer &          n,
                            Integer &          nnz,
                            Integer            irow[],
                            Integer            icol[],
                            nagad_t1w_w_rtype  a[],
                            nagad_t1w_w_rtype  y[],
                            Integer            istr[],
                            Integer            perm_fwd[],
                            Integer            perm_inv[],
                            Integer            iwork[])
{
  logical lopts[5];
  lopts[0] = 0;
  lopts[1] = 0;
  lopts[2] = 1;
  lopts[3] = 1;
  lopts[4] = 1;

  nagad_t1w_w_rtype *rwork = 0;
  Integer            info[4], mask[1];

  // SCS to CS, must add the upper triangle entries.
  Integer j = nnz;
  for (Integer i = 0; i < nnz; i++)
  {
    if (irow[i] > icol[i])
    {
      // strictly lower triangle, add the transposed
      a[j]    = a[i];
      irow[j] = icol[i];
      icol[j] = irow[i];
      j++;
    }
  }

  Integer nnz_cs = j;

  // Reorder, CS to CCS, icolzp in istr
  Integer ifail = 0;
  nag::ad::f11za(ad_handle, n, nnz_cs, a, icol, irow, "F", "F", istr, iwork,
                 ifail);

  // Calculate reverse Cuthill-McKee
  ifail = 0;
  nag::ad::f11ye(ad_handle, n, nnz_cs, istr, irow, lopts, mask, perm_fwd, info,
                 ifail);

  // compute inverse perm, in perm_inv
  for (int i = 0; i < n; i++)
  {
    perm_fwd[i]           = perm_fwd[i] - 1;
    perm_inv[perm_fwd[i]] = i;
  }

  // Apply permutation on column/row indices
  Integer *iswapc = 0, *iswapr = 0;
  iswapc = new Integer[nnz_cs];
  iswapr = new Integer[nnz_cs];
  for (int i = 0; i < nnz_cs; i++)
  {
    iswapc[i] = perm_inv[icol[i] - 1];
    iswapr[i] = perm_inv[irow[i] - 1];
  }
  for (int i = 0; i < nnz_cs; i++)
  {
    icol[i] = iswapc[i] + 1;
    irow[i] = iswapr[i] + 1;
  }
  delete[] iswapc;
  delete[] iswapr;

  // restrict to lower triangle, SCS format
  // copying entries upwards
  j = 0;
  for (Integer i = 0; i < nnz_cs; i++)
  {
    if (irow[i] >= icol[i])
    {
      // non-upper triangle, bubble up
      a[j]    = a[i];
      icol[j] = icol[i];
      irow[j] = irow[i];
      j++;
    }
  }

  Integer nnz_scs = j;
  // sort
  ifail = 0;
  nag::ad::f11zb(ad_handle, n, nnz_scs, a, irow, icol, "S", "K", istr, iwork,
                 ifail);

  // permute rhs vector
  rwork = new nagad_t1w_w_rtype[n];
  for (int i = 0; i < n; i++)
  {
    rwork[i] = y[perm_fwd[i]];
  }
  for (int i = 0; i < n; i++)
  {
    y[i] = rwork[i];
  }
  delete[] rwork;
  return;
}