*
* 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, stats;
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]

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(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 = 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;
}