// Copyright (C) 2005, 2007 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// $Id: MittelmannBndryCntrlDiri3Dsin.cpp 2005 2011-06-06 12:55:16Z stefan $
//
// Authors:  Andreas Waechter             IBM    2005-10-18
//           Olaf Schenk   (Univ. of Basel)      2007-08-01
//              modified MittelmannBndryCntrlDiri.cpp for 3-dim problem

#include "MittelmannBndryCntrlDiri3Dsin.hpp"

#ifdef HAVE_CASSERT
# include <cassert>
#else
# ifdef HAVE_ASSERT_H
#  include <assert.h>
# else
#  error "don't have header file for assert"
# endif
#endif

using namespace Ipopt;

/* Constructor. */
MittelmannBndryCntrlDiriBase3Dsin::MittelmannBndryCntrlDiriBase3Dsin()
    :
    y_d_(NULL)
{}

MittelmannBndryCntrlDiriBase3Dsin::~MittelmannBndryCntrlDiriBase3Dsin()
{
  delete [] y_d_;
}

void
MittelmannBndryCntrlDiriBase3Dsin::SetBaseParameters(Index N, Number alpha, Number lb_y,
    Number ub_y, Number lb_u, Number ub_u,
    Number d_const)
{
  N_ = N;
  h_ = 1./(N+1);
  hh_ = h_*h_;
  lb_y_ = lb_y;
  ub_y_ = ub_y;
  lb_u_ = lb_u;
  ub_u_ = ub_u;
  d_const_ = d_const;
  alpha_ = alpha;

  // Initialize the target profile variables
  delete [] y_d_;
  y_d_ = new Number[(N_+2)*(N_+2)*(N_+2)];
  for (Index k=0; k<= N_+1; k++) {
    for (Index j=0; j<= N_+1; j++) {
      for (Index i=0; i<= N_+1; i++) {
        y_d_[y_index(i,j,k)] = y_d_cont(x1_grid(i),x2_grid(j),x3_grid(k));
      }
    }
  }
}

bool MittelmannBndryCntrlDiriBase3Dsin::get_nlp_info(
  Index& n, Index& m, Index& nnz_jac_g,
  Index& nnz_h_lag, IndexStyleEnum& index_style)
{
  // We for each of the N_+2 times N_+2 times N_+2  mesh points we have
  // the value of the functions y, including the control parameters on
  // the boundary
  n = (N_+2)*(N_+2)*(N_+2);

  // For each of the N_ times N_ times N_ interior mesh points we have the
  // discretized PDE.
  m = N_*N_*N_;

  // y(i,j,k), y(i-1,j,k), y(i+1,j,k), y(i,j-1,k), y(i,j+1,k),
  // y(i-1,j,k-1), y(i,j,k+1)
  // of the N_*N_*N_ discretized PDEs
  nnz_jac_g = 7*N_*N_*N_;

  // diagonal entry for each y(i,j) in the interior
  nnz_h_lag = N_*N_*N_;
  if (alpha_>0.) {
    // and one entry for u(i,j) in the bundary if alpha is not zero
    nnz_h_lag += 21*6*N_*N_;
  }

  // We use the C indexing style for row/col entries (corresponding to
  // the C notation, starting at 0)
  index_style = C_STYLE;

  return true;
}

bool
MittelmannBndryCntrlDiriBase3Dsin::get_bounds_info(Index n, Number* x_l, Number* x_u,
    Index m, Number* g_l, Number* g_u)
{
  // Set overall bounds on the y variables
  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      for (Index k=0; k<=N_+1; k++) {
        Index iy = y_index(i,j,k);
        x_l[iy] = lb_y_;
        x_u[iy] = ub_y_;
      }
    }
  }
  // Set the overall 3Dsin bounds on the control variables
  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(i,j,0);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(i,j,N_+1);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(i,0,k);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(i,N_+1,k);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }
  for (Index j=0; j<=N_+1; j++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(0,j,k);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }
  for (Index j=0; j<=N_+1; j++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(N_+1,j,k);
      x_l[iu] = lb_u_;
      x_u[iu] = ub_u_;
    }
  }

