/* F11JB_P0W_F C++ Header Example Program.
 *
 * Copyright 2022 Numerical Algorithms Group.
 * Mark 28.4, 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[],
                              double             a[],
                              double             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_P0W_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;
  double * a = 0, *x = 0, *y = 0;
  Integer *icol = 0, *ipiv = 0, *irow = 0, *istr = 0, *iwork = 0;
  Integer *perm_fwd = 0, *perm_inv = 0;

  a        = new double[la];
  x        = new double[n];
  y        = new double[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];

  // Read the matrix A
  for (int i = 0; i < nnz; i++)
  {
    cin >> a[i] >> irow[i] >> icol[i];
  }

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

  ifail = 0;
  // Calculate Cholesky factorization
  Integer lfill = -1;
  Integer nnzc, npivm;
  double  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);

  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);

  // 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 << x[perm_inv[i]] << endl;
  }

END:

  delete[] a;
  delete[] x;
  delete[] y;
  delete[] icol;
  delete[] ipiv;
  delete[] irow;
  delete[] istr;
  delete[] iwork;
  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[],
                            double             a[],
                            double             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;

  double *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 double[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;
}