/* nag_opt_handle_solve_ipopt (e04stc) Example Program.
 *
 * NAGPRODCODE Version.
 *
 * Copyright 2016 Numerical Algorithms Group.
 *
 * Mark 26, 2016.
 */

/* 
 * NLP example: Linear objective + Linear constraint + Non-Linear constraint
 */

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

#ifdef __cplusplus
extern "C"
{
#endif
  static void NAG_CALL objfun(Integer nvar, const double x[],
                              double *fx,
                              Integer *flag, Nag_Comm *comm);
  static void NAG_CALL objgrd(Integer nvar, const double x[],
                              Integer nnzfd, double fdx[],
                              Integer *flag, Nag_Comm *comm);
  static void NAG_CALL confun(Integer nvar, const double x[],
                              Integer ncnln, double gx[],
                              Integer *flag, Nag_Comm *comm);
  static void NAG_CALL congrd(Integer nvar, const double x[],
                              Integer nnzgd, double gdx[],
                              Integer *flag, Nag_Comm *comm);
  static void NAG_CALL hess(Integer nvar, const double x[],
                            Integer ncnln, Integer idf, double sigma,
                            const double lambda[], Integer nnzh, double hx[],
                            Integer *flag, Nag_Comm *comm);
  static void NAG_CALL mon(Integer nvar, const double x[],
                           Integer nnzu, const double u[],
                           Integer *flag, const double rinfo[],
                           const double stats[], Nag_Comm *comm);
#ifdef __cplusplus
}
#endif