#if 0
  // The values of y on the corners doens't appear anywhere, so we fix
  // them to zero
  for (Index j=0; j<=N_+1; j++) {
    x_l[y_index(0,j,0)] = x_u[y_index(0,j,0)] = 0.;
    x_l[y_index(0,j,N_+1)] = x_u[y_index(0,j,N_+1)] = 0.;
    x_l[y_index(N_+1,j,0)] = x_u[y_index(N_+1,j,0)] = 0.;
    x_l[y_index(N_+1,j,N_+1)] = x_u[y_index(N_+1,j,N_+1)] = 0.;
  }
  for (Index k=0; k<=N_+1; k++) {
    x_l[y_index(0,0,k)] = x_u[y_index(0,0,k)] = 0.;
    x_l[y_index(0,N_+1,k)] = x_u[y_index(0,N_+1,k)] = 0.;
    x_l[y_index(N_+1,0,k)] = x_u[y_index(N_+1,0,k)] = 0.;
    x_l[y_index(N_+1,N_+1,k)] = x_u[y_index(N_+1,N_+1,k)] = 0.;
  }
  for (Index i=0; i<=N_+1; i++) {
    x_l[y_index(i,0,0)] = x_u[y_index(i,0,0)] = 0.;
    x_l[y_index(i,0,N_+1)] = x_u[y_index(i,0,N_+1)] = 0.;
    x_l[y_index(i,N_+1,0)] = x_u[y_index(i,N_+1,0)] = 0.;
    x_l[y_index(i,N_+1,N_+1)] = x_u[y_index(i,N_+1,N_+1)] = 0.;
  }
#endif

  // all discretized PDE constraints have right hand side equal to
  // minus the constant value of the function d
  for (Index i=0; i<m; i++) {
    g_l[i] = -hh_*d_const_;
    g_u[i] = -hh_*d_const_;
  }

  return true;
}

bool
MittelmannBndryCntrlDiriBase3Dsin::get_starting_point(Index n, bool init_x, Number* x,
    bool init_z, Number* z_L, Number* z_U,
    Index m, bool init_lambda,
    Number* lambda)
{
  // Here, we assume we only have starting values for x, if you code
  // your own NLP, you can provide starting values for the others if
  // you wish.
  assert(init_x == true);
  assert(init_z == false);
  assert(init_lambda == false);

  // set all y's to the perfect match with y_d
  for (Index i=0; i<= N_+1; i++) {
    for (Index j=0; j<= N_+1; j++) {
      for (Index k=0; k<= N_+1; k++) {
        x[y_index(i,j,k)] = y_d_[y_index(i,j,k)]; // 0 in AMPL model
      }
    }
  }

  Number umid = (ub_u_ + lb_u_)/2.;
  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(i,j,0);
      x[iu] = umid;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(i,j,N_+1);
      x[iu] = umid;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(i,0,k);
      x[iu] = umid;
    }
  }
  for (Index i=0; i<=N_+1; i++) {
    for (Index k=0; k<=N_+1; k++) {
      Index iu = y_index(i,N_+1,k);
      x[iu] = umid;
    }
  }
  for (Index k=0; k<=N_+1; k++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(0,j,k);
      x[iu] = umid;
    }
  }
  for (Index k=0; k<=N_+1; k++) {
    for (Index j=0; j<=N_+1; j++) {
      Index iu = y_index(N_+1,j,k);
      x[iu] = umid;
    }
  }


  return true;
}

bool
MittelmannBndryCntrlDiriBase3Dsin::get_scaling_parameters(Number& obj_scaling,
    bool& use_x_scaling, Index n, Number* x_scaling,
    bool& use_g_scaling, Index m, Number* g_scaling)
{
  obj_scaling = 1./hh_;
  use_x_scaling = false;
  use_g_scaling = false;
  return true;
}

