/* nag_opt_handle_set_quadobj (e04rfc) Example Program.
 *
 * Copyright 2017 Numerical Algorithms Group.
 *
 * Mark 26.1, 2017.
 */

/* Compute the nearest correlation matrix in Frobenius norm using nonlinear
 * semidefinite programming. By default, preserve the nonzero structure of
 * the input matrix (preserve_structure = 1).
 */

#include <stdio.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <nage04.h>
#include <nagx04.h>

int main(void)
{
  Integer preserve_structure = 1;

  Integer exit_status = 0;
  Integer dima, i, idblk, idx, inform, j, n, nblk, nnzasum, nnzh, nnzu,
         nnzua, nnzuc, nvar;
  double rinfo[32], stats[32];
  double *a = 0, *g = 0, *h = 0, *x = 0;
  Integer *icola = 0, *icolh = 0, *irowa = 0, *irowh = 0, *nnza = 0;
  void *handle = 0;
  /* Nag Types */
  NagError fail;

#define G(I,J) g[(J-1)*n + I-1]

  printf("nag_opt_handle_set_quadobj (e04rfc) Example Program Results\n\n");
  fflush(stdout);

  /* Skip heading in data file. */
  scanf("%*[^\n]");
  /* Read in the problem size and ignore the rest of the line. */
  scanf("%" NAG_IFMT "%*[^\n]", &n);

  /* Allocate memory for matrix G and read it in. */
  if (!(g = NAG_ALLOC(n * n, double)))
  {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }
  for (i = 1; i <= n; i++)
    for (j = 1; j <= n; j++)
      scanf("%lf", &G(i, j));
  scanf("%*[^\n]");

  /* Symmetrize G:  G = (G + G')/2 */
  for (j = 1; j <= n; j++)
    for (i = 1; i <= j - 1; i++) {
      G(i, j) = (G(i, j) + G(j, i)) / 2.0;
      G(j, i) = G(i, j);
    }

  /* There are as many variables as nonzeros above the main diagonal in
   * the input matrix. The variables are corrections of these elements. */
  nvar = 0;
  for (j = 2; j <= n; j++)
    for (i = 1; i <= j - 1; i++)
      if (!preserve_structure || G(i, j) != 0.0)
        nvar++;

  /* nag_opt_handle_init (e04rac).
   * Initialize an empty problem handle with NVAR variables. */
  nag_opt_handle_init(&handle, nvar, NAGERR_DEFAULT);

  /* Set up the objective - minimize Frobenius norm of the corrections.
   * Our variables are stored as a vector thus, just minimize
   * sum of squares of the corrections --> H is identity matrix, c = 0.*/
  nnzh = nvar;
  if (!(x = NAG_ALLOC(nvar, double)) ||
      !(irowh = NAG_ALLOC(nnzh, Integer)) ||
      !(icolh = NAG_ALLOC(nnzh, Integer)) || !(h = NAG_ALLOC(nnzh, double)))
  {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }

  for (i = 0; i < nvar; i++) {
    /* irowh, icolh use 1-based indices */
    irowh[i] = i + 1;
    icolh[i] = i + 1;
    h[i] = 1.0;
  }

  /* nag_opt_handle_set_quadobj (e04rfc).
   * Add the quadratic objective to the handle.*/
  nag_opt_handle_set_quadobj(handle, 0, NULL, NULL, nnzh, irowh, icolh, h,
                             NAGERR_DEFAULT);

  /* Construct linear matrix inequality to request that
   * matrix G with corrections X is positive semidefinite.
   * (Don't forget the sign at A_0!)
   * How many nonzeros do we need? Full triangle for A_0 and
   * one nonzero element for each A_i. */
  nnzasum = n * (n + 1) / 2 + nvar;
  if (!(nnza = NAG_ALLOC(nvar + 1, Integer)) ||
      !(irowa = NAG_ALLOC(nnzasum, Integer)) ||
      !(icola = NAG_ALLOC(nnzasum, Integer)) ||
      !(a = NAG_ALLOC(nnzasum, double)))
  {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }
  nnza[0] = n * (n + 1) / 2;
  for (j = 1; j <= nvar; j++)
    nnza[j] = 1;

  /* Copy G to A_0, only upper triangle with different sign (because -A_0)
   * and set the diagonal to 1.0 as that's what we want independently
   * of what was in G. */
  idx = 0;
  for (j = 1; j <= n; j++) {
    for (i = 1; i <= j - 1; i++) {
      irowa[idx] = i;
      icola[idx] = j;
      a[idx] = -G(i, j);
      idx++;
    }
    /* Unit diagonal. */
    irowa[idx] = j;
    icola[idx] = j;
    a[idx] = -1.0;
    idx++;
  }

  /* A_i has just one nonzero - it binds x_i with its position as
   * a correction. */
  for (j = 2; j <= n; j++)
    for (i = 1; i <= j - 1; i++)
      if (!preserve_structure || G(i, j) != 0.0) {
        irowa[idx] = i;
        icola[idx] = j;
        a[idx] = 1.0;
        idx++;
      }

  /* Just one matrix inequality of the dimension of the original matrix. */
  nblk = 1;
  dima = n;
  idblk = 0;
  /* nag_opt_handle_set_linconstr (e04rnc).
   * Add the linear matrix constraint to the problem formulation. */
  nag_opt_handle_set_linmatineq(handle, nvar, dima, nnza, nnzasum, irowa,
                                icola, a, nblk, NULL, &idblk, NAGERR_DEFAULT);

  /* Set optional arguments of the solver by calling
   * nag_opt_handle_opt_set (e04zmc). */
  nag_opt_handle_opt_set(handle, "Print Options = No", NAGERR_DEFAULT);
  nag_opt_handle_opt_set(handle, "Initial X = Automatic", NAGERR_DEFAULT);

  /* Pass the handle to the solver, we are not interested in
   * Lagrangian multipliers. */
  nnzu = 0;
  nnzuc = 0;
  nnzua = 0;
  INIT_FAIL(fail);
  /* nag_opt_handle_solve_pennon (e04svc). */
  nag_opt_handle_solve_pennon(handle, nvar, x, nnzu, NULL, nnzuc, NULL,
                              nnzua, NULL, rinfo, stats, &inform, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_opt_handle_solve_pennon (e04svc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }

  /* Form the new nearest correlation matrix as the sum
   * of G and the correction X. */
  idx = 0;
  for (j = 1; j <= n; j++) {
    for (i = 1; i <= j - 1; i++)
      if (!preserve_structure || G(i, j) != 0.0) {
        G(i, j) = G(i, j) + x[idx];
        idx++;
      }
    G(j, j) = 1.0;
  }

  /* nag_gen_real_mat_print (x04cac).
   * Print the result as an upper triangular matrix. */
  nag_gen_real_mat_print(Nag_ColMajor, Nag_UpperMatrix, Nag_NonUnitDiag, n, n,
                         g, n, "Nearest Correlation Matrix", NULL,
                         NAGERR_DEFAULT);

END:

  /* nag_opt_handle_free (e04rzc).
   * Destroy the problem handle and deallocate all the memory. */
  if (handle)
    nag_opt_handle_free(&handle, NAGERR_DEFAULT);

  NAG_FREE(a);
  NAG_FREE(g);
  NAG_FREE(h);
  NAG_FREE(x);
  NAG_FREE(icola);
  NAG_FREE(icolh);
  NAG_FREE(irowa);
  NAG_FREE(irowh);
  NAG_FREE(nnza);
  return exit_status;
}