/* nag_glopt_nlp_pso (e05sbc) Example Program.
 *
 * Copyright 2014 Numerical Algorithms Group.
 *
 * Mark 25, 2014.
 */
#include <math.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <nage05.h>
#include <nagx02.h>
#include <nagx04.h>

#ifdef __cplusplus
extern "C" {
#endif
static void NAG_CALL objfun_schwefel(Integer *mode, Integer ndim,
                                     const double x[], double *objf,
                                     double vecout[], Integer nstate,
                                     Nag_Comm *comm);
static void NAG_CALL confun(Integer *mode, Integer ncon, Integer ndim,
                            Integer tdcj, const Integer needc[],
                            const double x[], double c[], double cjac[],
                            Integer nstate, Nag_Comm *comm);
static void NAG_CALL monmod(Integer ndim, Integer ncon, Integer npar,
                            double x[], const double xb[], double fb,
                            const double cb[], const double xbest[],
                            const double fbest[], const double cbest[],
                            const Integer itt[], Nag_Comm *comm,
                            Integer *inform);
#ifdef __cplusplus
}
#endif

static Integer display_option(const char *optstr, const Integer iopts[],
                              const double opts[]);

static void display_result(Integer ndim, Integer ncon, const double xb[],
                           double fb, const double cb[], const Integer itt[],
                           Integer inform);

/* Global constants.*/
/* Set the behaviour of the monitoring function.*/
static const Integer detail_level = 0;
static const Integer report_freq = 100;
/* Known solution for a comparison.*/
static const double  f_target = -731.70709230672696;
static const double  c_scale[] = { 2490.0, 750000.0, 0.1 };
static const double  c_target[] = { 0.0, 0.0, 0.0 };
static const double  x_target[] = { -394.1470221120988, -433.48214189947606 };

