/* nag_glopt_nlp_multistart_sqp (e05ucc) Example Program.
 *
 * Copyright 2017 Numerical Algorithms Group.
 *
 * Mark 26.1, 2017.
 */

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

#ifdef __cplusplus
extern "C"
{
#endif
  static void NAG_CALL schwefel_obj(Integer *mode, Integer n,
                                    const double *x, double *objf,
                                    double *objgrd, Integer nstate,
                                    Nag_Comm *comm);
  static void NAG_CALL schwefel_confun(Integer *mode, Integer ncnln,
                                       Integer n, Integer tdcjsl,
                                       const Integer *needc,
                                       const double *x, double *c,
                                       double *cjsl, 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)
{

  Integer exit_status = 0;
  Integer print_all_solutions = 0;
  Integer liopts = 740, lopts = 485, n = 2, nclin = 1, ncnln = 2;

  /* Scalars */
  Integer i, ic, j, l, nb, npts, tda, ldcjac, sdcjac, ldr, sdr,
         ldx, ldobjgrd, ldclamda, ldistate, ldc;

  /* Arrays */
  static double ruser[3] = { -1.0, -1.0, -1.0 };
  double *a = 0, *bl = 0, *bu = 0, *c = 0, *cjac = 0, *clamda = 0, *objf = 0,
         *objgrd = 0, *r = 0, *opts = 0, *work = 0, *x = 0;
  Integer *info = 0, *istate = 0, *iter = 0, *iopts = 0;
  char nag_enum_arg[40];

  /* Nag Types */
  NagError fail;
  Nag_Comm comm;
  Nag_Boolean repeat;

  INIT_FAIL(fail);

  printf("nag_glopt_nlp_multistart_sqp (e05ucc) Example Program Results\n\n");

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

  /* Skip heading in data file */
  scanf("%*[^\n] ");
  scanf("%" NAG_IFMT "%" NAG_IFMT "%*[^\n]", &nb, &npts);
  scanf("%39s%*[^\n]", nag_enum_arg);
  /* nag_enum_name_to_value (x04nac).
   * Converts NAG enum member name to value
   */
  repeat = (Nag_Boolean) nag_enum_name_to_value(nag_enum_arg);

  /* The minimum trailing dimension for a is tda = n (or 1). */
  if (nclin > 0) {
    tda = n;
    if (!(a = NAG_ALLOC(nclin * tda, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
  }
  else
    tda = 1;

#define A(I,J) a[(I-1)*tda + (J-1)]
#define X(I,J) x[(J-1)*ldx + (I-1)]
#define ISTATE(I,J) istate[(J-1)*ldistate + (I-1)]
#define CLAMDA(I,J) clamda[(J-1)*ldclamda + (I-1)]
#define C(I,J) c[(J-1)*ldc + (I-1)]

  ldx = n;
  ldobjgrd = n;
  ldc = ncnln;
  ldcjac = ncnln;

  if (ncnln > 0) {
    sdcjac = n;
    if (!(c = NAG_ALLOC(ldc * nb, double)) ||
        !(cjac = NAG_ALLOC(ldcjac * sdcjac * nb, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
  }
  else
    sdcjac = 0;

  ldr = n;
  sdr = n;
  ldclamda = n + nclin + ncnln;
  ldistate = n + nclin + ncnln;

  if (!(bl = NAG_ALLOC(n + nclin + ncnln, double)) ||
      !(bu = NAG_ALLOC(n + nclin + ncnln, double)) ||
      !(clamda = NAG_ALLOC(ldclamda * nb, double)) ||
      !(objf = NAG_ALLOC(nb, double)) ||
      !(objgrd = NAG_ALLOC(ldobjgrd * nb, double)) ||
      !(r = NAG_ALLOC(ldr * sdr * nb, double)) ||
      !(opts = NAG_ALLOC(lopts, double)) ||
      !(work = NAG_ALLOC(nclin, double)) ||
      !(x = NAG_ALLOC(ldx * nb, double)) ||
      !(info = NAG_ALLOC(nb, Integer)) ||
      !(istate = NAG_ALLOC(ldistate * nb, Integer)) ||
      !(iter = NAG_ALLOC(nb, Integer)) ||
      !(iopts = NAG_ALLOC(liopts, Integer))
         )
  {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }

  bl[0] = -500.0;
  bl[1] = -500.0;
  bl[2] = -10000.0;
  bl[3] = -1.0;
  bl[4] = -0.9;
  bu[0] = 500.0;
  bu[1] = 500.0;
  bu[2] = 10.0;
  bu[3] = 500000.0;
  bu[4] = 0.9;
  A(1, 1) = 3.0;
  A(1, 2) = -2.0;

  /* Initialize nag_glopt_nlp_multistart_sqp (e05ucc).
   * nag_glopt_opt_set (e05zkc).
   * Option setting routine for global optimization.
   */
  nag_glopt_opt_set("Initialize = e05ucc", iopts, liopts, opts, lopts, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_glopt_opt_set (e05zkc).\n%s\n", fail.message);
    exit_status = 1;
    goto END;
  }

  /* Solve the problem with repeatable random starting points using
   * nag_glopt_nlp_multistart_sqp (e05ucc).
   * Global optimization using multi-start, nonlinear constraints.
   */
  nag_glopt_nlp_multistart_sqp(n, nclin, ncnln, a, tda, bl, bu,
                               schwefel_confun, schwefel_obj, npts, x, ldx,
                               mystart, repeat, nb, objf, objgrd, ldobjgrd,
                               iter, c, ldc, cjac, ldcjac, sdcjac, r, ldr,
                               sdr, clamda, ldclamda, istate, ldistate, iopts,
                               opts, &comm, info, &fail);
  /* Check for error exits. */
  switch (fail.code) {
  case NE_NOERROR:
    l = nb;
    break;
  case NW_SOME_SOLUTIONS:
    l = info[nb - 1];
    printf("Only %" NAG_IFMT " solutions found\n", l);
    break;
  default:
    exit_status = 2;
    printf("Error from nag_glopt_nlp_multistart_sqp (e05ucc)\n%s\n",
           fail.message);
    goto END;
  }

  for (i = 1; i <= l; i++) {
    printf("Solution number %" NAG_IFMT "\n\n", i);
    printf("Local minimization exited with code %" NAG_IFMT "\n",
           info[i - 1]);
    printf("\nVarbl  Istate   Value         Lagr Mult\n\n\n");

    for (j = 1; j <= n; j++)
      printf("V %3" NAG_IFMT " %3" NAG_IFMT "    %14.6g  %12.4g\n", j,
             ISTATE(j, i), X(j, i), CLAMDA(j, i));

    if (nclin > 0) {
      printf("\nL Con  Istate   Value         Lagr Mult\n\n");

      /* nag_dgemv (f16pac) performs the matrix vector multiplication A*x
       * (linear constraint values) and puts the result in
       * the first nclin locations of work.
       */
      nag_dgemv(Nag_RowMajor, Nag_NoTrans, nclin, n, 1.0, a, tda, &X(1, i), 1,
                0.0, work, 1, &fail);
      if (fail.code != NE_NOERROR) {
        printf("Error from nag_dgemv (f16pac).\n%s\n", fail.message);
        exit_status = 1;
        goto END;
      }

      for (j = n + 1; j <= n + nclin; j++)
        printf("L %3" NAG_IFMT " %3" NAG_IFMT "    %14.6g  %12.4g\n", j - n,
               ISTATE(j, i), work[j - n - 1], CLAMDA(j, i));
    }

    if (ncnln > 0) {
      printf("\n\nN Con  Istate   Value         Lagr Mult\n\n");

      for (j = n + nclin + 1; j <= n + nclin + ncnln; j++) {
        ic = j - n - nclin;
        printf("N %3" NAG_IFMT " %3" NAG_IFMT "    %14.6g  %12.4g\n", ic,
               ISTATE(j, i), C(ic, i), CLAMDA(j, i));
      }

    }

    printf("\n\nFinal objective value = %15.7g\n", objf[i - 1]);

    printf("\nQP multipliers\n");
    for (j = 1; j <= n + nclin + ncnln; j++)
      printf("%12.4e\n", CLAMDA(j, i));

    if (l == 1)
      goto END;

    if (print_all_solutions == 0) {
      printf("\n(Printing of further solutions suppressed)\n");
      goto END;
    }

    printf("\n");
    for (j = 0; j < 61; j++)
      printf("-");
    printf("\n");
  }

END:
  NAG_FREE(a);
  NAG_FREE(bl);
  NAG_FREE(bu);
  NAG_FREE(c);
  NAG_FREE(cjac);
  NAG_FREE(clamda);
  NAG_FREE(objf);
  NAG_FREE(objgrd);
  NAG_FREE(r);
  NAG_FREE(opts);
  NAG_FREE(work);
  NAG_FREE(x);
  NAG_FREE(info);
  NAG_FREE(istate);
  NAG_FREE(iter);
  NAG_FREE(iopts);
  return exit_status;
}

static void NAG_CALL schwefel_obj(Integer *mode, Integer n, const double *x,
                                  double *objf, double *objgrd,
                                  Integer nstate, Nag_Comm *comm)
{
  /* Scalars */
  Integer i;

#ifdef _OPENMP
#pragma omp master
#endif
  if (comm->user[0] == -1.0) {
    printf("(User-supplied callback schwefel_obj, first invocation.)\n");
    comm->user[0] = 0.0;
  }

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

  if (*mode == 0 || *mode == 2) {
    /* Evaluate the objective function. */
    *objf = 0.0;
    for (i = 0; i < n; i++)
      *objf += x[i] * sin(sqrt(fabs(x[i])));
  }

  if (*mode == 1 || *mode == 2) {
    /* Calculate the gradient of the objective function. */
    for (i = 0; i < n; i++) {
      double t;
      t = sqrt(fabs(x[i]));
      objgrd[i] = sin(t) + 0.5 * t * cos(t);
    }
  }
}

static void NAG_CALL schwefel_confun(Integer *mode, Integer ncnln, Integer n,
                                     Integer tdcjsl, const Integer *needc,
                                     const double *x, double *c, double *cjsl,
                                     Integer nstate, Nag_Comm *comm)
{

  /* Scalars */
  double t1, t2;
  Integer k;
  Nag_Boolean evalc, evalcjsl;
#ifdef _OPENMP
#pragma omp master
#endif
  if (comm->user[1] == -1.0) {
    printf("(User-supplied callback schwefel_confun, first invocation.)\n");
    comm->user[1] = 0.0;
  }

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

  /* mode: what is required - constraints, derivatives, or both? */
  evalc = (*mode == 0 || *mode == 2) ? Nag_TRUE : Nag_FALSE;
  evalcjsl = (*mode == 1 || *mode == 2) ? Nag_TRUE : Nag_FALSE;

  for (k = 1; k <= ncnln; k++) {
    if (needc[k - 1] <= 0)
      continue;
    if (evalc == Nag_TRUE) {
      /* Constraint values are required. */
      switch (k) {
      case 1:
        c[k - 1] = pow(x[0], 2.0) - pow(x[1], 2.0) + 3.0 * x[0] * x[1];
        break;
      case 2:
        c[k - 1] = cos(pow((x[0] / 200.0), 2.0) + (x[1] / 100.0));
        break;
      default:
        /* This constraint is not coded (there are only two).
         * Terminate.
         */
        *mode = -1;
        break;
      }
    }
    if (*mode < 0)
      break;
    if (evalcjsl == Nag_TRUE) {
      /* Constraint derivatives are required. */
#define CJSL(K, J) cjsl[(K-1)*tdcjsl + (J-1)]
      switch (k) {
      case 1:
        CJSL(k, 1) = 2.0 * x[0] + 3.0 * x[1];
        CJSL(k, 2) = -2.0 * x[1] + 3.0 * x[0];
        break;
      case 2:
        t1 = x[0] / 200.0;
        t2 = x[1] / 100.0;
        CJSL(k, 1) = -sin(pow(t1, 2.0) + t2) * (2.0 * t1) / 200.0;
        CJSL(k, 2) = -sin(pow(t1, 2.0) + t2) / 100.0;
        break;
      }
#undef CJSL
    }
  }
}

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)
{
  /* Only nonzero elements of quas need to be specified here. */
  Integer i, j;
  if (comm->user[2] == -1.0) {
    printf("(User-supplied callback mystart, first invocation.)\n");
    comm->user[2] = 0.0;
  }
#define QUAS(J, I) quas[(J-1)*npts + (I-1)]
  if (repeat == Nag_TRUE) {
    /* Generate a uniform spread of points between bl and bu. */
    for (j = 1; j <= npts; j++)
      for (i = 1; i <= n; i++)
        QUAS(i, j) =
               bl[i - 1] + (bu[i - 1] - bl[i - 1]) * (double) (j -
                                                               1) /
               (double) (npts - 1);
  }
  else {
    /* Generate a non-repeatable spread of points between bl and bu. */
    Nag_BaseRNG genid;
    Integer 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
}