/* nag_opt_handle_solve_socp_ipm (e04ptc) Example Program.
 *
 * Copyright 2019 Numerical Algorithms Group.
 *
 * Mark 27.0, 2019.
 */

#include <stdio.h>
#include <math.h>
#include <nag.h>

#ifdef __cplusplus
extern "C"
{
#endif
static void NAG_CALL monit(void *handle, const double rinfo[],
                           const double stats[], Nag_Comm *comm,
                           Integer *inform);
#ifdef __cplusplus
}
#endif

int main(void){

  Integer n, nnzp0, nnzp1, idxa, rptr, nu, nv, idxf;
  Integer nqc = 1;
  Integer nclin, nvar, nnza, nnzu, nnzuc, exit_status, i, j, x_idx;
  Integer  nvarc1, nvarc2, idgroup = 0;
  Integer idlc;
  Integer verbose_output;
  Integer tol_reached;
  Integer *irowa = 0, *icola = 0;
  Integer *idxc1 = 0, *idxc2 = 0;
  Integer *irowp0 = 0, *icolp0 = 0, *irowp1 = 0, *icolp1 = 0;
  double *c = 0, *a = 0, *bla = 0, *bua = 0, *xl = 0, *xu = 0,
    *x = 0, *u = 0, *uc = 0;
  double *p0 = 0, *p1 = 0, *q0 = 0, *q1 = 0, *f0 = 0, *f1 = 0, *lambda0 = 0,
    *lambda1 = 0;
  Nag_OrderType order = Nag_ColMajor;
  Nag_JobType job = Nag_EigVecs;
  Nag_UploType uplo = Nag_Lower;
  double rinfo[100], stats[100];
  double r1, tol_monit;
  void *handle = 0;
  /* Nag Types */
  Nag_Comm comm;
  NagError fail;

  exit_status = 0;

  printf("nag_opt_handle_solve_socp_ipm (e04ptc) Example Program Results\n\n");
  fflush(stdout);

  /* Read the data file and allocate memory */
  scanf(" %*[^\n]"); /* Skip heading in data file */
  scanf("%" NAG_IFMT" %" NAG_IFMT" %" NAG_IFMT" %*[^\n]",&n,&nnzp0,&nnzp1);

  /* Initialize size of linear constraints in SOCP */
  nclin = nqc;
  nvar = n + nqc + 1;
  nnza = nqc + n;

  /* Initialize size of cone constraints */
  nvarc1 = 2;
  nvarc2 = 2;

  /* Allocate memory to read data */
  if  (!(irowp0 = NAG_ALLOC(nnzp0, Integer))            ||
      !(icolp0 = NAG_ALLOC(nnzp0, Integer))             ||
      !(irowp1 = NAG_ALLOC(nnzp1, Integer))             ||
      !(icolp1 = NAG_ALLOC(nnzp1, Integer))             ||
      !(p0 = NAG_ALLOC(nnzp0,double))                   ||
      !(p1 = NAG_ALLOC(nnzp1,double))                   ||
      !(q0 = NAG_ALLOC(n,double))                       ||
      !(q1 = NAG_ALLOC(n,double))                       ||
      !(f0 = NAG_ALLOC(n*n,double))                     ||
      !(f1 = NAG_ALLOC(n*n,double))                     ||
      !(lambda0 = NAG_ALLOC(n,double))                  ||
      !(lambda1 = NAG_ALLOC(n,double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
  /* Read P0 matrix row indices */
  for (i=0; i<nnzp0; i++){
    scanf("%" NAG_IFMT,&irowp0[i]);
  }
  scanf("%*[^\n]");
  /* Read P0 matrix column indices */
  for (i=0; i<nnzp0; i++){
    scanf("%" NAG_IFMT,&icolp0[i]);
  }
  scanf("%*[^\n]");
  /* Read P0 values*/
  for (i=0; i<nnzp0; i++){
    scanf("%lf",&p0[i]);
  }
  scanf("%*[^\n]");
  /* Read P1 matrix row indices */
  for (i=0; i<nnzp1; i++){
    scanf("%" NAG_IFMT,&irowp1[i]);
  }
  scanf("%*[^\n]");
  /* Read P1 matrix column indices */
  for (i=0; i<nnzp1; i++){
    scanf("%" NAG_IFMT,&icolp1[i]);
  }
  scanf("%*[^\n]");
  /* Read P1 values*/
  for (i=0; i<nnzp1; i++){
    scanf("%lf",&p1[i]);
  }
  scanf("%*[^\n]");
  /* Read q0 values*/
  for (i=0; i<n; i++){
    scanf("%lf",&q0[i]);
  }
  scanf("%*[^\n]");
  /* Read q1 values*/
  for (i=0; i<n; i++){
    scanf("%lf",&q1[i]);
  }
  scanf("%*[^\n]");
  /* Read r1 */
    scanf("%lf",&r1);

  /* Initialize f0 and f1 */
  for (i=0; i<n*n; i++){
    f0[i] = 0.0;
    f1[i] = 0.0;
  }

  /* Store full P0 and P1 in F0 and F1 */
  for (i=0; i<nnzp0; i++){
    idxf = (icolp0[i]-1)*n + irowp0[i] - 1;
    f0[idxf] = p0[i];
  }
  for (i=0; i<nnzp1; i++){
    idxf = (icolp1[i]-1)*n + irowp1[i] - 1;
    f1[idxf] = p1[i];
  }
 
  /* Factorize P0 and P1 via eigenvalue decomposition */
  nag_lapackeig_dsyevd(order,job,uplo,n,f0,n,lambda0,NAGERR_DEFAULT);
  nag_lapackeig_dsyevd(order,job,uplo,n,f1,n,lambda1,NAGERR_DEFAULT);

  /* Fomulate F0 and F1 in P0 = F0'*F0, P1 = F1'*F1 */
  nu = 0;
  nv = 0;
  for (i=0; i<n; i++){
    if (lambda0[i]>0)
      {
        for (j=0; j<n; j++){
          idxf = i*n + j;
          f0[idxf] = f0[idxf]*sqrt(lambda0[i]);
        }
        nclin = nclin + 1;
        nu = nu + 1;
        nnza = nnza + n;
      }
    if (lambda1[i]>0)
      {
        for (j=0; j<n; j++){
          idxf = i*n + j;
          f1[idxf] = f1[idxf]*sqrt(lambda1[i]);
        }
        nclin = nclin + 1;
        nv = nv + 1;
        nnza = nnza + n;
      }
  }
  nnza = nnza + nu + nv;
  nvar = nvar + nu + nv;
  nvarc1 = nvarc1 + nu;
  nvarc2 = nvarc2 + nv;

  /* Add two fixed variable for two rotated quadratic cones */
  nvar = nvar + 2;
  nclin = nclin + 2;
  nnza = nnza + 2;

  /* Compute size of multipliers */
  nnzu = 2*nvar + 2*nclin;
  nnzuc = nvarc1 + nvarc2;

  /* Allocate memory */
  if (!(irowa = NAG_ALLOC(nnza, Integer))               ||
      !(icola = NAG_ALLOC(nnza, Integer))               ||
      !(c = NAG_ALLOC(nvar,double))                     ||
      !(a = NAG_ALLOC(nnza,double))                     ||
      !(bla = NAG_ALLOC(nclin,double))                  ||
      !(bua = NAG_ALLOC(nclin,double))                  ||
      !(xl = NAG_ALLOC(nvar,double))                    ||
      !(xu = NAG_ALLOC(nvar,double))                    ||
      !(x = NAG_ALLOC(nvar,double))                     ||
      !(u = NAG_ALLOC(nnzu,double))                     ||
      !(uc = NAG_ALLOC(nnzuc,double))                   ||
      !(idxc1 = NAG_ALLOC(nvarc1, Integer))             ||
      !(idxc2 = NAG_ALLOC(nvarc2, Integer)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
  for (i=0; i<nvar; i++){
    x[i] = 0.0;
  }

  /* Build objective function parameter c*/
  for (i=0; i<n; i++){
    c[i] = q0[i];
  }
  for (i=n; i<nvar-1; i++){
    c[i] = 0.0;
  }
  c[nvar-1] = 1.0;

  /* Build linear constraints parameter A */
  idxa = 0;
  rptr = 0;
  /* q1 in First row */
  rptr = rptr + 1;
  for (i=0; i<n; i++){
    irowa[i] = rptr;
    icola[i] = i + 1;
    a[i] = q1[i];
  }
  idxa = idxa + n;

  /* F0 in F0*x-u=0 row */
  for (i=0; i<n; i++){
    if (lambda0[i]>0)
      {
        rptr = rptr + 1;
        for (j=0; j<n; j++){
          irowa[idxa+j] = rptr;
          icola[idxa+j] = j + 1;
          idxf = i*n + j;
          a[idxa+j] = f0[idxf];     
        }
        idxa = idxa + n;
      }
  }
  /* F1 in F1*x-v=0 row */
  for (i=0; i<n; i++){
    if (lambda1[i]>0)
      {
        rptr = rptr + 1;
        for (j=0; j<n; j++){
          irowa[idxa+j] = rptr;
          icola[idxa+j] = j + 1;
          idxf = i*n + j;
          a[idxa+j] = f1[idxf];     
        }
        idxa = idxa + n;
      }
  }
  /* Rest of A, a diagonal matrix */
  for (i=0; i<nclin; i++){
    irowa[idxa+i] = i + 1;
    icola[idxa+i] = i + n + 1;
    a[idxa+i] = 1.0;
  }
  for (i=0; i<nu+nv; i++){
    a[idxa+i+1] = -1.0;
  }

  /* RHS in linear constraints */
  for (i=0; i<nclin; i++){
    bla[i] = 0.0;
    bua[i] = 0.0;
  }
  bla[0] = -r1;
  bua[0] = -r1;
  for (i=0; i<nqc+1; i++){
    bla[nclin-1-i] = 1.0;
    bua[nclin-1-i] = 1.0;
  }

  /* box constraints, all variables are free*/
  for (i=0; i<nvar; i++){
    xl[i] = -1.0e20;
    xu[i] = 1.0e20;
  }

  /* Cone constraints */
  /* First cone */
  idxc1[0] = nvar;
  idxc1[1] = n+1+nu+nv+1;
  for (i=0; i<nu; i++){
    idxc1[2+i] = n + 2 + i;
  }
  /* Second cone */
  idxc2[0] = n + 1;
  idxc2[1] = n+1+nu+nv+2;
  for (i=0; i<nu; i++){
    idxc2[2+i] = n + 2 + nu + i;
  }

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

  /* nag_opt_handle_set_linobj (e04rec)
   * Define a linear objective */
  nag_opt_handle_set_linobj(handle,nvar,c,NAGERR_DEFAULT);

  /* nag_opt_handle_set_simplebounds (e04rhc)
   * Define bounds on the variables */
  nag_opt_handle_set_simplebounds(handle,nvar,xl,xu,NAGERR_DEFAULT);

  /* nag_opt_handle_set_linconstr (e04rjc)
   * Define linear constraints */
  idlc = 0;
  nag_opt_handle_set_linconstr(handle,nclin,bla,bua,nnza,irowa,
                               icola,a,&idlc,NAGERR_DEFAULT);

  /* nag_opt_handle_set_group (e04rbc)
   * Define cone constraints */
  idgroup = 0;
  nag_opt_handle_set_group(handle, "RQUAD", nvarc1, idxc1, &idgroup,
                           NAGERR_DEFAULT);

  /* nag_opt_handle_set_group (e04rbc)
   * Define cone constraints */
  idgroup = 0;
  nag_opt_handle_set_group(handle, "RQUAD", nvarc2, idxc2, &idgroup, 
                           NAGERR_DEFAULT);

  /* nag_opt_handle_opt_set (e04zmc) */
  /* Turn on monitoring */
  nag_opt_handle_opt_set(handle, "SOCP Monitor Frequency = 1",
                         NAGERR_DEFAULT);
  /* Set this to 1 to cause nag_opt_handle_solve_socp_ipm (e04ptc)
   * to produce intermediate progress output */
  verbose_output = 0;

  if (verbose_output){
    /* Require printing of primal and dual solutions at the end of the solve */
    nag_opt_handle_opt_set(handle, "Print Solution = Yes",
                           NAGERR_DEFAULT);
  }
  else{
    /* Turn off printing of intermediate progress output */
    nag_opt_handle_opt_set(handle, "Print Level = 1",
                           NAGERR_DEFAULT);
  }

  tol_reached = 0;
  tol_monit = 1.0e-7;
  comm.iuser = &tol_reached;
  comm.user = &tol_monit;
  /* nag_opt_handle_solve_socp_ipm (e04ptc) */
  INIT_FAIL(fail);
  nag_opt_handle_solve_socp_ipm(handle,nvar,x,nnzu,u,nnzuc,uc,rinfo,stats,monit,
                              &comm,&fail);
  if (fail.code != NE_NOERROR && fail.code != NW_NOT_CONVERGED){
    printf("nag_opt_handle_solve_socp_ipm (e04ptc) failed.\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }

  /* Print solution if optimal or suboptimal solution found */
  printf(" Optimal X:\n");
  printf("  x_idx       Value\n");
  for (x_idx=0; x_idx<n; x_idx++){
    printf("  %5" NAG_IFMT"   %11.3E\n", x_idx+1, x[x_idx]);
  }

 END:
  NAG_FREE(c);
  NAG_FREE(irowa);
  NAG_FREE(icola);
  NAG_FREE(a);
  NAG_FREE(bla);
  NAG_FREE(bua);
  NAG_FREE(xl);
  NAG_FREE(xu);
  NAG_FREE(x);
  NAG_FREE(u);
  NAG_FREE(uc);
  NAG_FREE(idxc1);
  NAG_FREE(idxc2);
  NAG_FREE(irowp0);
  NAG_FREE(icolp0);
  NAG_FREE(irowp1);
  NAG_FREE(icolp1);
  NAG_FREE(p0);
  NAG_FREE(p1);
  NAG_FREE(q0);
  NAG_FREE(q1);
  NAG_FREE(f0);
  NAG_FREE(f1);
  NAG_FREE(lambda0);
  NAG_FREE(lambda1);

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

  return exit_status;
}

static void NAG_CALL monit(void *handle, const double rinfo[],
                      const double stats[], Nag_Comm *comm,
                      Integer *inform){
   /*  Monitoring function can be used to monitor the progress
    *  or, for example,  to implement bespoke stopping criteria */
  double tol = comm->user[0];
  Integer *tol_reached = comm->iuser;

  /* If x is close to the solution, print a message */
  if (rinfo[14]<tol && rinfo[15]<tol &&rinfo[16]<tol &&rinfo[17]<tol){
    if (!*tol_reached){
      printf("\n     monit() reports good approximate solution "
             "(tol = %8.2E)\n",tol);
      *tol_reached = 1;
    }
  }
  fflush(stdout);
}