int main(void)
{
  #define BIGBND 1.0E40
  /* Scalars */
  double     solve_timeout = 5.0;
  Integer    exit_status = 0, islinear;
  Integer    i, idlc, idx, j, nnzu, nvar, nclin, ncnln, nnzgd;

  /* Arrays */
  Integer    iuser[2];
  Integer    idxfd[4] = {1,2,3,4};
  double     ruser[4] = {24.55, 26.75, 39.00, 40.50};
  double     rinfo[32], stats[32], *x = 0, *u = 0;
  double     bl[4] = {0,0,0,0};
  double     bu[4] = {BIGBND,BIGBND,BIGBND,BIGBND};
  double     linbl[2] = {5.0,1.0};
  double     linbu[2] = {BIGBND,1.0};
  double     nlnbl[1] = {21.0};
  double     nlnbu[1] = {BIGBND};
  Integer    irowb[8] = {1,1,1,1,2,2,2,2};
  Integer    icolb[8] = {1,2,3,4,1,2,3,4};
  Integer    irowgd[4] = {1,1,1,1};
  Integer    icolgd[4] = {1,2,3,4};
  Integer    irowh[10], icolh[10];
  double     b[8]= {2.3, 5.6, 11.1, 1.3, 1.0, 1.0, 1.0, 1.0};
  char       opt[80];
  void       *handle = 0;

  /* Nag Types */
  NagError   fail;
  Nag_Comm   comm;
  Nag_FileID nout, file_out, mon_out, umon_out;

  /* nag_open_file (x04acc).
   *  Open unit number for reading, writing or appending, and
   *  associate unit with named file  
   */
  nag_open_file("", 1, &nout, NAGERR_DEFAULT);

  /* nag_write_line (x04bac).
   * Write formatted record to external file
   */
  nag_write_line(nout, "nag_opt_handle_solve_ipopt (e04stc) "
                 "Example Program Results");

  nag_open_file("e04stc.out", 1, &file_out, NAGERR_DEFAULT);
  nag_open_file("e04stc.mon", 1, &mon_out, NAGERR_DEFAULT);
  nag_open_file("e04stc.umon", 1, &umon_out, NAGERR_DEFAULT);

  for (islinear=0;islinear<=1;islinear++){
    nnzu = 0;
    nvar = 4;
    /* nag_opt_handle_init (e04rac).
     * Initialize an empty problem handle with NVAR variables. 
     */
    nag_opt_handle_init(&handle, nvar, NAGERR_DEFAULT);

    sprintf(opt,"Infinite Bound Size = %e",BIGBND);
    /* nag_opt_handle_opt_set (e04zmc).
     * Set optional arguments of the solver 
     */
    nag_opt_handle_opt_set(handle, opt, NAGERR_DEFAULT);

    nnzu += 2*nvar;
    /* nag_opt_handle_set_simplebounds (e04rhc).
     * Define bounds on the variables
     */
    nag_opt_handle_set_simplebounds(handle, nvar, bl, bu, NAGERR_DEFAULT);

    iuser[0] = islinear;
    comm.iuser = iuser;
    comm.user = ruser;
    if (islinear==1) {
      /* nag_opt_handle_set_linobj (e04rec).
       * Define linear objective 
       */
      nag_opt_handle_set_linobj(handle, nvar, ruser, NAGERR_DEFAULT);
    } else {
      /* nag_opt_handle_set_nlnobj (e04rgc).
       * Define non-linear objective 
       */
      nag_opt_handle_set_nlnobj(handle, nvar, idxfd, NAGERR_DEFAULT);
    }
    nclin = 2;
    nnzu += 2*nclin;
    idlc = 0;
    /* nag_opt_handle_set_linconstr (e04rjc).
     * Define a block of linear constraints 
     */
    nag_opt_handle_set_linconstr(handle, nclin, linbl, linbu, nclin*nvar,
                                 irowb, icolb, b, &idlc, NAGERR_DEFAULT);

    ncnln = 1;
    /* dense gradients */
    nnzgd = ncnln * nvar;
    nnzu += 2*ncnln;
    /* nag_opt_handle_set_nlnconstr (e04rkc).
     * Define a block of nonlinear constraints 
     */
    nag_opt_handle_set_nlnconstr(handle, ncnln, nlnbl, nlnbu, nnzgd,
                                 irowgd, icolgd, NAGERR_DEFAULT);

    /* Define structure of the Hessian, dense upper triangle */
    for(idx=0,i=1;i<=nvar;i++)
      for(j=i;j<=nvar;j++,idx++){
        icolh[idx] = j;
        irowh[idx] = i;
    }
    /* nag_opt_handle_set_nlnhess (e04rlc).
     * Define structure of Hessian of objective, constraints or the Lagrangian
     */
    nag_opt_handle_set_nlnhess(handle, 1, idx, irowh, icolh, NAGERR_DEFAULT);
    if (islinear!=1)
      nag_opt_handle_set_nlnhess(handle, 0, idx, irowh, icolh, NAGERR_DEFAULT);
    /* nag_opt_handle_print (e04ryc).
       Print information about a problem
     */
    nag_opt_handle_print(handle, nout, "Overview", NAGERR_DEFAULT);

    if (!(x = NAG_ALLOC(nvar, double)) ||
        !(u = NAG_ALLOC(nnzu, double))) {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }

    for (i=0;i<nvar;i++) x[i]=1.0;
    iuser[1] = umon_out;

    sprintf(opt, "Monitoring File = %" NAG_IFMT, mon_out);
    nag_opt_handle_opt_set(handle, opt, NAGERR_DEFAULT);
    nag_opt_handle_opt_set(handle, "Monitoring Level = 3", NAGERR_DEFAULT);
    nag_opt_handle_opt_set(handle, "Outer Iteration Limit = 26",
                           NAGERR_DEFAULT);

    sprintf(opt, "Print File = %" NAG_IFMT, file_out);
    nag_opt_handle_opt_set(handle, opt, NAGERR_DEFAULT);
    nag_opt_handle_opt_set(handle, "Print Level = 2", NAGERR_DEFAULT);
    nag_opt_handle_opt_set(handle, "Stop Tolerance 1 = 2.5e-8", NAGERR_DEFAULT);
    nag_opt_handle_opt_set(handle, "Time Limit = 60", NAGERR_DEFAULT);

    INIT_FAIL(fail);
    /* nag_opt_handle_solve_ipopt (e04stc).
     * Run Ipopt solver on a sparse nonlinear programming problem
     */
    nag_opt_handle_solve_ipopt(handle, objfun, objgrd, confun, congrd, hess,
                               mon, nvar, x, nnzu, u, rinfo, stats, &comm,
                               &fail);
    if (fail.code == NE_NOERROR) {
      char msg[80];
      nag_opt_handle_print(handle, nout, "Options", &fail);
      nag_write_line(nout, "Variables");
      for (i=0; i<nvar; i++) {
        sprintf(msg, "     x(%10" NAG_IFMT ")%17s=%20.6e", i+1, "", x[i]);
        nag_write_line(nout, msg);
      }
      nag_write_line(nout, "Variable bound Lagrange multipliers");
      for (i=0; i<nvar; i++) {
        sprintf(msg, "     zL(%10" NAG_IFMT ")%16s=%20.6e", i+1, "", u[2*i]);
        nag_write_line(nout, msg);
        sprintf(msg, "     zU(%10" NAG_IFMT ")%16s=%20.6e", i+1, "", u[2*i+1]);
        nag_write_line(nout, msg);
      }
      nag_write_line(nout, "Linear constraints Lagrange multipliers");
      for (i=0;i<nclin;i++) {
        sprintf(msg, "     l+(%10" NAG_IFMT ")%16s=%20.6e", i+1, "",
                u[2*nvar+2*i]);
        nag_write_line(nout,msg);
        sprintf(msg, "     l-(%10" NAG_IFMT ")%16s=%20.6e", i+1, "",
                u[2*nvar+2*i+1]);
        nag_write_line(nout, msg);
      }
      nag_write_line(nout, "Nonlinear constraints Lagrange multipliers");
      for (i=0;i<ncnln;i++) {
        sprintf(msg, "     l+(%10" NAG_IFMT ")%16s=%20.6e", i+1, "",
                u[2*(nvar+nclin)+2*i]);
        nag_write_line(nout, msg);
        sprintf(msg, "     l-(%10" NAG_IFMT ")%16s=%20.6e", i+1, "",
                u[2*(nvar+nclin)+2*i+1]);
        nag_write_line(nout, msg);
      }
      sprintf(msg, "\nAt solution, Objective minimum     =%20.7e", rinfo[0]);
      nag_write_line(nout, msg);
      sprintf(msg, "             Constraint violation  =%20.2e", rinfo[1]);
      nag_write_line(nout, msg);
      sprintf(msg, "             Dual infeasibility    =%20.2e", rinfo[2]);
      nag_write_line(nout, msg);
      sprintf(msg, "             Complementarity       =%20.2e", rinfo[3]);
      nag_write_line(nout, msg);
      sprintf(msg, "             KKT error             =%20.2e", rinfo[4]);
      nag_write_line(nout, msg);
      if (stats[7] < solve_timeout)
        sprintf(msg, "Solved in allotted time limit");
      else
        sprintf(msg, "Solution took %6.2f sec, which is longer than expected",
                stats[7]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after iterations                        :%11.0f",
              stats[0]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after objective evaluations             :%11.0f",
              stats[18]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after objective gradient evaluations    :%11.0f",
              stats[4]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after constraint evaluations            :%11.0f",
              stats[19]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after constraint gradient evaluations   :%11.0f",
              stats[20]);
      nag_write_line(nout, msg);
      sprintf(msg, "    after hessian evaluations               :%11.0f",
              stats[3]);
      nag_write_line(nout, msg);
      nag_opt_handle_print(handle, nout, "Overview", NAGERR_DEFAULT);
      nag_write_line(nout,
                     "-----------------------------------------------------");
    } else {
      printf("Error from nag_opt_handle_solve_ipopt (e04stc).\n%s\n",
             fail.message);
      exit_status = 1;
    }
    if (handle)
      /* nag_opt_handle_free (e04rzc).
       * Destroy the problem handle and deallocate all the memory used 
       */
      nag_opt_handle_free(&handle, NAGERR_DEFAULT);

    NAG_FREE(x);
    NAG_FREE(u);
  }

 END:
  return exit_status;
}