bool
MittelmannBndryCntrlDiriBase3Dsin::eval_f(Index n, const Number* x,
    bool new_x, Number& obj_value)
{
  // return the value of the objective function
  obj_value = 0.;

  // First the integration of y-td over the interior
  for (Index i=1; i<=N_; i++) {
    for (Index j=1; j<= N_; j++) {
      for (Index k=1; k<= N_; k++) {
        Index iy = y_index(i,j,k);
        Number tmp = x[iy] - y_d_[iy];
        obj_value += tmp*tmp;
      }
    }
  }
  obj_value *= hh_/2.;

  // Now the integration of u over the boundary
  if (alpha_>0.) {
    Number usum = 0.;

    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,j,0)]
                          - x[y_index(i-1,j,0)] - x[y_index(i+1,j,0)]
                          - x[y_index(i,j-1,0)] - x[y_index(i,j+1,0)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }
    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,j,N_+1)]
                          - x[y_index(i-1,j,N_+1)] - x[y_index(i+1,j,N_+1)]
                          - x[y_index(i,j-1,N_+1)] - x[y_index(i,j+1,N_+1)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }

    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(0,i,j)]
                          - x[y_index(0,i-1,j)] - x[y_index(0,i+1,j)]
                          - x[y_index(0,i,j-1)] - x[y_index(0,i,j+1)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }
    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(N_+1,i,j)]
                          - x[y_index(N_+1,i-1,j)] - x[y_index(N_+1,i+1,j)]
                          - x[y_index(N_+1,i,j-1)] - x[y_index(N_+1,i,j+1)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }

    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,0,j)]
                          - x[y_index(i-1,0,j)] - x[y_index(i+1,0,j)]
                          - x[y_index(i,0,j-1)] - x[y_index(i,0,j+1)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }
    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,N_+1,j)]
                          - x[y_index(i-1,N_+1,j)] - x[y_index(i+1,N_+1,j)]
                          - x[y_index(i,N_+1,j-1)] - x[y_index(i,N_+1,j+1)])/hh_;
        const Number sinD = sin(D) - 0.5;
        usum += sinD*sinD;
      }
    }

    obj_value += alpha_*h_/2.*usum;
  }

  return true;
}

bool
MittelmannBndryCntrlDiriBase3Dsin::eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f)
{
  // return the gradient of the objective function grad_{x} f(x)

  // now let's take care of the nonzero values coming from the
  // integrant over the interior
  for (Index i=1; i<=N_; i++) {
    for (Index j=1; j<= N_; j++) {
      for (Index k=1; k<= N_; k++) {
        Index iy = y_index(i,j,k);
        grad_f[iy] = hh_*(x[iy] - y_d_[iy]);
      }
    }
  }

  for (Index i=0; i<= N_+1; i++) {
    for (Index j=0; j<= N_+1; j++) {
      grad_f[y_index(i,j,0)] = 0.;
    }
  }
  for (Index i=0; i<= N_+1; i++) {
    for (Index j=0; j<= N_+1; j++) {
      grad_f[y_index(i,j,N_+1)] = 0.;
    }
  }

  for (Index k=0; k<= N_+1; k++) {
    for (Index j=0; j<= N_+1; j++) {
      grad_f[y_index(0,j,k)] = 0.;
    }
  }
  for (Index k=0; k<= N_+1; k++) {
    for (Index j=0; j<= N_+1; j++) {
      grad_f[y_index(N_+1,j,k)] = 0.;
    }
  }

  for (Index i=0; i<= N_+1; i++) {
    for (Index k=0; k<= N_+1; k++) {
      grad_f[y_index(i,0,k)] = 0.;
    }
  }
  for (Index i=0; i<= N_+1; i++) {
    for (Index k=0; k<= N_+1; k++) {
      grad_f[y_index(i,N_+1,k)] = 0.;
    }
  }

  // The values for variables on the boundary
  if (alpha_>0.) {
    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        const Number D = (4*x[y_index(i,j,0)] - x[y_index(i-1,j,0)] - x[y_index(i+1,j,0)] - x[y_index(i,j-1,0)] - x[y_index(i,j+1,0)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(i,j,0)] += 4.*FD;
        grad_f[y_index(i-1,j,0)] += -1.*FD;
        grad_f[y_index(i+1,j,0)] += -1.*FD;
        grad_f[y_index(i,j-1,0)] += -1.*FD;
        grad_f[y_index(i,j+1,0)] += -1.*FD;
      }
    }


    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        const Number D = (4*x[y_index(i,j,N_+1)]
                          - x[y_index(i-1,j,N_+1)] - x[y_index(i+1,j,N_+1)]
                          - x[y_index(i,j-1,N_+1)] - x[y_index(i,j+1,N_+1)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(i,j,N_+1)] += 4.*FD;
        grad_f[y_index(i-1,j,N_+1)] += -1.*FD;
        grad_f[y_index(i+1,j,N_+1)] += -1.*FD;
        grad_f[y_index(i,j-1,N_+1)] += -1.*FD;
        grad_f[y_index(i,j+1,N_+1)] += -1.*FD;
      }
    }

    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(0,i,j)]
                          - x[y_index(0,i-1,j)] - x[y_index(0,i+1,j)]
                          - x[y_index(0,i,j-1)] - x[y_index(0,i,j+1)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(0,i,j)] += 4.*FD;
        grad_f[y_index(0,i-1,j)] += -1.*FD;
        grad_f[y_index(0,i+1,j)] += -1.*FD;
        grad_f[y_index(0,i,j-1)] += -1.*FD;
        grad_f[y_index(0,i,j+1)] += -1.*FD;
      }
    }
    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(N_+1,i,j)]
                          - x[y_index(N_+1,i-1,j)] - x[y_index(N_+1,i+1,j)]
                          - x[y_index(N_+1,i,j-1)] - x[y_index(N_+1,i,j+1)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(N_+1,i,j)] += 4.*FD;
        grad_f[y_index(N_+1,i-1,j)] += -1.*FD;
        grad_f[y_index(N_+1,i+1,j)] += -1.*FD;
        grad_f[y_index(N_+1,i,j-1)] += -1.*FD;
        grad_f[y_index(N_+1,i,j+1)] += -1.*FD;
      }
    }

    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,0,j)]
                          - x[y_index(i-1,0,j)] - x[y_index(i+1,0,j)]
                          - x[y_index(i,0,j-1)] - x[y_index(i,0,j+1)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(i,0,j)] += 4.*FD;
        grad_f[y_index(i-1,0,j)] += -1.*FD;
        grad_f[y_index(i+1,0,j)] += -1.*FD;
        grad_f[y_index(i,0,j-1)] += -1.*FD;
        grad_f[y_index(i,0,j+1)] += -1.*FD;
      }
    }
    for (Index i=1; i<=N_; i++) {
      for (Index j=1; j<=N_; j++) {
        const Number D = (4*x[y_index(i,N_+1,j)]
                          - x[y_index(i-1,N_+1,j)] - x[y_index(i+1,N_+1,j)]
                          - x[y_index(i,N_+1,j-1)] - x[y_index(i,N_+1,j+1)])/hh_;
        const Number FD = alpha_*h_*(cos(D)*(sin(D) - 0.5)/hh_);
        grad_f[y_index(i,N_+1,j)] += 4.*FD;
        grad_f[y_index(i-1,N_+1,j)] += -1.*FD;
        grad_f[y_index(i+1,N_+1,j)] += -1.*FD;
        grad_f[y_index(i,N_+1,j-1)] += -1.*FD;
        grad_f[y_index(i,N_+1,j+1)] += -1.*FD;
      }
    }

  }

  return true;
}

