/* bbowda - Black Box Optimization With Data Analysis
   Copyright (C) 2006-2012 Kevin Kofler <Kevin@tigcc.ticalc.org>
   Copyright (C) 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 <IpStdCInterface.h>

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

#undef USE_BFGS /* instead of symbolic Hessian */

/* The number of nonzero entries in the lower triangular part of a full
   symmetric matrix. */
#define FULL_SYMM_MAT(n) (((n)*((n)+1))>>1)

static Bool eval_f_0(Index n, Number* x, Bool new_x,
                     Number* obj_value, UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  Index i;
  Number f=0.;
  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)];
  }
  *obj_value=f;
  return TRUE;
}

static Bool eval_grad_f_0(Index n, Number* x, Bool new_x,
                          Number* grad_f, UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  Index 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 TRUE;
}


static Bool eval_g_0(Index n, Number* x, Bool new_x,
                     Index m, Number* g, UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  Index i,k,l;

  /* compute (X-Xbar)T M (X-Xbar) */
  Number X[DIMX+DIMZ+DIMY+DIMY_EQ], MX[DIMX+DIMZ+DIMY+DIMY_EQ];
  Number 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];
  }
  *g=z;

  for (i=1; i<=DIMZ; i++) {
    /* compute zi-xk*xl */
    k=0;
    l=i-1;
    while (l>k) {
      k++;
      l-=k;
    }
    g[i]=x[i+(DIMX-1)]-x[k]*x[l];
  }
  return TRUE;
}


static Bool eval_jac_g_0(Index n, Number *x, Bool new_x,
                         Index m, Index nele_jac,
                         Index *iRow, Index *jCol, Number *values,
                         UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  if (!values) { /* sparsity structure */
    Index i=1,j,k,l;
    /* first row full */
    for (j=0; j<(DIMX+DIMZ+DIMY); j++) {
      iRow[j]=0; jCol[j]=j;
    }
    /* equality constraints */
    for (k=0; k<DIMX; k++) {
      for (l=0; l<k; l++) { /* bilinear terms in z */
        iRow[j]=i; jCol[j++]=l;
        iRow[j]=i; jCol[j++]=k;
        iRow[j]=i; jCol[j++]=(i++)+(DIMX-1);
      }
      /* quadratic terms in z */
      iRow[j]=i; jCol[j++]=k;
      iRow[j]=i; jCol[j++]=(i++)+(DIMX-1);
    }
  } else { /* values */
    Index j=0,k,l;
    /* compute ((X-Xbar)T M (X-Xbar))' = 2 M (X-Xbar) */
    Number X[DIMX+DIMZ+DIMY+DIMY_EQ], MX[DIMX+DIMZ+DIMY+DIMY_EQ];
    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; k++) {
      Number z=MX[k];
      values[j++]=2.*z; /* Jac[0][j] */
    }
    /* equality constraints */
    for (k=0; k<DIMX; k++) {
      for (l=0; l<k; l++) { /* bilinear terms in z */
        values[j++]=-x[k]; /* Jac[i][k] */
        values[j++]=-x[l]; /* Jac[i][l] */
        values[j++]=1.; /* Jac[i][i+(DIMX-1)] */
      }
      /* quadratic terms in z */
      values[j++]=-2.*x[k]; /* Jac[i][k] */
      values[j++]=1.; /* Jac[i][i+(DIMX-1)] */
    }
  }
  return TRUE;
}


static Bool eval_h_0(Index n, Number *x, Bool new_x, Number obj_factor,
                     Index m, Number *lambda, Bool new_lambda,
                     Index nele_hess, Index *iRow, Index *jCol,
                     Number *values, UserDataPtr user_data)
{
#ifdef USE_BFGS
  return FALSE;
#else
  const struct bbowda_problem *problem
    =((struct covar_problem *)user_data)->problem;
  if (!values) { /* sparsity structure */
    /* full Hessian */
    Index k=0,i,j;
    for (i=0; i<DIMX+DIMZ+DIMY; i++) {
      for (j=0; j<=i; j++) {
        iRow[k]=i; jCol[k++]=j;
      }
    }
  } else { /* values */
    /* The objective function is linear, skip it. */
    /* first constraint: Hessian = 2 M */
    Index k=0,i,j,l;
    Number X[DIMX+DIMZ+DIMY+DIMY_EQ], MX[DIMX+DIMZ+DIMY+DIMY_EQ];
    for (i=0; i<DIMX+DIMZ+DIMY; i++) {
      for (l=0; l<DIMX+DIMZ+DIMY+DIMY_EQ; l++) {
        X[l]=(l==i)?(2.*(*lambda)):0.;
      }
      /* M is symmetric, so we'll get away with computing columns instead of rows */
      compute_MX(((struct covar_problem *)user_data)->covar,problem,X,MX);
      for (j=0; j<=i; j++) {
        values[k++]=MX[j];
      }
    }
    /* equality constraints */
    j=0;
    for (k=0; k<DIMX; k++) {
      for (l=0; l<k; l++) { /* bilinear terms in z */
        values[j]-=lambda[j+1];
        j++;
      }
      /* quadratic terms in z */
      values[j]-=2.*lambda[j+1];
      j++;
    }
  }
  return TRUE;
#endif
}

