/* bbowda - Black Box Optimization With Data Analysis
   Copyright (C) 2006-2012 Kevin Kofler <Kevin@tigcc.ticalc.org>
   Copyright (C) 2024-2025 DAGOPT Optimization Technologies GmbH
                           (www.dagopt.com)
                           written by Kevin Kofler <kofler@dagopt.com>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version. A copy of the GNU General Public
   License version 3 can be found in the file gpl-3.0.txt.

   Linking bbowda statically or dynamically (directly or indirectly) with
   other modules is making a combined work based on bbowda. Thus, the terms
   and conditions of the GNU General Public License cover the whole
   combination.

   In addition, as a special exception, the copyright holder of bbowda gives
   you permission to combine the bbowda program:
   * with free software programs or libraries that are released under the
     GNU Library or Lesser General Public License (LGPL), either version 2
     of the License, or (at your option) any later version,
   * with free software programs or libraries that are released under the
     IBM Common Public License (CPL), either version 1.0 of the License, or
     (at your option) any later version,
   * with free software programs or libraries that are released under the
     eclipse.org Eclipse Public License (EPL), either version 1.0 of the
     License, or (at your option) any later version,
   * with free software programs or libraries that are released under the
     CeCILL-C Free Software License Agreement, either version 1 of the License,
     or (at your option) any later version,
   * with code included in the standard release of MUMPS under the old MUMPS
     Conditions of Use as reproduced in licenses.txt (or modified versions
     of such code, with unchanged license; variants of the license where only
     the list of contributors and/or the list of suggested citations changed
     shall be considered the same license) and
   * if you qualify for a free of charge license of DONLP2, with code
     included in the standard release of DONLP2 under the DONLP2 Conditions
     of Use as reproduced in licenses.txt (or modified versions of such code,
     with unchanged license).
   (For avoidance of doubt, this implies that it is permitted, e.g., to combine
   the bbowda program with current versions of Ipopt released under the EPL
   version 2.0, because 2.0 is >= 1.0. Its dependency MUMPS is released under
   the CeCILL-C version 1, which is also listed above.)

   You may copy and distribute such a system following the terms of the GNU
   GPL for bbowda and the licenses of the other code concerned, provided that
   you include the source code of that other code when and as the GNU GPL
   requires distribution of source code.

   Note that people who make modified versions of bbowda are not obligated
   to grant this special exception for their modified versions; it is their
   choice whether to do so. The GNU General Public License gives permission
   to release a modified version without this exception; this exception also
   makes it possible to release a modified version which carries forward
   this exception.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>. */

#define _ISOC99_SOURCE
#include "nlpopt.h"

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <nlopt.h>

#include "problem.h"
#include "covar.h"
#include "gmmem.h"
#include "eqconst.h"
#include "eval.h"

#ifdef _WIN32
#include <malloc.h>
#ifndef alloca
#define alloca _alloca
#endif
#else
#include <alloca.h>
#endif

#define NLOPT_ALG NLOPT_LD_SLSQP

static double eval_f_0(unsigned n, const double *x, double *grad_f,
                       void *user_data)
{
  const struct bbowda_problem *problem=(const struct bbowda_problem *)user_data;
  int i;
  double f=0.;
  /*printf("Eval f at x=[");
  if (DIMX) {
    printf("%lf",x[0]);
  }
  for (i=1; i<DIMX; i++) {
    printf(",%lf",x[i]);
  }
  printf("]\n");*/
  for (i=0; i<DIMX; i++) {
    f+=problem->c[i]*x[i];
  }
  for (i=0; i<DIMY; i++) {
    f+=problem->c[i+DIMX]*x[i+(DIMX+DIMZ)];
  }
  if (grad_f != NULL) {
    int j;
    for (j=0; j<DIMX; j++) {
      grad_f[j]=problem->c[j];
    }
    for (; j<DIMX+DIMZ; j++) {
      grad_f[j]=0.;
    }
    for (j=0; j<DIMY; j++) {
      grad_f[j+(DIMX+DIMZ)]=problem->c[j+DIMX];
    }
  }
  return f;
}


static void eval_g_0(unsigned m, double *result, unsigned n, const double *x,
                     double *grad_g, void *user_data)
{
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  int k;

  /* compute (X-Xbar)T M (X-Xbar) */
  double X[DIMX+DIMZ+DIMY+DIMY_EQ], MX[DIMX+DIMZ+DIMY+DIMY_EQ];
  double z=0.;
  for (k=0; k<DIMX+DIMZ+DIMY; k++) {
    X[k]=x[k]-((struct covar_problem *)user_data)->covar->Xbar[k];
  }
  for (; k<DIMX+DIMZ+DIMY+DIMY_EQ; k++) {
    X[k]=-((struct covar_problem *)user_data)->covar->Xbar[k];
  }
  compute_MX(((struct covar_problem *)user_data)->covar,problem,X,MX);
  for (k=0; k<DIMX+DIMZ+DIMY+DIMY_EQ; k++) {
    z+=X[k]*MX[k];
  }
  result[0]=((struct covar_problem *)user_data)->covar->klow-z; /* i=0: z>=klow, i.e., klow-z<=0 */
  result[1]=z-((struct covar_problem *)user_data)->covar->kup; /* i=1: z<=kup, i.e., z-kup<=0 */
#define jac_g ((double(*)[n]) grad_g)
  if (jac_g) {
    /* compute ((X-Xbar)T M (X-Xbar))' = 2 M (X-Xbar) */
    for (k=0; k<DIMX+DIMZ+DIMY; k++) {
      double gz=MX[k]+MX[k]; /* 2*Mx[k] */
      jac_g[0][k]=-gz; /* negate for i=0, see above */
      jac_g[1][k]=gz;
    }
    for (; k<DIMX+DIMZ+DIMY+DIMY_EQ; k++) {
      jac_g[0][k]=0.;
      jac_g[1][k]=0.;
    }
  }
#undef jac_g
}


