/* nag_glopt_nlp_multistart_sqp_lsq (e05usc) Example Program.
 *
 * Copyright 2014 Numerical Algorithms Group.
 *
 * Mark 25, 2014.
 */

#include <stdio.h>
#include <math.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <nage05.h>
#include <nagf16.h>
#include <nagg05.h>

#ifdef __cplusplus
extern "C" {
#endif
  static void NAG_CALL confun(Integer *mode, Integer ncnln, Integer n,
                              Integer pdcj, const Integer needc[], 
                              const double x[], double c[], double cjac[],
                              Integer nstate, Nag_Comm *comm);
  static void NAG_CALL objfun(Integer *mode, Integer m, Integer n, Integer pdfj,
                              Integer needfi, const double x[], double f[],
                              double fjac[], Integer nstate, Nag_Comm *comm);
  static void NAG_CALL mystart(Integer npts, double quas[], Integer n,
                               Nag_Boolean repeat, const double bl[],
                               const double bu[], Nag_Comm *comm,
                               Integer *mode);
#ifdef __cplusplus
}
#endif

int main(void)
{
#define LEN_OPTS 485
#define LEN_IOPTS 740

#define ISTATE(I,J) istate[(J-1)* pdistate + I-1]
#define A(I,J) a[(J-1)* pda + I-1]
#define C(I,J) c[(J-1)* pdc + I-1]
#define CLAMDA(I,J) clamda[(J-1)* pdclamda + I-1]
#define X(I,J) x[(J-1)* pdx + I-1]


  static double ruser[3] = {-1.0, -1.0, -1.0};
  Integer     exit_status = 0;
  Integer     m = 44, n = 2, nb = 1, nclin = 1, ncnln = 1, npts = 3;
  Integer     pdistate, pda, pdc, ldcjac, pdclamda, ldfjac, pdx;
  Integer     sdcjac, sdfjac;
  Integer     i, j, k, l;
  Nag_Boolean repeat = Nag_TRUE;
  Integer     inc;
  double      alpha, beta;
  double      *a = 0, *bl = 0, *bu = 0, *c = 0, *cjac = 0, *clamda = 0, *f = 0;
  double      *fjac = 0, *objf = 0, *work = 0, *x = 0, *y = 0;
  Integer     *info = 0, *istate = 0, *iter = 0;
  double      opts[LEN_OPTS];
  Integer     iopts[LEN_IOPTS];
  Integer     len_opts = LEN_OPTS, len_iopts = LEN_IOPTS;
  /* Nag Types */
  Nag_Comm    comm;
  NagError    fail;

  INIT_FAIL(fail);

  printf("nag_glopt_nlp_multistart_sqp_lsq (e05usc) Example Program Results\n");

  /* For communication with user-supplied functions: */
  comm.user = ruser;

  fflush(stdout);

  pda = nclin;
  pdc = ncnln;
  ldcjac = ncnln;
  sdcjac = (ncnln > 0 ? n : 0);
  ldfjac = m;
  sdfjac = n;
  pdclamda = n + nclin + ncnln;
  pdistate = n + nclin + ncnln;
  pdx = n;

  if (nclin > 0)
    {
      if (!(a = NAG_ALLOC(pda*n, double)))
        {
          printf("Allocation failure\n");
          exit_status = -1;
          goto END;
        }
    }
      
  if (!(bl = NAG_ALLOC(n + nclin + ncnln, double))||
      !(bu = NAG_ALLOC(n + nclin + ncnln, double))||
      !(y = NAG_ALLOC(m, double))||
      !(c = NAG_ALLOC(pdc*nb, double))||
      !(cjac = NAG_ALLOC(ldcjac*sdcjac*nb, double))||
      !(f = NAG_ALLOC(m*nb, double))||
      !(fjac = NAG_ALLOC(ldfjac*sdfjac*nb, double))||
      !(clamda = NAG_ALLOC(pdclamda*nb, double))||
      !(x = NAG_ALLOC(pdx*nb, double))||
      !(objf = NAG_ALLOC(nb, double))||
      !(istate = NAG_ALLOC(pdistate*nb, Integer))||
      !(info = NAG_ALLOC(nb, Integer))||
      !(iter = NAG_ALLOC(nb, Integer))||
      !(work = NAG_ALLOC(nclin, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }

  /* Skip heading in data file*/
  scanf("%*[^\n] ");

  if (nclin > 0)
    {
      for (i = 1; i <= nclin; i++) 
        for (j = 1; j <= n; j++) 
          scanf("%lf", &A(i, j));
      scanf("%*[^\n] ");
    }
  
  for (i = 0; i < m; i++) 
    scanf("%lf", &y[i]);
  scanf("%*[^\n] ");
  
  for (i = 0; i < (n + nclin + ncnln); i++) 
    scanf("%lf", &bl[i]);
  scanf("%*[^\n] ");

  for (i = 0; i < (n + nclin + ncnln); i++) 
    scanf("%lf", &bu[i]);
  scanf("%*[^\n] ");

  /* nag_glopt_opt_set (e05zkc).
   * Option setting routine for nag_glopt_nlp_multistart_sqp_lsq (e05usc).
   * First initialize.
   */ 
  nag_glopt_opt_set("Initialize = e05usc",
                    iopts, len_iopts, opts, len_opts, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_glopt_opt_set (e05zkc).\n%s\n",
             fail.message);
      exit_status = 1;
      goto END;
    }

  /* nag_glopt_nlp_multistart_sqp_lsq (e05usc).
   * Global optimization of a sum of squares problem using multi-start,
   * nonlinear constraints.
   * Solve the problem.
   */
  nag_glopt_nlp_multistart_sqp_lsq(m, n, nclin, ncnln, a, pda, bl, bu, y,
                                   confun, objfun, npts, x, pdx, mystart,
                                   repeat, nb, objf, f, fjac, ldfjac, sdfjac,
                                   iter, c, pdc, cjac, ldcjac, sdcjac,
                                   clamda, pdclamda, istate, pdistate,
                                   iopts, opts, &comm, info, &fail);

  if (fail.code != NE_NOERROR && fail.code != NW_SOME_SOLUTIONS)
    {
      printf("Error from nag_glopt_nlp_multistart_sqp_lsq (e05usc).\n%s\n",
             fail.message);
      exit_status = 4;
      goto END;
    }

  switch (fail.code)
    {
    case NE_NOERROR:
      l = nb;
      break;
    case NW_SOME_SOLUTIONS:
      l = info[nb-1];
      printf("%16ld starting points converged\n", iter[nb-1]);
      break;
    }
  
  for (i=1; i<=l; i++) 
    {
      printf("\nSolution number %16ld\n", i);
      printf("\nLocal minimization exited with code %ld\n", info[i-1]);
      printf("\nVarbl  Istate   Value         Lagr Mult\n\n");
      for (j=1; j<=n; j++) 
        {
          printf("V %3ld %3"NAG_IFMT, j, ISTATE(j, i));
          printf(" %14.6f %12.4f\n", X(j, i), CLAMDA(j, i));
        }
      if (nclin>0) 
        {
          /* Below is a call to the NAG version of the level 2 BLAS 
           * routine nag_dgemv.
           * This performs the matrix vector multiplication A*X
           * (linear constraint values) and puts the result in
           * the first nclin locations of work.
           */
          inc = 1;
          alpha = 1.0;
          beta = 0.0;

          /* nag_dgemv (f16pac).
           * Matrix-vector product, real rectangular matrix.
           */ 
          nag_dgemv(Nag_ColMajor, Nag_NoTrans, nclin, n, alpha, a, pda, &X(1,i),
                    inc, beta, work, inc, &fail);
          if (fail.code != NE_NOERROR) 
            {
              printf("Error from nag_dgemv (f16pac).\n%s\n",
                     fail.message);
              exit_status = 5;
              goto END;
            }

          printf("\nL Con  Istate   Value         Lagr Mult\n\n");
          for (k = n + 1; k <= n + nclin; k++) 
            {
              j = k - n;
              printf("L %3ld %3"NAG_IFMT, j, ISTATE(k, i));
              printf(" %14.6f %12.4f\n", work[j-1], CLAMDA(k, i));
            }
        }
      if (ncnln>0) 
        {
          printf("\nN Con  Istate   Value         Lagr Mult\n\n");
          for (k = n + nclin + 1; k <= n + nclin + ncnln; k++) 
            {
              j = k - n - nclin;
              printf("N %3ld %3"NAG_IFMT, j, ISTATE(k, i));
              printf(" %14.6f %12.4f\n", C(j, i), CLAMDA(k, i));
            }
        }
      printf("\nFinal objective value = %15.7f\n", objf[i-1]);
      printf("\nQP multipliers\n");
      for (k = 1; k <= n + nclin + ncnln; k++) 
        printf("%12.4f\n", CLAMDA(k, i));

      if (l==1) break;

      printf("\n------------------------------------------------------\n");
    }

 END:
  NAG_FREE(a);
  NAG_FREE(bl);
  NAG_FREE(bu);
  NAG_FREE(c);
  NAG_FREE(cjac);
  NAG_FREE(clamda);
  NAG_FREE(f);
  NAG_FREE(fjac);
  NAG_FREE(x);
  NAG_FREE(y);
  NAG_FREE(work);
  NAG_FREE(objf);
  NAG_FREE(istate);
  NAG_FREE(info);
  NAG_FREE(iter);
  return exit_status;
}
static void NAG_CALL confun(Integer *mode, Integer ncnln, Integer n,
                            Integer pdcj, const Integer needc[], 
                            const double x[], double c[], double cjac[],
                            Integer nstate, Nag_Comm *comm)
{
#define CJAC(I, J) cjac[(J-1) * pdcj + I-1]
  /* Function to evaluate the nonlinear constraint and its 1st derivatives. */
  Integer i, j;
  
#pragma omp master
  if (comm->user[0] == -1.0)
    {
      fflush(stdout);
      printf("(User-supplied callback confun, first invocation.)\n");
      comm->user[0] = 0.0;
      fflush(stdout);
    }

  /* This problem has only one constraint.
   * As an example of using the mode mechanism,
   * terminate if any other size is supplied.        
   */
  if (ncnln != 1)
    {
      *mode = -1;
      return;
    }

  if (nstate == 1) 
    {
      /* First call to confun. Set all Jacobian elements to zero.
       * Note that this will only work when 'Derivative Level = 3'
       * (the default; see Section 11.1).
       */
      for (i = 1; i <= ncnln; i++) 
        for (j = 1; j <= n; j++) 
          CJAC(i, j) = 0.0;
    }

  if (needc[0] > 0)
    {
      if (*mode == 0 || *mode == 2) 
        c[0] = -0.09 - x[0] * x[1] + 0.49 * x[1];

      if (*mode == 1 || *mode == 2) 
        {
          CJAC(1, 1) = -x[1];
          CJAC(1, 2) = -x[0] + 0.49;
        }
    }
}
static void NAG_CALL objfun(Integer *mode, Integer m, Integer n, Integer pdfj,
                            Integer needfi, const double x[], double f[],
                            double fjac[], Integer nstate, Nag_Comm *comm)
{
#define FJAC(I, J) fjac[J * pdfj + I]

  /* Function to evaluate the subfunctions and their 1st derivatives. */
  double     a[] = {  8.0,  8.0, 10.0, 10.0, 10.0, 10.0, 12.0, 12.0, 12.0,
                      12.0, 14.0, 14.0, 14.0, 16.0, 16.0, 16.0, 18.0, 18.0,
                      20.0, 20.0, 20.0, 22.0, 22.0, 22.0, 24.0, 24.0, 24.0,
                      26.0, 26.0, 26.0, 28.0, 28.0, 30.0, 30.0, 30.0, 32.0,
                      32.0, 34.0, 36.0, 36.0, 38.0, 38.0, 40.0, 42.0 };
  double     temp, x1, x2;
  Integer    i;

#pragma omp master
  if (comm->user[1] == -1.0)
    {
      fflush(stdout);
      printf("(User-supplied callback objfun, first invocation.)\n");
      comm->user[1] = 0.0;
      fflush(stdout);
    }

  /* This is a two-dimensional objective function.
   * As an example of using the mode mechanism,
   * terminate if any other problem size is supplied.
   */
  if (n != 2)
    {
      *mode = -1;
      return;
    }

  if (nstate==1)
    {
      /* This is the first call.
       * Take any special action here if desired.
       */
    }

  x1 = x[0];
  x2 = x[1];

  if (*mode==0 && needfi>0)
    {
      f[needfi-1] = x1 + (0.49 - x1) * exp(-x2 * (a[needfi-1] - 8.0));
      return;
    }

  for (i = 0; i < m; i++) 
    {
      temp = exp(-x2 * (a[i] - 8.0));

      if (*mode == 0 || *mode == 2) 
        f[i] = x1 + (0.49 - x1) * temp;

      if (*mode == 1 || *mode == 2) 
        {
          FJAC(i, 0) = 1.0 - temp;
          FJAC(i, 1) = -(0.49 - x1) * (a[i] - 8.0) * temp;
        }
    }

  return;
}
static void NAG_CALL mystart(Integer npts, double quas[], Integer n,
                             Nag_Boolean repeat, const double bl[],
                             const double bu[], Nag_Comm *comm, Integer *mode)
{
#define QUAS(I,J) quas[(J-1) * n + I-1]

  if (comm->user[2] == -1.0)
    {
      fflush(stdout);
      printf("(User-supplied callback mystart, first invocation.)\n");
      comm->user[2] = 0.0;
      fflush(stdout);
    }

  /* All elements of quas[n*npts] are pre-assigned to zero,
   * so we only need to set non-zero elements.
   */
  if (repeat == Nag_TRUE)
    {
      QUAS(1, 1) = 0.4;
      QUAS(2, 2) = 1.0;
    }
  else
    {
      /* Generate a non-repeatable spread of points between bl and bu. */
      Nag_BaseRNG genid;
      Integer i, j, lstate, subid;
      Integer *state=0;
      NagError fail;

      INIT_FAIL(fail);

      genid = Nag_WichmannHill_I;
      subid = 53;
      lstate = -1;

      nag_rand_init_nonrepeatable(genid, subid, NULL, &lstate, &fail);
      if (fail.code != NE_NOERROR)
        {
          *mode = -1;
          return;
        }

      if (!(state = NAG_ALLOC(lstate, Integer)))
        {
          *mode = -1;
          return;
        }

      nag_rand_init_nonrepeatable(genid, subid, state, &lstate, &fail);
      if (fail.code != NE_NOERROR)
        {
          *mode = -1;
          goto END;
        }

      for (j = 2; j <= npts; j++)
        for (i = 1; i <= n; i++)
          {
            nag_rand_uniform(1, bl[i-1], bu[i-1], state, &QUAS(i, j), &fail);
            if (fail.code != NE_NOERROR)
              {
                *mode = -1;
                goto END;
              }
          }

    END:
      NAG_FREE(state);
    }

#undef QUAS    
}