/* Subroutine */
#include <assert.h>
  static void NAG_CALL objfun(Integer nvar, const double x[],
                              double *fx,
                              Integer *flag, Nag_Comm *comm)
  {
    *flag = 0;
    /* nag_ddot (f16eac).
     * Dot product of two vectors, allows scaling and accumulation
     */
    nag_ddot(Nag_NoConj,4,1.0,x,1,0.0,comm->user,1,fx,NAGERR_DEFAULT);
    assert (comm->iuser[0]!=1) ;
  }
  static void NAG_CALL objgrd(Integer nvar, const double x[],
                              Integer nnzfd, double fdx[],
                              Integer *flag, Nag_Comm *comm)
  {
    *flag = 0;
    /* nag_dge_copy (f16qfc).
     * Matrix copy, real rectangular matrix
     */
    nag_dge_copy(Nag_ColMajor, Nag_NoTrans, nnzfd, 1, comm->user, nnzfd, fdx,
                 nnzfd, NAGERR_DEFAULT);
    assert(comm->iuser[0]!=1);
  }
  static void NAG_CALL confun(Integer nvar, const double x[],
                              Integer ncnln, double gx[],
                              Integer *flag, Nag_Comm *comm)
  {
    *flag = 0;
    gx[0]= 12.0*x[0] + 11.9*x[1] + 41.8*x[2] + 52.1*x[3] -
      1.645*sqrt(.28*x[0]*x[0]+.19*x[1]*x[1]+20.5*x[2]*x[2]+.62*x[3]*x[3]);
  }
  static void NAG_CALL congrd(Integer nvar, const double x[],
                              Integer nnzgd, double gdx[],
                              Integer *flag, Nag_Comm *comm)   
  {
    double tmp;
    *flag = 0;
    tmp  = sqrt(0.62*x[3]*x[3]+20.5*x[2]*x[2]+0.19*x[1]*x[1]+0.28*x[0]*x[0]);
    gdx[0] = (12.0*tmp-0.4606*x[0])/tmp;
    gdx[1] = (11.9*tmp-0.31255*x[1])/tmp;
    gdx[2] = (41.8*tmp-33.7225*x[2])/tmp;
    gdx[3] = (52.1*tmp-1.0199*x[3])/tmp;
  }
  static void NAG_CALL hess(Integer nvar, const double x[], Integer ncnln,
                            Integer idf, double sigma, const double lambda[],
                            Integer nnzh, double hx[],
                            Integer *flag, Nag_Comm *comm)
  {
    double tmp;
    *flag = 0;
    /* nag_dload (f16fbc).
     * Broadcast scalar into real vector
     */
    nag_dload(nnzh, 0.0, hx, 1, NAGERR_DEFAULT);

    if (idf==0) return; /* objective is linear */
    tmp = sqrt(0.62*x[3]*x[3] + 20.5*x[2]*x[2] + 0.19*x[1]*x[1]
               + 0.28*x[0]*x[0]);
    tmp = tmp*(x[3]*x[3] + 33.064516129032258064 *x[2]*x[2]
                         + 0.30645161290322580645*x[1]*x[1]
                         + 0.45161290322580645161*x[0]*x[0]);
    /* Col 1 */
    hx[0] = (-0.4606*x[3]*x[3] - 15.229516129032258064 *x[2]*x[2]
                               - 0.14115161290322580645*x[1]*x[1])/tmp;
    hx[1] = (0.14115161290322580645*x[0]*x[1])/tmp;
    hx[2] = (15.229516129032258064 *x[0]*x[2])/tmp;
    hx[3] = (0.4606*x[0]*x[3])/tmp;
    /* Col 2 */
    hx[4] = (-0.31255*x[3]*x[3] - 10.334314516129032258 *x[2]*x[2]
                                - 0.14115161290322580645*x[0]*x[0])/tmp;
    hx[5] = (10.334314516129032258*x[1]*x[2])/tmp;
    hx[6] = (0.31255*x[1]*x[3])/tmp;
    /* Col 3 */
    hx[7] = (-33.7225*x[3]*x[3] - 10.334314516129032258*x[1]*x[1]
                                - 15.229516129032258065*x[0]*x[0])/tmp;
    hx[8] = (33.7225*x[2]*x[3])/tmp;
    /* Col 4 */
    hx[9] = (-33.7225*x[2]*x[2] - 0.31255*x[1]*x[1] - 0.4606*x[0]*x[0])/tmp;
    /* nag_daxpby (f16ecc).
     * Real weighted vector addition
     */
    if (idf==-1)
      nag_daxpby(nnzh, 0.0, hx, 1, lambda[0], hx, 1, NAGERR_DEFAULT);
    else
      assert(idf == 1);
  }
  static void NAG_CALL mon(Integer nvar, const double x[],
                           Integer nnzu, const double u[],
                           Integer *flag, const double rinfo[],
                           const double stats[], Nag_Comm *comm)
  {
    Integer i;
    char    msg[80];
    char    fmt[80] = "%2" NAG_IFMT "%14.6e %2" NAG_IFMT "%14.6e ";

    *flag = 0;
    nag_write_line(comm->iuser[1], "Monitoring...");
    nag_write_line(comm->iuser[1], "   x[]");
    for (i=0; i<nvar; i+=2) {
      sprintf(msg, fmt, i, x[i], i+1, x[i+1]);
      nag_write_line(comm->iuser[1], msg);
    }
    nag_write_line(comm->iuser[1], "   u[]");
    for (i=0; i<nnzu; i+=2) {
      sprintf(msg, fmt, i, u[i], i+1, u[i+1]);
      nag_write_line(comm->iuser[1], msg);
    }
    nag_write_line(comm->iuser[1], "   rinfo[31]");
    for (i=0; i<32; i+=2) {
      sprintf(msg, fmt, i, rinfo[i], i+1, rinfo[i+1]);
      nag_write_line(comm->iuser[1], msg);
    }
    nag_write_line(comm->iuser[1], "   stats[31]");
    for (i=0; i<32; i+=2) {
      sprintf(msg, fmt, i, stats[i], i+1, stats[i+1]);
      nag_write_line(comm->iuser[1], msg);
    }
  }