static Bool eval_f_1(Index n, Number* x, Bool new_x,
                     Number* obj_value, UserDataPtr user_data)
{
  *obj_value=gmm_prob(((struct GMM_problem *)user_data)->gmm,
                      ((struct GMM_problem *)user_data)->problem,x);
  return TRUE;
}

static Bool eval_grad_f_1(Index n, Number* x, Bool new_x,
                          Number* grad_f, UserDataPtr user_data)
{
  gmm_grad(((struct GMM_problem *)user_data)->gmm,
           ((struct GMM_problem *)user_data)->problem,x,grad_f);
  return TRUE;
}


static Bool eval_g_1(Index n, Number* x, Bool new_x,
                     Index m, Number* g, UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct GMM_problem *)user_data)->problem;
  if (m) {
    Index i,k,l;
    for (i=0; i<m; i++) {
      /* evaluate quadratic polynomial */
      Number *p=estimate_constraint_coeffs(
        ((struct GMM_problem *)user_data)->estimatec)[i];
      Number 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]);
        }
      }
      g[i]=z;
    }
  }
  return TRUE;
}


static Bool eval_jac_g_1(Index n, Number *x, Bool new_x,
                         Index m, Index nele_jac,
                         Index *iRow, Index *jCol, Number *values,
                         UserDataPtr user_data)
{
  const struct bbowda_problem *problem
    =((struct GMM_problem *)user_data)->problem;
  if (m) {
    if (!values) { /* sparsity structure */
      /* full Jacobian */
      Index k=0,i,j;
      for (i=0; i<m; i++) {
        for (j=0; j<DIMX; j++) {
          iRow[k]=i; jCol[k++]=j;
        }
      }
    } else { /* values */
      Index i,k,l;
      for (i=0; i<m; i++) {
        /* evaluate gradient of the quadratic polynomial */
        Number *p=estimate_constraint_coeffs(
          ((struct GMM_problem *)user_data)->estimatec)[i]+1; /* skip constant term */
        Number *grad=values+i*DIMX;
        for (k=0; k<DIMX; k++) { /* linear terms (constant derivatives) */
          grad[k]=*(p++);
        }
        for (k=0; k<DIMX; k++) { /* quadratic/bilinear terms (linear derivatives) */
          for (l=0; l<=k; l++) {
            grad[k]+=*p * x[l];
            grad[l]+=*(p++) * x[k];
            /* If k==l, then this is grad[k][i+shift]+=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. */
          }
        }
      }
    }
  }
  return TRUE;
}