bool MittelmannBndryCntrlDiriBase3Dsin::eval_g(Index n, const Number* x, bool new_x,
    Index m, Number* g)
{
  // return the value of the constraints: g(x)

  // compute the discretized PDE for each interior grid point
  Index ig = 0;
  for (Index i=1; i<=N_; i++) {
    for (Index j=1; j<=N_; j++) {
      for (Index k=1; k<=N_; k++) {
        Number val;

        // Start with the discretized Laplacian operator
        val = 6.* x[y_index(i,j,k)]
              - x[y_index(i-1,j,k)] - x[y_index(i+1,j,k)]
              - x[y_index(i,j-1,k)] - x[y_index(i,j+1,k)]
              - x[y_index(i,j,k-1)] - x[y_index(i,j,k+1)];

        g[ig] = val;
        ig++;
      }
    }
  }

  DBG_ASSERT(ig==m);

  return true;
}

bool MittelmannBndryCntrlDiriBase3Dsin::eval_jac_g(Index n, const Number* x, bool new_x,
    Index m, Index nele_jac, Index* iRow, Index *jCol,
    Number* values)
{
  if (values == NULL) {
    // return the structure of the jacobian of the constraints

    Index ijac = 0;
    Index ig = 0;
    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        for (Index k=1; k<= N_; k++) {

          // y(i,j,k)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i,j,k);
          ijac++;

          // y(i-1,j,k)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i-1,j,k);
          ijac++;

          // y(i+1,j,k)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i+1,j,k);
          ijac++;

          // y(i,j-1,k)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i,j-1,k);
          ijac++;

          // y(i,j+1,k)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i,j+1,k);
          ijac++;

          // y(i,j,k-1)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i,j,k-1);
          ijac++;

          // y(i,j,k+1)
          iRow[ijac] = ig;
          jCol[ijac] = y_index(i,j,k+1);
          ijac++;

          ig++;
        }
      }
    }

    DBG_ASSERT(ijac==nele_jac);
  }
  else {
    // return the values of the jacobian of the constraints
    Index ijac = 0;
    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        for (Index k=1; k<= N_; k++) {
          // y(i,j,k)
          values[ijac] = 6.;
          ijac++;

          // y(i-1,j,k)
          values[ijac] = -1.;
          ijac++;

          // y(i+1,j,k)
          values[ijac] = -1.;
          ijac++;

          // y(1,j-1,k)
          values[ijac] = -1.;
          ijac++;

          // y(1,j+1,k)
          values[ijac] = -1.;
          ijac++;

          // y(1,j,k-1)
          values[ijac] = -1.;
          ijac++;

          // y(1,j,k+1)
          values[ijac] = -1.;
          ijac++;
        }
      }
    }

    DBG_ASSERT(ijac==nele_jac);
  }

  return true;
}