struct h0_data {
  const struct bbowda_problem *problem;
  int i;
};

static double eval_h_0(unsigned n, const double *x, double *grad_h,
                       void *user_data)
{
  const struct bbowda_problem *problem=((struct h0_data *)user_data)->problem;
  int i=((struct h0_data *)user_data)->i,j,k,l;

  /* compute z[i]-xk*xl */
  k=0;
  l=i;
  while (l>k) {
    k++;
    l-=k;
  }
  if (grad_h) {
    /* compute (zi-xk*xl)' = ezi - xl exk - xk exl */
    for (j=0; j<DIMX+DIMZ+DIMY+DIMY_EQ; j++) {
      grad_h[j] = 0.;
    }
    grad_h[i+DIMX] = 1.;
    grad_h[k] -= x[l];
    grad_h[l] -= x[k];
  }
  return x[i+DIMX]-x[k]*x[l];
}


static double eval_f_1(unsigned n, const double *x, double *grad_f,
                       void *user_data)
{
  const struct bbowda_problem *problem
    = ((struct GMM_problem *)user_data)->problem;
  /*printf("Eval f at x=[");
  if (DIMX) {
    printf("%lf",x[0]);
  }
  int i;
  for (i=1; i<DIMX; i++) {
    printf(",%lf",x[i]);
  }
  printf("]\n");*/
  if (grad_f) {
    gmm_grad(((struct GMM_problem *)user_data)->gmm,problem,x,grad_f);
    /* if the gradient is too large, lie, or SLSQP will fail */
    double scale=1.;
    int i;
    for (i=0; i<DIMX; i++) {
/* for some reason, a non-power-of-2 works better here */
#define GRAD_MAX 1000.
      if (fabs(scale*grad_f[i]) > GRAD_MAX) {
        scale=GRAD_MAX/fabs(grad_f[i]);
      }
    }
    if (scale<1.) {
      for (i=0; i<DIMX; i++) {
        grad_f[i]*=scale;
      }
    }
  }
  return gmm_prob(((struct GMM_problem *)user_data)->gmm,problem,x);
}


struct g1_data {
  const struct bbowda_problem *problem;
  double *estimate_constraint_coeff_vec;
  double estimate_constraint_tol;
};

static double eval_g_1(unsigned n, const double *x, double *grad_g,
                       void *user_data)
{
  const struct bbowda_problem *problem=((struct g1_data *)user_data)->problem;
  int k,l;
  /* evaluate quadratic polynomial */
  double *p=((struct g1_data *)user_data)->estimate_constraint_coeff_vec;
  double z=*(p++); /* constant term */
  for (k=0; k<DIMX; k++) { /* linear terms */
    z+=*(p++) * x[k];
  }
  for (k=0; k<DIMX; k++) { /* quadratic/bilinear terms */
    for (l=0; l<=k; l++) {
      z+=*(p++) * (x[k]*x[l]);
    }
  }
  if (grad_g) {
    /* evaluate gradient of the quadratic polynomial - negate, see below */
    p=(double *)user_data+1; /* skip constant term */
    for (k=0; k<DIMX; k++) { /* linear terms (constant derivatives) */
      grad_g[k]=-*(p++);
    }
    for (k=0; k<DIMX; k++) { /* quadratic/bilinear terms (linear derivatives) */
      for (l=0; l<=k; l++) {
        grad_g[k]-=*p * x[l];
        grad_g[l]-=*(p++) * x[k];
        /* If k==l, then this is grad_g[k]-=2 * *(p++) * x[k], which is correct,
           since d(xk^2)/dxk=2 xk.
           For the bilinear terms, d(xk*xl)/dxk=xl, d(xk*xl)/dxl=xk. */
      }
    }
  }
  /* z>=-estimate_constraint_tol, i.e., -(z+estimate_constraint_tol)<=0 */
  return -(z+((struct g1_data *)user_data)->estimate_constraint_tol);
}