static Bool eval_h_1(Index n, Number *x, Bool new_x, Number obj_factor,
                     Index m, Number *lambda, Bool new_lambda,
                     Index nele_hess, Index *iRow, Index *jCol,
                     Number *values, UserDataPtr user_data)
{
#ifdef USE_BFGS
  return FALSE;
#else
  const struct bbowda_problem *problem
    =((struct GMM_problem *)user_data)->problem;
  if (!values) { /* sparsity structure */
    /* full Hessian */
    Index k=0,i,j;
    for (i=0; i<DIMX; i++) {
      for (j=0; j<=i; j++) {
        iRow[k]=i; jCol[k++]=j;
      }
    }
  } else { /* values */
    Index i,k,l;
    /* Hessian of the objective function */
    gmm_hess(((struct GMM_problem *)user_data)->gmm,problem,x,values);
    for (i=0; i<FULL_SYMM_MAT(DIMX); i++) {
      values[i]*=obj_factor;
    }
    if (m) {
      for (i=0; i<m; i++) {
        /* Hessian of the quadratic polynomial */
        Number *p=estimate_constraint_coeffs(
          ((struct GMM_problem *)user_data)->estimatec)[i]+(1+DIMX); /* skip constant and linear terms */
        Number *q=values;
        for (k=0; k<DIMX; k++) { /* quadratic/bilinear terms (linear derivatives) */
          for (l=0; l<=k; l++) {
            *(q++)+=lambda[i]*((k==l)?2.:1.)*(*(p++));
          }
        }
      }
    }
  }
  return TRUE;
#endif
}

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;
  Number x[DIMX+DIMZ+DIMY];
  IpoptProblem ipopt_problem=NULL;
  Index i,j,k,l;

  switch (optimization_problem) {
    case 0:
      {
        /* lower and upper bounds */
        Number x_L[DIMX+DIMZ+DIMY];
        Number x_U[DIMX+DIMZ+DIMY];
        Number g_L[1+DIMZ];
        Number g_U[1+DIMZ];
        /* 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]] */
            Number 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]};
            Number 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)];
        }
        /* covariance model interval constraint */
        *g_L=((struct covar_problem *)data)->covar->klow;
        *g_U=((struct covar_problem *)data)->covar->kup;
        /* equality constraints */
        for (i=1; i<=DIMZ; i++) {
          g_L[i]=0.;
          g_U[i]=0.;
        }
        /* nele_jac: first row full: DIMX+DIMZ+DIMY
                     + rows for quadratic terms in z: 2 nonzero entries * DIMX
                     + rows for bilinear terms in z: 3 nonzero entries * (DIMZ-DIMX)
                     = DIMX+DIMZ+DIMY + 2*DIMX + 3*(DIMZ-DIMX)
                     = DIMX+DIMZ+DIMY + 2*DIMX + 3*DIMZ - 3*DIMX
                     = DIMY + 4*DIMZ */
        ipopt_problem=CreateIpoptProblem(/*n=*/DIMX+DIMZ+DIMY,x_L,x_U,
                                         /*m=*/1+DIMZ,g_L,g_U,
                                         /*nele_jac=*/DIMY+(DIMZ<<2),
                                         /*nele_hess=*/FULL_SYMM_MAT(DIMX+DIMZ+DIMY) /*full Hessian*/,
                                         /*index_style=*/0,
                                         eval_f_0,eval_g_0,eval_grad_f_0,
                                         eval_jac_g_0, eval_h_0);
        /* 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:
      {
        Index num_constraints=ignore_constraints?0
          :(Index)
           ((struct GMM_problem *)data)->estimatec->num_estimate_constraints;
        /* estimate constraints */
        Number g_L[num_constraints];
        Number g_U[num_constraints];
        for (j=0; j<(Index)num_constraints; j++) {
          g_L[j]=-((struct GMM_problem *)data)->estimate_constraint_tol;
          g_U[j]=INF;
        }
        ipopt_problem=CreateIpoptProblem(/*n=*/DIMX,
                                         /*x_L=*/(double *)problem->xlow,
                                         /*x_U=*/(double *)problem->xup,
                                         /*m=*/num_constraints,g_L,g_U,
                                         /*nele_jac=*/DIMX*num_constraints /* full Jacobian */,
                                         /*nele_hess=*/FULL_SYMM_MAT(DIMX) /* full Hessian */,
                                         /*index_style=*/0,
                                         eval_f_1,eval_g_1,eval_grad_f_1,
                                         eval_jac_g_1, eval_h_1);
        if (ignore_constraints) {
          /* starting value: center of box */
          for (i=0; i<DIMX; i++) {
            x[i]=(problem->xlow[i]+problem->xup[i])*.5;
          }
        } else {
          /* starting value: unconstrained minimum */
          for (i=0; i<DIMX; i++) {
            x[i]=optimum_x[i];
          }
        }
      }
      break;
  }

  if (!ipopt_problem) {
    fprintf(stderr, "CreateIpoptProblem failed\n");
    exit(1);
  }
  /* Scale objective for better performance */
  if (optimization_problem) {
    if (!AddIpoptNumOption(ipopt_problem,(char *)"obj_scaling_factor",16384.)) {
      fprintf(stderr, "AddIpoptNumOption failed\n");
      exit(1);
    }
  }
#ifdef USE_BFGS
  /* These are of course const char * (the underlying C++ functions take const
     std::string &), but the prototype is wrong, so silence -Wwrite-strings
     warnings. */
  if (!AddIpoptStrOption(ipopt_problem,(char *)"hessian_approximation",(char *)"limited-memory")) {
    fprintf(stderr, "AddIpoptStrOption failed\n");
    exit(1);
  }
#endif
  if (!AddIpoptIntOption(ipopt_problem,(char *)"print_level",2 /*J_WARNING*/)) {
    fprintf(stderr, "AddIpoptIntOption failed\n");
    exit(1);
  }
  int ret=IpoptSolve(ipopt_problem,x,NULL,NULL,NULL,NULL,NULL,data);
  FreeIpoptProblem(ipopt_problem);
  /* remember optimum x */
  for (i=0; i<DIMX; i++) optimum_x[i]=x[i];
  return ret;
}