int main(void)
{
  /* This example program demonstrates how to use
   * nag_glopt_nlp_pso (e05sbc) in standard execution, and with
   * nag_opt_nlp (e04ucc) as a coupled local minimizer.
   * The non-default option 'Repeatability = On' is used here, giving
   * repeatable results.
   */
  /* Scalars */
  Integer          ncon = 3, ndim = 2, npar = 20;
  Integer          exit_status = 0, lcvalue = 17;
  Integer          liopts = 100, lopts = 100;
  double           fb, rvalue;
  Integer          i, inform, ivalue;
  /* Arrays */
  static double ruser[3] = {-1.0, -1.0, -1.0};
  char             cvalue[17], optstr[81];
  double           opts[100], *bl = 0, *bu = 0, *cb = 0, *xb = 0;
  double           *cbest = 0, *fbest = 0, *xbest = 0;
  Integer          iopts[100], itt[7];
  /* Nag Types */
  Nag_VariableType optype;
  Nag_Comm         comm;
  NagError         fail;

  /* Print advisory information.*/
  printf("nag_glopt_nlp_pso (e05sbc) Example Program Results\n\n");

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

  printf("Minimization of the Schwefel function.\n");
  printf("Subject to one linear and two nonlinear constraints.\n\n");

  /* Allocate memory for arrays.*/
  if (!(bl = NAG_ALLOC(ndim+ncon, double)) ||
      !(bu = NAG_ALLOC(ndim+ncon, double)) ||
      !(cb = NAG_ALLOC(ncon, double)) ||
      !(cbest = NAG_ALLOC(ncon*npar, double)) ||
      !(fbest = NAG_ALLOC(npar, double)) ||
      !(xb = NAG_ALLOC(ndim, double)) ||
      !(xbest = NAG_ALLOC(ndim*npar, double)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }

  for (i = 0; i < npar; i++)
    fbest[i] = 0.0;
  for (i = 0; i < npar*ndim; i++)
    xbest[i] = 0.0;
  for (i = 0; i < npar*ncon; i++)
    cbest[i] = 0.0;

  /* Set problem specific values.*/
  /* Set box bounds.*/
  for (i = 0; i < ndim; i++) {
    bl[i] = -500.0;
    bu[i] = 500.0;
  }

  /* Set constraint bounds.*/
  bl[ndim]   = -1.0e6;
  bl[ndim+1] = -1.0;
  bl[ndim+2] = -0.9;
  bu[ndim]   = 10.0;
  bu[ndim+1] = 5.0e5;
  bu[ndim+2] = 0.9;

  /* Initialize NagError structure.*/
  INIT_FAIL(fail);

  /* Initialize the option arrays for nag_glopt_nlp_pso (e05sbc)
   * using nag_glopt_opt_set (e05zkc).
   */
  nag_glopt_opt_set("Initialize = e05sbc", 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;
  }
  /* Query some default option values.*/
  printf("  Default Option Queries:\n\n");
  exit_status = display_option("Constraint Norm", iopts, opts);
  if (exit_status) goto END;
  exit_status = display_option("Maximum Iterations Completed", iopts, opts);
  if (exit_status) goto END;
  exit_status = display_option("Distance Tolerance", iopts, opts);
  if (exit_status) goto END;

  /* ------------------------------------------------------------------*/
  printf("\n1. Solution without using coupled local minimizer.\n\n");

  /* Set various options to non-default values if required.*/
  nag_glopt_opt_set("Distance Tolerance = 1.0e-5", iopts, liopts, opts, lopts,
                    &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Constraint Tolerance = 1.0e-4", iopts, liopts, opts,
                      lopts, &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Constraint Norm = Euclidean", iopts, liopts, opts, lopts,
                      &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Repeatability = On", iopts, liopts, opts, lopts, &fail);
  sprintf(optstr, "Target Objective Value = %32.16e", f_target);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set(optstr, iopts, liopts, opts, lopts, &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Target Objective Tolerance = 1.0e-4", 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;
  }

  /* nag_glopt_nlp_pso (e05sbc).
   * Global optimization using particle swarm algorithm (PSO), comprehensive.
   */
  nag_glopt_nlp_pso(ndim, ncon, npar, xb, &fb, cb, bl, bu, xbest,
                    fbest, cbest, objfun_schwefel, confun,
                    monmod, iopts, opts, &comm, itt, &inform, &fail);
  /* It is essential to test fail.code on exit.*/
  switch (fail.code)
    {
    case NE_NOERROR:
    case NW_FAST_SOLUTION:
    case NW_SOLUTION_NOT_GUARANTEED:
      /* No errors, best found solution at xb returned in fb.*/
      display_result(ndim, ncon, xb, fb, cb, itt, inform);
      break;
    case NE_USER_STOP:
      /* Exit flag set in objfun, confun or monmod and returned in inform.*/
      display_result(ndim, ncon, xb, fb, cb, itt, inform);
      break;
    default: /* An error was detected.*/
      exit_status = 1;
      printf("Error from nag_glopt_nlp_pso (e05sbc)\n%s\n", fail.message);
      goto END;
    }

  /* ------------------------------------------------------------------*/

  printf("2. Solution using coupled local minimizer nag_opt_nlp (e04ucc).\n\n");

  /*  Set the local minimizer to be nag_opt_nlp (e04ucc) and set corresponding
   *  options.
   */
  nag_glopt_opt_set("Local Minimizer = e04ucc", iopts, liopts, opts, lopts,
                    &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Local Interior Major Iterations = 15", iopts, liopts,
                      opts, lopts, &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Local Interior Minor Iterations = 5", iopts, liopts,
                      opts, lopts, &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Local Exterior Major Iterations = 50", iopts, liopts,
                      opts, lopts, &fail);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set("Local Exterior Minor Iterations = 15", 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;
  }

  /* Query the option Distance Tolerance*/
  nag_glopt_opt_get("Distance Tolerance", &ivalue, &rvalue, cvalue, lcvalue,
                    &optype, iopts, opts, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_glopt_opt_get (e05zlc).\n%s\n", fail.message);
    exit_status = 1;
    goto END;
  }

  /* Adjust Distance Tolerance dependent upon its current value*/
  rvalue = rvalue*10.0;
  sprintf(optstr, "Distance Tolerance = %32.16e", rvalue);
  nag_glopt_opt_set(optstr, iopts, liopts, opts, lopts, &fail);
  rvalue = 0.1*rvalue;
  sprintf(optstr, "Local Interior Tolerance = %32.16e", rvalue);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set(optstr, iopts, liopts, opts, lopts, &fail);
  rvalue = rvalue*1.0e-4;
  sprintf(optstr, "Local Exterior Tolerance = %32.16e", rvalue);
  if (fail.code == NE_NOERROR)
    nag_glopt_opt_set(optstr, 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;
  }

  /* nag_glopt_nlp_pso (e05sbc).
   * Global optimization using particle swarm algorithm (PSO), comprehensive.
   */
  nag_glopt_nlp_pso(ndim, ncon, npar, xb, &fb, cb, bl, bu, xbest,
                    fbest, cbest, objfun_schwefel, confun,
                    monmod, iopts, opts, &comm, itt, &inform, &fail);
  /* It is essential to test fail.code on exit.*/
  switch (fail.code)
    {
    case NE_NOERROR:
    case NW_FAST_SOLUTION:
    case NW_SOLUTION_NOT_GUARANTEED:
    case NW_NOT_FEASIBLE:
      /* nag_glopt_nlp_pso (e05sbc) encountered no errors during
       * operation, and will have returned the best solution found.
       */
      display_result(ndim, ncon, xb, fb, cb, itt, inform);
      break;
    case NE_USER_STOP:
      /* Exit flag set in objfun, confun or monmod and returned in inform.*/
      display_result(ndim, ncon, xb, fb, cb, itt, inform);
      break;
    default: /* An error was detected.*/
      exit_status = 1;
      printf("Error from nag_glopt_nlp_pso (e05sbc)\n%s\n", fail.message);
      goto END;
    }

 END:
  /* Clean up memory.*/
  NAG_FREE(bl);
  NAG_FREE(bu);
  NAG_FREE(cb);
  NAG_FREE(cbest);
  NAG_FREE(fbest);
  NAG_FREE(xb);
  NAG_FREE(xbest);

  return exit_status;
}

static void NAG_CALL objfun_schwefel(Integer *mode, Integer ndim,
                                     const double x[], double *objf,
                                     double vecout[], Integer nstate,
                                     Nag_Comm *comm)
{
  /* Objective function routine returning the schwefel function and
   * its gradient.
   */
  Nag_Boolean evalobjf, evalobjg;
  Integer     i;
  if (comm->user[0] == -1.0)
    {
      printf("(User-supplied callback objfun_schwefel, first invocation.)\n");
      comm->user[0] = 0.0;
    }
  /* Test nstate to indicate what stage of computation has been reached.*/
  switch (nstate)
    {
    case 2:
      /* objfun is called for the very first time. */
      break;
    case 1:
      /* objfun is called on entry to a NAG local minimizer. */
      break;
    default:
      /* This will be the normal value of nstate. */
      ;
    }
  /* Test mode to determine whether to calculate objf and/or objgrd.*/
  evalobjf = Nag_FALSE;
  evalobjg = Nag_FALSE;
  switch (*mode)
    {
    case 0:
    case 5:
      /* Only the value of the objective function is needed.*/
      evalobjf = Nag_TRUE;
      break;
    case 1:
    case 6:
      /* Only the values of the ndim gradients are required.*/
      evalobjg = Nag_TRUE;
      break;
    case 2:
    case 7:
      /* Both the objective function and the ndim gradients are required.*/
      evalobjf = Nag_TRUE;
      evalobjg = Nag_TRUE;
    }
  if (evalobjf)
    {
      /* Evaluate the objective function.*/
      *objf = 0.0;
      for (i = 0; i < ndim; i++)
        *objf += x[i]*sin(sqrt(fabs(x[i])));
    }
  if (evalobjg)
    {
      /* Calculate the gradient of the objective function, */
      /* and return the result in vecout.*/
      for (i = 0; i < ndim; i++)
        {
          vecout[i] = sqrt(fabs(x[i]));
          vecout[i] = sin(vecout[i]) + 0.5*vecout[i]*cos(vecout[i]);
        }
    }
}

static void NAG_CALL confun(Integer *mode, Integer ncon, Integer ndim,
                            Integer tdcj, const Integer needc[],
                            const double x[], double c[], double cjac[],
                            Integer nstate, Nag_Comm *comm)
{
  /* Supplies constraints.
   * cjac[(k-1)*tdcj + (i-1)] corresponds to dc[k]/dx[i]
   * for k=1,...,ncon and i=1,...,ndim.
   */
  Integer     k;
  Nag_Boolean evalc, evalcjac;

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

  /* Test nstate to determine whether the local minimizer is being called
   * for the first time from a new start point.
   */
  if (nstate == 1) {
    /* Set any constant elements of the Jacobian matrix.*/
    cjac[0] = 3.0;
    cjac[1] = -2.0;
  }
  /* mode: are constraints, derivatives, or both are required? */
  evalc = (*mode == 0 || *mode == 2) ? Nag_TRUE : Nag_FALSE;
  evalcjac = (*mode == 1 || *mode == 2) ? Nag_TRUE : Nag_FALSE;

  for (k = 0; k < ncon; k++) {
    if (needc[k] <= 0)
      continue;

    if (evalc == Nag_TRUE) {
      /* Constraint values are required.
       * Only those for which needc is non-zero need be set.
       */
      switch (k)
        {
        case 0:
          c[k] = 3.0*x[0] - 2.0*x[1];
          break;
        case 1:
          c[k] = x[0]*x[0] - x[1]*x[1] + 3.0*x[0]*x[1];
          break;
        case 2:
          c[k] = cos(pow((x[0]/200.0), 2) + (x[1]/100.0));
          break;
        default:
          /* This constraint is not coded (there are only three).
           * Terminate.
           */
          *mode = -1;
          break;
        }
    }
    if (*mode < 0)
      break;
    if (evalcjac == Nag_TRUE) {
      /* Constraint derivatives (cjac) are required.*/
      switch (k)
        {
        case 0:
          /* Constant derivatives set when nstate=1 remain throughout
           * the local minimization.
           */
          break;
        case 1:
          /* If the constraint derivatives are known and are readily
           * calculated, populate cjac when required.
           */
          cjac[k*tdcj] = 2.0*x[0] + 3.0*x[1];
          cjac[k*tdcj+1] = -2.0*x[1] + 3.0*x[0];
          break;
        default:
          /* Any elements of cjac left unaltered will be approximated
           * using finite differences when required.
           */
          ;
        }
    }
  }
}

static void NAG_CALL monmod(Integer ndim, Integer ncon, Integer npar,
                            double x[], const double xb[], double fb,
                            const double cb[], const double xbest[],
                            const double fbest[], const double cbest[],
                            const Integer itt[], Nag_Comm *comm,
                            Integer *inform)
{
  Integer i, j;
#define X(J, I) x[(J-1)*ndim + (I-1)]
#define XBEST(J, I) xbest[(J-1)*ndim + (I-1)]
#define CBEST(J, I) cbest[(J-1)*ncon + (I-1)]
  if (comm->user[2] == -1.0)
    {
      printf("(User-supplied callback monmod, first invocation.)\n");
      comm->user[2] = 0.0;
    }
  if (detail_level)
    {
      /* Report on the first iteration, and every report_freq iterations.*/
      if (itt[0] == 1 || itt[0]%report_freq == 0)
        {
          printf("* Locations of particles\n");
          for (j = 1; j <= npar; j++)
            {
              printf("  * Particle %2ld\n", j);
              for (i = 1; i <= ndim; i++)
                printf("    %2ld  %13.5f\n", i, X(j, i));
            }
          printf("* Cognitive memory\n");
          for (j = 1; j <= npar; j++)
            {
              printf("  * Particle %2ld\n", j);
              printf("    * Best position\n");
              for (i = 1; i <= ndim; i++)
                printf("      %2ld  %13.5f\n", i, XBEST(j, i));
              printf("    * Function value at best position\n");
              printf("      %13.5f\n", fbest[j - 1]);
              printf("    * Best constraint violations\n");
              for (i = 1; i <= ncon; i++)
                printf("      %2ld  %13.5f\n", i, CBEST(j, i));
            }
          printf("* Current global optimum candidate\n");
          for (i = 1; i <= ndim; i++)
            printf("  %2ld  %13.5f\n", i, xb[i - 1]);
          printf("* Current global optimum value\n");
          printf("     %13.5f\n\n", fb);
          printf("* Constraint violations of candidate\n");
          for (i = 1; i <= ncon; i++)
            printf("  %2ld  %13.5f\n", i, cb[i - 1]);
        }
    }
  /* If required set *inform<0 to force exit.*/
  *inform = 0;
#undef CBEST
#undef XBEST
#undef X
}

static Integer display_option(const char *optstr, const Integer iopts[],
                              const double opts[])
{
  /* Subroutine to query optype and print the appropriate option values.*/

  /* Scalars */
  Integer          exit_status = 0, lcvalue = 17;
  double           rvalue = 0.0;
  Integer          ivalue = 0;
  /* Arrays */
  char             cvalue[17];
  /* Nag Types */
  Nag_VariableType optype = 0;
  NagError         fail;
  
  INIT_FAIL(fail);

  /* nag_glopt_opt_get (e05zlc).
   * Option getting routine for nag_glopt_nlp_pso (e05sbc).
   */
  nag_glopt_opt_get(optstr, &ivalue, &rvalue, cvalue, lcvalue, &optype, iopts,
                    opts, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_glopt_opt_get (e05zlc).\n%s\n", fail.message);
    exit_status = 1;
    goto END;
  }

  switch (optype)
    {
    case Nag_Integer:
      printf("%-38s: %13ld\n", optstr, ivalue);
      break;
    case Nag_Real:
      printf("%-38s: %13.4f\n", optstr, rvalue);
      break;
    case Nag_Character:
      printf("%-38s: %13s\n", optstr, cvalue);
      break;
    case Nag_Integer_Additional:
      printf("%-38s: %13ld %16s\n", optstr, ivalue, cvalue);
      break;
    case Nag_Real_Additional:
      printf("%-38s: %13.4f %16s\n", optstr, rvalue, cvalue);
      break;
    default:;
    }
 END:
  return exit_status;
}

static void display_result(Integer ndim, Integer ncon, const double xb[],
                           double fb, const double cb[], const Integer itt[],
                           Integer inform)
{
  /* Display final results in comparison to known global optimum.*/
  Integer i;

  /* Display final counters.*/
  printf(" Algorithm Statistics\n");
  printf(" --------------------\n");
  printf("%-38s: %13ld\n", "Total complete iterations", itt[0]);
  printf("%-38s: %13ld\n", "Complete iterations since improvement",
         itt[1]);
  printf("%-38s: %13ld\n", "Total particles converged to xb", itt[2]);
  printf("%-38s: %13ld\n", "Total improvements to global optimum",
         itt[3]);
  printf("%-38s: %13ld\n", "Total function evaluations", itt[4]);
  printf("%-38s: %13ld\n", "Total particles re-initialized",
         itt[5]);
  printf("%-38s: %13ld\n\n", "Total constraints violated",
         itt[6]);
  /* Display why finalization occurred.*/
  switch (inform)
    {
    case 1:
      printf("Solution Status : Target value achieved\n");
      break;
    case 2:
      printf("Solution Status : Minimum swarm standard deviation obtained\n");
      break;
    case 3:
      printf("Solution Status : Sufficient number of particles converged\n");
      break;
    case 4:
      printf("Solution Status : Maximum static iterations attained\n");
      break;
    case 5:
      printf("Solution Status : Maximum complete iterations attained\n");
      break;
    case 6:
      printf("Solution Status : Maximum function evaluations exceeded\n");
      break;
    case 7:
      printf("Solution Status : Feasible point located\n");
      break;
    default:
      if (inform < 0) {
        printf("Solution Status : User termination, inform = %16ld\n",
               inform);
        return;
      }
      printf("Solution Status : Termination, an error has been detected\n");
      break;
    }
  /* Display final objective value and location.*/
  printf("  Known constrained objective optimum : %13.3f\n", f_target);
  printf("  Achieved objective value            : %13.3f\n\n", fb);

  printf("  Comparison between the known optimum and the achieved solution.\n");
  printf("          x_target            xb\n");
  for (i = 0; i < ndim; i++)
    printf("  %2ld  %12.2f  %12.2f\n", i+1, x_target[i], xb[i]);

  printf("\n");

  if (ncon > 0) {
    printf("  Comparison between scaled constraint violations.\n");
    printf("          c_target            cb\n");
    for (i = 0; i < ncon; i++)
      printf("  %2ld  %12.5f  %12.5f\n", i+1, c_target[i]/c_scale[i],
             cb[i]/c_scale[i]);
    
    printf("\n");
  }
}