/* nag_glopt_nlp_multistart_sqp_lsq (e05usc) 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>

#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("%16" NAG_IFMT " starting points converged\n", iter[nb - 1]);
    break;
  }

  for (i = 1; i <= l; i++) {
    printf("\nSolution number %16" NAG_IFMT "\n", i);
    printf("\nLocal minimization exited with code %" NAG_IFMT "\n",
           info[i - 1]);
    printf("\nVarbl  Istate   Value         Lagr Mult\n\n");
    for (j = 1; j <= n; j++) {
      printf("V %3" NAG_IFMT " %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 %3" NAG_IFMT " %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 %3" NAG_IFMT " %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;

#ifdef _OPENMP
#pragma omp master
#endif
  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;

#ifdef _OPENMP
#pragma omp master
#endif
  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 nonzero 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
}