inline static void hessstruct(Index* iRow, Index* jCol, Index ij, Index imj, Index ipj, Index ijm, Index ijp, Index& ihes)
{
  //printf("ihes = %3d ij = %3d imj = %3d ipj = %3d ijm = %3d ijp = %3d\n",ihes,ij,imj,ipj,ijm,ijp);
  iRow[ihes] = ij;
  jCol[ihes] = ij;
  ihes++;

  Index firstidx = ij;
  iRow[ihes] = firstidx;
  jCol[ihes] = imj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ipj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijm;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijp;
  ihes++;

  iRow[ihes] = imj;
  jCol[ihes] = imj;
  ihes++;
  iRow[ihes] = ipj;
  jCol[ihes] = ipj;
  ihes++;
  iRow[ihes] = ijm;
  jCol[ihes] = ijm;
  ihes++;
  iRow[ihes] = ijp;
  jCol[ihes] = ijp;
  ihes++;

  firstidx = imj;
  //iRow[ihes] = firstidx;
  //jCol[ihes] = imj;
  //ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ipj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijm;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijp;
  ihes++;

  firstidx = ipj;
  iRow[ihes] = firstidx;
  jCol[ihes] = imj;
  ihes++;
  //iRow[ihes] = firstidx;
  //jCol[ihes] = ipj;
  //ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijm;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijp;
  ihes++;

  firstidx = ijm;
  iRow[ihes] = firstidx;
  jCol[ihes] = imj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ipj;
  ihes++;
  //iRow[ihes] = firstidx;
  //jCol[ihes] = ijm;
  //ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijp;
  ihes++;

  firstidx = ijp;
  iRow[ihes] = firstidx;
  jCol[ihes] = imj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ipj;
  ihes++;
  iRow[ihes] = firstidx;
  jCol[ihes] = ijm;
  ihes++;
  //iRow[ihes] = firstidx;
  //jCol[ihes] = ijp;
  //ihes++;
}

inline static void hessvals(const Number* x, Number* values, Index ij, Index imj, Index ipj, Index ijm, Index ijp, Index& ihes, Number hh_, Number fact)
{
  //printf("ihes = %d\n",ihes);
  const Number D = (4*x[ij] - x[imj] - x[ipj] - x[ijm] - x[ijp])/hh_;
  const Number val = fact*(1. + 0.5*sin(D) -2.*sin(D)*sin(D));

  values[ihes] = 16.*val;
  ihes++;

  for (Index i=0; i<4; i++) {
    values[ihes] = -4.*val;
    ihes++;
  }

  for (Index i=0; i<4; i++) {
    values[ihes] = val;
    ihes++;
  }

  for (Index i=0; i<12; i++) {
    values[ihes] = .5*val;
    ihes++;
  }
}