int solve_nlp(int optimization_problem, void *data, int ignore_constraints,
              const double *best_point, double *optimum_x)
{
  const struct bbowda_problem *problem
    =optimization_problem?((struct GMM_problem *)data)->problem
                         :((struct covar_problem *)data)->problem;
  double x[DIMX+DIMZ+DIMY];
  nlopt_opt opt=NULL;
  int i,j,k,l;

  /* ensure reproducible results */
  nlopt_srand(31337);

#define NLOPT_CALL(f,a) if (f a < 0) fprintf(stderr, #f " failed\n"), exit(1)

  switch (optimization_problem) {
    case 0:
      {
        /* lower and upper bounds */
        double x_L[DIMX+DIMZ+DIMY];
        double x_U[DIMX+DIMZ+DIMY];
        /* x */
        for (i=0; i<DIMX; i++) {
          x_L[i]=problem->xlow[i];
          x_U[i]=problem->xup[i];
        }
        /* z */
        for (j=0; j<DIMX; j++) {
          for (k=0; k<=j; k++) {
            /* interval multiplication [xlow[j],xup[j]]*[xlow[k],xup[k]] */
            double bounds[4]={problem->xlow[j]*problem->xlow[k],
                              problem->xlow[j]*problem->xup[k],
                              problem->xup[j]*problem->xlow[k],
                              problem->xup[j]*problem->xup[k]};
            double zlow=*bounds, zup=*bounds;
            for (l=1; l<4; l++) {
              if (bounds[l]<zlow) zlow=bounds[l];
              if (bounds[l]>zup) zup=bounds[l];
            }
            x_L[i]=zlow;
            x_U[i++]=zup;
          }
        }
        /* y */
        for (; i<DIMX+DIMZ+DIMY; i++) {
          x_L[i]=problem->Flow[i-(DIMX+DIMZ)];
          x_U[i]=problem->Fup[i-(DIMX+DIMZ)];
        }
        opt=nlopt_create(NLOPT_ALG,DIMX+DIMZ+DIMY);
        if (!opt) {
          fprintf(stderr, "nlopt_create failed\n");
          exit(1);
        }
        NLOPT_CALL(nlopt_set_lower_bounds,(opt,x_L));
        NLOPT_CALL(nlopt_set_upper_bounds,(opt,x_U));
        NLOPT_CALL(nlopt_set_min_objective,(opt,eval_f_0,(void *)problem));
        NLOPT_CALL(nlopt_add_inequality_mconstraint,
                   (opt,2,eval_g_0,data,
                    (const double[]){1e-12,1e-12}));
        for (i=0; i<DIMZ; i++) {
          struct h0_data *user_data=alloca(sizeof(struct h0_data));
          user_data->problem=problem;
          user_data->i=i;
          NLOPT_CALL(nlopt_add_equality_constraint,
                     (opt,eval_h_0,user_data,1e-12));
        }
        /* starting value: best point */
        for (i=0; i<DIMX; i++) {
          x[i]=best_point[i];
        }
        for (j=0; j<DIMX; j++) {
          for (k=0; k<=j; k++) {
            x[i++] = x[j]*x[k];
          }
        }
        for (; i<DIMX+DIMZ+DIMY; i++) {
          x[i]=best_point[i-DIMZ];
        }
      }
      break;
    case 1:
      {
        opt=nlopt_create(NLOPT_ALG,DIMX);
        if (!opt) {
          fprintf(stderr, "nlopt_create failed\n");
          exit(1);
        }
        NLOPT_CALL(nlopt_set_lower_bounds,(opt,problem->xlow));
        NLOPT_CALL(nlopt_set_upper_bounds,(opt,problem->xup));
        NLOPT_CALL(nlopt_set_min_objective,(opt,eval_f_1,data));
        if (ignore_constraints) {
          /* starting value: center of box */
          for (i=0; i<DIMX; i++) {
            x[i]=(problem->xlow[i]+problem->xup[i])*.5;
          }
        } else {
          for (i=0; i<(int)((struct GMM_problem *)data)->
                            estimatec->num_estimate_constraints; i++) {
            struct g1_data *user_data=alloca(sizeof(struct g1_data));
            user_data->problem=problem;
            user_data->estimate_constraint_coeff_vec=estimate_constraint_coeffs(
              ((struct GMM_problem *)data)->estimatec)[i];
            user_data->estimate_constraint_tol
              =((struct GMM_problem *)data)->estimate_constraint_tol;
            NLOPT_CALL(nlopt_add_inequality_constraint,
                       (opt,eval_g_1,user_data,1e-12));
          }
          /* starting value: unconstrained minimum */
          for (i=0; i<DIMX; i++) {
            x[i]=optimum_x[i];
          }
        }
      }
      break;
  }

  NLOPT_CALL(nlopt_set_ftol_rel,(opt,optimization_problem ? 1e-13 : 5e-13));
  NLOPT_CALL(nlopt_set_ftol_abs,(opt,1e-18));
  NLOPT_CALL(nlopt_set_xtol_rel,(opt,1e-9));
  NLOPT_CALL(nlopt_set_xtol_abs1,(opt,1e-13));
  NLOPT_CALL(nlopt_set_maxeval,(opt,20000));

  double opt_f;
  int ret=nlopt_optimize(opt,x,&opt_f);
  nlopt_destroy(opt);
  /* remember optimum x */
  for (i=0; i<DIMX; i++) optimum_x[i]=x[i];
  return ret;
}