bool
MittelmannBndryCntrlDiriBase3Dsin::eval_h(Index n, const Number* x, bool new_x,
    Number obj_factor, Index m,
    const Number* lambda,
    bool new_lambda, Index nele_hess, Index* iRow,
    Index* jCol, Number* values)
{
  if (values == NULL) {
    // return the structure. This is a symmetric matrix, fill the lower left
    // triangle only.

    Index ihes=0;
    // First the diagonal entries for y(i,j)
    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        for (Index k=1; k<= N_; k++) {
          iRow[ihes] = y_index(i,j,k);
          jCol[ihes] = y_index(i,j,k);
          ihes++;
        }
      }
    }

    if (alpha_>0.) {

      // Now the diagonal entries for u at the boundary
      for (Index i=1; i<=N_; i++) {
        for (Index j=1; j<=N_; j++) {
          hessstruct(iRow, jCol, y_index (i,j,0), y_index (i-1,j,0),  y_index (i+1,j,0), y_index (i,j-1,0), y_index (i,j+1,0), ihes);

          hessstruct(iRow, jCol, y_index (i,j,N_+1), y_index (i-1,j,N_+1),  y_index (i+1,j,N_+1), y_index (i,j-1,N_+1), y_index (i,j+1,N_+1), ihes);

          hessstruct(iRow, jCol, y_index (0,i,j), y_index (0,i-1,j),  y_index (0,i+1,j), y_index (0,i,j-1), y_index (0,i,j+1), ihes);

          hessstruct(iRow, jCol, y_index (N_+1,i,j), y_index (N_+1,i-1,j),  y_index (N_+1,i+1,j), y_index (N_+1,i,j-1), y_index (N_+1,i,j+1), ihes);

          hessstruct(iRow, jCol, y_index (i,0,j), y_index (i-1,0,j),  y_index (i+1,0,j), y_index (i,0,j-1), y_index (i,0,j+1), ihes);

          hessstruct(iRow, jCol, y_index (i,N_+1,j), y_index (i-1,N_+1,j),  y_index (i+1,N_+1,j), y_index (i,N_+1,j-1), y_index (i,N_+1,j+1), ihes);
        }
      }

    }
    DBG_ASSERT(ihes==nele_hess);
  }
  else {
    // return the values

    Index ihes=0;
    // First the diagonal entries for y(i,j)
    for (Index i=1; i<= N_; i++) {
      for (Index j=1; j<= N_; j++) {
        for (Index k=1; k<= N_; k++) {
          // Contribution from the objective function
          values[ihes] = obj_factor*hh_;

          ihes++;
        }
      }
    }

    // Now the diagonal entries for u(i,j)
    if (alpha_>0.) {
      // Now the diagonal entries for u at the boundary
      for (Index i=1; i<=N_; i++) {
        for (Index j=1; j<=N_; j++) {
          hessvals(x, values, y_index (i,j,0), y_index (i-1,j,0),  y_index (i+1,j,0), y_index (i,j-1,0), y_index (i,j+1,0), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));
          hessvals(x, values, y_index (i,j,N_+1), y_index (i-1,j,N_+1),  y_index (i+1,j,N_+1), y_index (i,j-1,N_+1), y_index (i,j+1,N_+1), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));

          hessvals(x, values, y_index (0,i,j), y_index (0,i-1,j),  y_index (0,i+1,j), y_index (0,i,j-1), y_index (0,i,j+1), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));
          hessvals(x, values, y_index (N_+1,i,j), y_index (N_+1,i-1,j),  y_index (N_+1,i+1,j), y_index (N_+1,i,j-1), y_index (N_+1,i,j+1), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));

          hessvals(x, values, y_index (i,0,j), y_index (i-1,0,j),  y_index (i+1,0,j), y_index (i,0,j-1), y_index (i,0,j+1), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));
          hessvals(x, values, y_index (i,N_+1,j), y_index (i-1,N_+1,j),  y_index (i+1,N_+1,j), y_index (i,N_+1,j-1), y_index (i,N_+1,j+1), ihes, hh_, obj_factor*alpha_*h_/(hh_*hh_));
        }
      }
    }
  }

  return true;
}

void
MittelmannBndryCntrlDiriBase3Dsin::finalize_solution(SolverReturn status,
    Index n, const Number* x, const Number* z_L, const Number* z_U,
    Index m, const Number* g, const Number* lambda, Number obj_value,
    const IpoptData* ip_data,
    IpoptCalculatedQuantities* ip_cq)
{
  /*
  FILE* fp = fopen("solution.txt", "w+");

  for (Index i=0; i<=N_+1; i++) {
    for (Index j=0; j<=N_+1; j++) {
      fprintf(fp, "y[%6d,%6d] = %15.8e\n", i, j, x[y_index(i,j)]);
    }
  }

  fclose(fp);
  */
}
