feasiblesqpmethod.cpp

/*
 *    This file is part of CasADi.
 *
 *    CasADi -- A symbolic framework for dynamic optimization.
 *    Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl,
 *                            KU Leuven. All rights reserved.
 *    Copyright (C) 2011-2014 Greg Horn
 *    Copyright (C) 2022-2023 David Kiessling
 *
 *    CasADi is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 3 of the License, or (at your option) any later version.
 *
 *    CasADi 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
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with CasADi; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */
 
#include "feasiblesqpmethod.hpp"
 
#include "casadi/core/casadi_misc.hpp"
#include "casadi/core/calculus.hpp"
#include "casadi/core/conic.hpp"
#include "casadi/core/conic_impl.hpp"
#include "casadi/core/convexify.hpp"
 
#include <ctime>
#include <iomanip>
#include <fstream>
#include <cmath>
#include <cfloat>
#include <signal.h>
 
namespace casadi {
 
 
  extern "C"
  int CASADI_NLPSOL_FEASIBLESQPMETHOD_EXPORT
      casadi_register_nlpsol_feasiblesqpmethod(Nlpsol::Plugin* plugin) {
    plugin->creator = Feasiblesqpmethod::creator;
    plugin->name = "feasiblesqpmethod";
    plugin->doc = Feasiblesqpmethod::meta_doc.c_str();
    plugin->version = CASADI_VERSION;
    plugin->options = &Feasiblesqpmethod::options_;
    plugin->deserialize = &Feasiblesqpmethod::deserialize;
    return 0;
  }
 
  extern "C"
  void CASADI_NLPSOL_FEASIBLESQPMETHOD_EXPORT casadi_load_nlpsol_feasiblesqpmethod() {
    Nlpsol::registerPlugin(casadi_register_nlpsol_feasiblesqpmethod);
  }
 
  Feasiblesqpmethod::Feasiblesqpmethod(const std::string& name, const Function& nlp)
    : Nlpsol(name, nlp) {
  }
 
  Feasiblesqpmethod::~Feasiblesqpmethod() {
    clear_mem();
  }
 
  const Options Feasiblesqpmethod::options_
  = {{&Nlpsol::options_},
     {{"solve_type",
       {OT_STRING,
        "The solver type: Either SQP or SLP. Defaults to SQP"}},
      {"qpsol",
       {OT_STRING,
        "The QP solver to be used by the SQP method [qpoases]"}},
      {"qpsol_options",
       {OT_DICT,
        "Options to be passed to the QP solver"}},
      {"hessian_approximation",
       {OT_STRING,
        "limited-memory|exact"}},
      {"max_iter",
       {OT_INT,
        "Maximum number of SQP iterations"}},
      {"min_iter",
       {OT_INT,
        "Minimum number of SQP iterations"}},
      {"tol_pr",
       {OT_DOUBLE,
        "Stopping criterion for primal infeasibility"}},
      {"tol_du",
       {OT_DOUBLE,
        "Stopping criterion for dual infeasability"}},
      {"merit_memory",
       {OT_INT,
        "Size of memory to store history of merit function values"}},
      {"lbfgs_memory",
       {OT_INT,
        "Size of L-BFGS memory."}},
      {"print_header",
       {OT_BOOL,
        "Print the header with problem statistics"}},
      {"print_iteration",
       {OT_BOOL,
        "Print the iterations"}},
      {"print_status",
       {OT_BOOL,
        "Print a status message after solving"}},
      {"f",
       {OT_FUNCTION,
        "Function for calculating the objective function (autogenerated by default)"}},
      {"g",
       {OT_FUNCTION,
        "Function for calculating the constraints (autogenerated by default)"}},
      {"grad_f",
       {OT_FUNCTION,
        "Function for calculating the gradient of the objective (autogenerated by default)"}},
      {"jac_g",
       {OT_FUNCTION,
        "Function for calculating the Jacobian of the constraints (autogenerated by default)"}},
      {"hess_lag",
       {OT_FUNCTION,
        "Function for calculating the Hessian of the Lagrangian (autogenerated by default)"}},
      {"convexify_strategy",
       {OT_STRING,
        "NONE|regularize|eigen-reflect|eigen-clip. "
        "Strategy to convexify the Lagrange Hessian before passing it to the solver."}},
      {"convexify_margin",
       {OT_DOUBLE,
        "When using a convexification strategy, make sure that "
        "the smallest eigenvalue4 is at least this (default: 1e-7)."}},
      {"max_iter_eig",
       {OT_DOUBLE,
        "Maximum number of iterations to compute an eigenvalue decomposition (default: 50)."}},
      {"init_feasible",
       {OT_BOOL,
        "Initialize the QP subproblems with a feasible initial value (default: false)."}},
      {"optim_tol",
       {OT_DOUBLE,
        "Optimality tolerance. Below this value an iterate is considered to be optimal."}},
      {"feas_tol",
       {OT_DOUBLE,
        "Feasibility tolerance. Below this tolerance an iterate is considered to be feasible."}},
      {"tr_rad0",
       {OT_DOUBLE,
        "Initial trust-region radius."}},
      {"tr_eta1",
       {OT_DOUBLE,
        "Lower eta in trust-region acceptance criterion."}},
      {"tr_eta2",
       {OT_DOUBLE,
        "Upper eta in trust-region acceptance criterion."}},
      {"tr_alpha1",
       {OT_DOUBLE,
        "Lower alpha in trust-region size criterion."}},
      {"tr_alpha2",
       {OT_DOUBLE,
        "Upper alpha in trust-region size criterion."}},
      {"tr_tol",
       {OT_DOUBLE,
        "Trust-region tolerance. "
        "Below this value another scalar is equal to the trust region radius."}},
      {"tr_acceptance",
       {OT_DOUBLE,
        "Is the trust-region ratio above this value, the step is accepted."}},
      {"tr_rad_min",
       {OT_DOUBLE,
        "Minimum trust-region radius."}},
      {"tr_rad_max",
       {OT_DOUBLE,
        "Maximum trust-region radius."}},
      {"tr_scale_vector",
       {OT_DOUBLEVECTOR,
        "Vector that tells where trust-region is applied."}},
      {"contraction_acceptance_value",
       {OT_DOUBLE,
        "If the empirical contraction rate in the feasibility iterations "
        "is above this value in the heuristics the iterations are aborted."}},
      {"watchdog",
       {OT_INT,
        "Number of watchdog iterations in feasibility iterations. "
        "After this amount of iterations, it is checked with the contraction acceptance value, "
        "if iterations are converging."}},
      {"max_inner_iter",
       {OT_DOUBLE,
        "Maximum number of inner iterations."}},
      {"use_anderson",
       {OT_BOOL,
        "Use Anderson Acceleration. (default false)"}},
      {"anderson_memory",
       {OT_INT,
        "Anderson memory. If Anderson is used default is 1, else default is 0."}},
     }
  };
 
  void Feasiblesqpmethod::init(const Dict& opts) {
    // Call the init method of the base class
    Nlpsol::init(opts);
 
    // Default options
    min_iter_ = 0;
    max_iter_ = 50;
    lbfgs_memory_ = 10;
    tol_pr_ = 1e-6;
    tol_du_ = 1e-6;
    std::string hessian_approximation = "exact";
    // min_step_size_ = 1e-10;
    std::string solve_type = "SQP";
    std::string qpsol_plugin = "qpoases";
    Dict qpsol_options;
    print_header_ = true;
    print_iteration_ = true;
    print_status_ = true;
    // so_corr_ = false;
    init_feasible_ = false;
 
    // parameters and options for FP-SQP solver
    optim_tol_ = 1e-8;
    feas_tol_ = 1e-8;
    tr_eta1_ = 0.25;
    tr_eta2_ = 0.75;
    tr_alpha1_ = 0.5;
    tr_alpha2_ = 2.0;
    tr_tol_ = 1e-8;
    tr_acceptance_ = 1e-8;
    tr_rad_min_ = 1e-10; //is this valid??
    tr_rad_max_ = 10.0;
    tr_rad0_ = 1.0;
    tr_scale_vector_ = std::vector<double>(nx_, 1.0);
    contraction_acceptance_value_ = 0.5;
    watchdog_ = 5;
    max_inner_iter_ = 50;
    use_anderson_ = false;
    sz_anderson_memory_ = 0;
 
 
    std::string convexify_strategy = "none";
    double convexify_margin = 1e-7;
    casadi_int max_iter_eig = 200;
 
    // Read user options
    for (auto&& op : opts) {
      if (op.first=="max_iter") {
        max_iter_ = op.second;
      } else if (op.first=="min_iter") {
        min_iter_ = op.second;
 
      } else if (op.first=="use_anderson") {
        use_anderson_ = op.second;
      } else if (op.first=="anderson_memory") {
        sz_anderson_memory_ = op.second;
 
      } else if (op.first=="lbfgs_memory") {
        lbfgs_memory_ = op.second;
      } else if (op.first=="tol_pr") {
        tol_pr_ = op.second;
      } else if (op.first=="tol_du") {
        tol_du_ = op.second;
      } else if (op.first=="hessian_approximation") {
        hessian_approximation = op.second.to_string();
      } else if (op.first=="solve_type") {
        solve_type = op.second.to_string();
      } else if (op.first=="qpsol") {
        qpsol_plugin = op.second.to_string();
      } else if (op.first=="qpsol_options") {
        qpsol_options = op.second;
      } else if (op.first=="print_header") {
        print_header_ = op.second;
      } else if (op.first=="print_iteration") {
        print_iteration_ = op.second;
      } else if (op.first=="print_status") {
        print_status_ = op.second;
      } else if (op.first=="hess_lag") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==4);
        casadi_assert_dev(f.n_out()==1);
        set_function(f, "nlp_hess_l");
      } else if (op.first=="jac_g") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==2);
        casadi_assert_dev(f.n_out()==1);
        set_function(f, "nlp_jac_g");
      } else if (op.first=="grad_f") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==2);
        casadi_assert_dev(f.n_out()==1);
        set_function(f, "nlp_grad_f");
      } else if (op.first=="f") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==2);
        casadi_assert_dev(f.n_out()==1);
        set_function(f, "nlp_f");
      } else if (op.first=="g") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==2);
        casadi_assert_dev(f.n_out()==1);
        set_function(f, "nlp_g");
      /*
       else if (op.first=="nlp_jac_fg") {
        Function f = op.second;
        casadi_assert_dev(f.n_in()==2);
        casadi_assert_dev(f.n_out()==4);
        set_function(f, "nlp_jac_fg");
      }*/
      } else if (op.first=="convexify_strategy") {
        convexify_strategy = op.second.to_string();
      } else if (op.first=="convexify_margin") {
        convexify_margin = op.second;
      } else if (op.first=="max_iter_eig") {
        max_iter_eig = op.second;
      } else if (op.first=="init_feasible") {
        init_feasible_ = op.second;
 
      // from here FP-SQP
      } else if (op.first == "optim_tol") {
        optim_tol_ = op.second;
      } else if (op.first == "feas_tol") {
        feas_tol_ = op.second;
      } else if (op.first == "tr_rad0") {
        tr_rad0_ = op.second;
      } else if (op.first == "tr_eta1") {
        tr_eta1_ = op.second;
      } else if (op.first == "tr_eta2") {
        tr_eta2_ = op.second;
      } else if (op.first == "tr_alpha1") {
        tr_alpha1_ = op.second;
      } else if (op.first == "tr_alpha2") {
        tr_alpha2_ = op.second;
      } else if (op.first == "tr_tol") {
        tr_tol_ = op.second;
      } else if (op.first == "tr_acceptance") {
        tr_acceptance_ = op.second;
      } else if (op.first == "tr_rad_min") {
        tr_rad_min_ = op.second;
      } else if (op.first == "tr_rad_max") {
        tr_rad_max_ = op.second;
      } else if (op.first == "tr_scale_vector") {
        tr_scale_vector_ = op.second;
      } else if (op.first == "contraction_acceptance_value") {
        contraction_acceptance_value_ = op.second;
      } else if (op.first == "watchdog") {
        watchdog_ = op.second;
      } else if (op.first == "max_inner_iter") {
        max_inner_iter_ = op.second;
      }
    }
 
    // Use exact Hessian?
    exact_hessian_ = hessian_approximation =="exact";
    uout() << "print solve type" << solve_type << std::endl;
    use_sqp_ = solve_type=="SQP";
 
    convexify_ = false;
 
    // Get/generate required functions
    //if (max_iter_ls_ || so_corr_) create_function("nlp_fg", {"x", "p"}, {"f", "g"});
    // First order derivative information
 
    if (!has_function("nlp_f")) {
      create_function("nlp_f", {"x", "p"},
                     {"f"});
    }
    if (!has_function("nlp_g")) {
      create_function("nlp_g", {"x", "p"},
                     {"g"});
    }
    if (!has_function("nlp_jac_g")) {
      create_function("nlp_jac_g", {"x", "p"},
                     {"jac:g:x"});
    }
    if (!has_function("nlp_grad_f")) {
      create_function("nlp_grad_f", {"x", "p"},
                     {"grad:f:x"});
    }
    Asp_ = get_function("nlp_jac_g").sparsity_out(0);
 
    /*
    if (!has_function("nlp_jac_fg")) {
      create_function("nlp_jac_fg", {"x", "p"},
                     {"f", "grad:f:x", "g", "jac:g:x"});
    }
    Asp_ = get_function("nlp_jac_fg").sparsity_out(3);*/
    if (use_sqp_) {
      if (exact_hessian_) {
        if (!has_function("nlp_hess_l")) {
          create_function("nlp_hess_l", {"x", "p", "lam:f", "lam:g"},
                        {"hess:gamma:x:x"}, {{"gamma", {"f", "g"}}});
        }
        Hsp_ = get_function("nlp_hess_l").sparsity_out(0);
        uout() << "Sparsity pattern: " << Hsp_ << std::endl;
        casadi_assert(Hsp_.is_symmetric(), "Hessian must be symmetric");
        if (convexify_strategy!="none") {
          convexify_ = true;
          Dict opts;
          opts["strategy"] = convexify_strategy;
          opts["margin"] = convexify_margin;
          opts["max_iter_eig"] = max_iter_eig;
          opts["verbose"] = verbose_;
          Hsp_ = Convexify::setup(convexify_data_, Hsp_, opts);
        }
      } else {
        Hsp_ = Sparsity::dense(nx_, nx_);
      }
    }
 
    casadi_assert(!qpsol_plugin.empty(), "'qpsol' option has not been set");
    // qpsol_options["dump_in"] = true;
    // qpsol_options["dump_out"] = true;
    // qpsol_options["dump"] = true;
    // qpsol_options["print_out"] = true;
    // qpsol_options["error_on_fail"] = false;
 
    // Hsp_.to_file("h.mtx");
    // Asp_.to_file("a.mtx");
    // uout() << qpsol_options << std::endl;
    if (use_sqp_) {
      qpsol_ = conic("qpsol", qpsol_plugin, {{"h", Hsp_}, {"a", Asp_}},
                   qpsol_options);
      // cout << qpsol_ <<std::endl;
    } else {
      Hsp_ = Sparsity(nx_, nx_);
      uout() << "Sparsity pattern: " << Hsp_ << std::endl;
      uout() << "Sparsity pattern: " << Asp_ << std::endl;
      // uout() << "Nonzeros: " << Hsp_.nnz() << std::endl;
      // qpsol_ = conic("qpsol", qpsol_plugin, {{"h", Hsp_}, {"a", Asp_}},
      //              qpsol_options);
      qpsol_ = conic("qpsol", qpsol_plugin, {{"a", Asp_}},
                   qpsol_options);
      // qpsol_ = Function::load("/home/david/testproblems_feasible_casadi/qpsol.casadi");
      // cout << qpsol_ <<std::endl;
    }
 
    alloc(qpsol_);
 
    // BFGS?
    if (!exact_hessian_) {
      alloc_w(2*nx_); // casadi_bfgs
    }
 
    // Header
    if (print_header_) {
      print("-------------------------------------------\n");
      print("This is casadi::Feasiblesqpmethod.\n");
      if (exact_hessian_) {
        print("Using exact Hessian\n");
      } else {
        print("Using limited memory BFGS Hessian approximation\n");
      }
      print("Number of variables:                       %9d\n", nx_);
      print("Number of constraints:                     %9d\n", ng_);
      print("Number of nonzeros in constraint Jacobian: %9d\n", Asp_.nnz());
      print("Number of nonzeros in Lagrangian Hessian:  %9d\n", Hsp_.nnz());
      print("\n");
    }
 
 
    set_feasiblesqpmethod_prob();
    // Allocate memory
    casadi_int sz_w, sz_iw;
    casadi_feasiblesqpmethod_work(&p_, &sz_iw, &sz_w, sz_anderson_memory_);
    alloc_iw(sz_iw, true);
    alloc_w(sz_w, true);
    if (convexify_) {
      alloc_iw(convexify_data_.sz_iw);
      alloc_w(convexify_data_.sz_w);
    }
  }
 
  void Feasiblesqpmethod::set_feasiblesqpmethod_prob() {
 
    p_.sp_h = Hsp_;
 
    p_.sp_a = Asp_;
    // p_.merit_memsize = merit_memsize_;
    // p_.max_iter_ls = max_iter_ls_;
    p_.nlp = &p_nlp_;
  }
 
  void Feasiblesqpmethod::set_work(void* mem, const double**& arg, double**& res,
                                casadi_int*& iw, double*& w) const {
    auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
 
    // Set work in base classes
    Nlpsol::set_work(mem, arg, res, iw, w);
 
    m->d.prob = &p_;
    casadi_feasiblesqpmethod_init(&m->d, &iw, &w, sz_anderson_memory_);
 
    m->iter_count = -1;
  }
 
  int Feasiblesqpmethod::init_mem(void* mem) const {
    if (Nlpsol::init_mem(mem)) return 1;
    auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
 
    if (convexify_) m->add_stat("convexify");
    m->add_stat("BFGS");
    m->add_stat("QP");
    return 0;
  }
 
double Feasiblesqpmethod::eval_m_k(void* mem) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d = &m->d;
  if (use_sqp_) {
    return 0.5*casadi_bilin(d->Bk, Hsp_, d->dx, d->dx) + casadi_dot(nx_, d->gf, d->dx);
  } else {
    return casadi_dot(nx_, d->gf, d->dx);
  }
}
 
double Feasiblesqpmethod::eval_tr_ratio(double val_f, double val_f_corr, double val_m_k) const {
  return (val_f - val_f_corr) / (-val_m_k);
}
 
void Feasiblesqpmethod::tr_update(void* mem, double& tr_rad, double tr_ratio) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d = &m->d;
 
  if (tr_ratio < tr_eta1_) {
    tr_rad = tr_alpha1_ * casadi_masked_norm_inf(nx_, d->dx, d->tr_mask);
  } else if (tr_ratio > tr_eta2_ &&
             abs(casadi_masked_norm_inf(nx_, d->dx, d->tr_mask) - tr_rad) < optim_tol_) {
    tr_rad = fmin(tr_alpha2_*tr_rad, tr_rad_max_);
  }
  // else: keep trust-region as it is....
}
 
int Feasiblesqpmethod::step_update(void* mem, double tr_ratio) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d_nlp = &m->d_nlp;
  auto d = &m->d;
 
  if (tr_ratio > tr_acceptance_) {
    // This is not properly implemented yet: d_nlp->z_old = d_mlp->z;
    casadi_copy(d->z_feas, nx_ + ng_, d_nlp->z);
    d_nlp->objective = d->f_feas;
    casadi_copy(d->dlam_feas, nx_ + ng_, d_nlp->lam);
 
    uout() << "ACCEPTED" << std::endl;
    return 0;
  } else {
    uout() << "REJECTED" << std::endl;
    return -1;
  }
}
 
 
/*
Do the Anderson step update here and also update the memory here
*/
void Feasiblesqpmethod::anderson_acc_step_update(void* mem, casadi_int iter_index) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d = &m->d;
 
  if (sz_anderson_memory_ == 1) {
    // Calculte gamma
    casadi_copy(d->dx_feas, nx_, d->z_tmp);
    casadi_axpy(nx_, -1.0, d->anderson_memory_step, d->z_tmp);
    *d->gamma = casadi_dot(nx_, d->dx_feas, d->z_tmp) / casadi_dot(nx_, d->z_tmp, d->z_tmp);
    // DM(gamma).to_file("gamma.mtx");
 
 
    // Prepare the step update
    casadi_copy(d->z_feas, nx_, d->z_tmp);
    casadi_axpy(nx_, -1.0, d->anderson_memory_iterate, d->z_tmp);
    casadi_axpy(nx_, 1.0, d->dx_feas, d->z_tmp);
    casadi_axpy(nx_, -1.0, d->anderson_memory_step, d->z_tmp);
 
    // Update the Anderson memory
    anderson_acc_update_memory(mem, d->dx_feas, d->z_feas);
    // casadi_copy(d->dx_feas, nx_, d->anderson_memory_step);
    // casadi_copy(d->z_feas, nx_, d->anderson_memory_iterate);
 
    // Do the step update
    double beta = 1.0;
    casadi_axpy(nx_, beta, d->dx_feas, d->z_feas);
    casadi_axpy(nx_, -*d->gamma, d->z_tmp, d->z_feas);
    // DM(std::vector<double>(d->z_feas,d->z_feas+nx_)).to_file("dx_anderson2.mtx");
 
  } else {
    print("This is not implemented yet!!!");
    casadi_int curr_stage = fmin(iter_index+1, sz_anderson_memory_);
    // Create matrix F_k
    casadi_copy(d->dx_feas, nx_, d->z_tmp);
    casadi_copy(d->anderson_memory_step, (curr_stage-1)*nx_, d->z_tmp+nx_);
    casadi_axpy(curr_stage*nx_, -1.0, d->anderson_memory_step+nx_, d->z_tmp);
 
    // Solve the least-squares problem
    casadi_dense_lsqr_solve(d->z_tmp, d->dx_feas, 1, 1, curr_stage, nx_, d->gamma);
 
    // Update the Anderson memory
    anderson_acc_update_memory(mem, d->dx_feas, d->z_feas);
 
    double beta = 1.0;
    // Calculate E_k + beta*F_k
    casadi_axpy(nx_, 1.0, d->z_feas, d->z_tmp);
    casadi_axpy((curr_stage-1)*nx_, 1.0, d->anderson_memory_iterate, d->z_tmp+nx_);
    casadi_axpy(curr_stage*nx_, -1.0, d->anderson_memory_iterate, d->z_tmp);
    // Do the final update
    casadi_axpy(nx_, beta, d->dx_feas, d->z_feas);
    casadi_axpy(nx_, -*d->gamma, d->z_tmp, d->z_feas);
 
  }
}
 
/*
Initialize the memory of the Anderson acceleration
*/
void Feasiblesqpmethod::anderson_acc_init_memory(void* mem, double* step, double* iterate) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d = &m->d;
 
  casadi_clear(d->anderson_memory_step, sz_anderson_memory_*nx_);
  casadi_clear(d->anderson_memory_iterate, sz_anderson_memory_*nx_);
 
  // if (sz_anderson_memory_ == 1) {
  //   casadi_copy(step, nx_, d->anderson_memory_step);
  //   casadi_copy(x, nx_, d->anderson_memory_iterate);
  // } else {
  //   print("This is not implemented yet!!!");
  // }
 
  casadi_copy(step, nx_, d->anderson_memory_step);
  casadi_copy(iterate, nx_, d->anderson_memory_iterate);
 
}
 
/*
Update the memory of the Anderson acceleration
*/
void Feasiblesqpmethod::anderson_acc_update_memory(void* mem, double* step, double* iterate) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d = &m->d;
 
  if (sz_anderson_memory_ == 1) {
    casadi_copy(step, nx_, d->anderson_memory_step);
    casadi_copy(iterate, nx_, d->anderson_memory_iterate);
  } else {
    // Shift old values further
    casadi_copy(d->anderson_memory_step,
      (sz_anderson_memory_-1)*nx_, d->anderson_memory_step + nx_);
    casadi_copy(d->anderson_memory_iterate,
      (sz_anderson_memory_-1)*nx_, d->anderson_memory_iterate + nx_);
    // Insert new values
    casadi_copy(step, nx_, d->anderson_memory_step);
    casadi_copy(iterate, nx_, d->anderson_memory_iterate);
  }
}
 
 
/*
Calculates the feasibility_iterations. If iterations are accepted return 0.
If iterations are aborted return -1.
*/
 
int Feasiblesqpmethod::feasibility_iterations(void* mem, double tr_rad) const {
  auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  auto d_nlp = &m->d_nlp;
  auto d = &m->d;
 
  // p_tmp = p
  casadi_copy(d->dx, nx_, d->dx_feas);
 
//   lam_p_g_tmp = self.lam_p_g_k
//   lam_p_x_tmp = self.lam_p_x_k
  casadi_copy(d->dlam, nx_ + ng_, d->dlam_feas);
 
  // Why do we do this at the moment??
  casadi_copy(d->dlam, nx_+ng_, d->z_tmp);
  casadi_axpy(nx_+ng_, -1.0, d_nlp->lam, d->z_tmp);
 
  // this is in solve in fslp.py
  double step_inf_norm = casadi_masked_norm_inf(nx_, d->dx, d->tr_mask);
  double prev_step_inf_norm = step_inf_norm;
 
  // bool kappa_acceptance = false;
 
  // self.x_tmp = self.x_k + p_tmp
  casadi_copy(d_nlp->z, nx_+ng_, d->z_feas);
  casadi_axpy(nx_, 1., d->dx_feas, d->z_feas);
 
  if (use_anderson_) {
    // anderson_acc_init_memory(mem, d->dx_feas, d->z_feas);
    anderson_acc_init_memory(mem, d->dx_feas, d_nlp->z);
  }
 
  // DM(std::vector<double>(d->z_feas,d->z_feas+nx_)).to_file("dx_anderson1.mtx");
  // Evaluate g
  //   self.g_tmp = self.__eval_g(self.x_tmp)
  m->arg[0] = d->z_feas;
  m->arg[1] = d_nlp->p;
  m->res[0] = d->z_feas + nx_;
  if (calc_function(m, "nlp_g")) {
    uout() << "What does it mean that calc_function fails here??" << std::endl;
  }
  int inner_iter = 0;
 
//   asymptotic_exactness = []
  // double asymptotic_exactness = 0.0;
//   self.prev_infeas = self.feasibility_measure(self.x_tmp, self.g_tmp)
//   self.curr_infeas = self.feasibility_measure(self.x_tmp, self.g_tmp)
 
  double prev_infeas = casadi_max_viol(nx_+ng_, d->z_feas, d_nlp->lbz, d_nlp->ubz);
  double curr_infeas = prev_infeas;
//   feasibilities = [self.prev_infeas]
//   step_norms = []
//   kappas = []
 
//   watchdog_prev_inf_norm = self.prev_step_inf_norm
//   accumulated_as_ex = 0
 
  // Calculate asymptotic exactness of current step
  casadi_copy(d->dx, nx_, d->z_tmp);
  casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
  casadi_axpy(nx_, 1., d_nlp->z, d->z_tmp);
  double as_exac = casadi_norm_2(nx_, d->z_tmp) / casadi_norm_2(nx_, d->dx);
 
  double kappa_watchdog = 0.0;
  double kappa = 0.0;
  double acc_as_exac = 0.0;
 
 
  double watchdog_prev_inf_norm = prev_step_inf_norm; // until here everything is correct!
 
  for (int j=0; j<max_inner_iter_; ++j) {
    if (curr_infeas < feas_tol_) {
      inner_iter = j;
      // kappa_acceptance = true;
      if (as_exac < 0.5) {
        return 0;
      } else {
        return -1;
      }
    } else if (j>0 && (curr_infeas > 1.0 || as_exac > 1.0)) {
      // kappa_acceptance = false;
      return -1;
    }
    inner_iter = j+1;
 
    //       self.lam_tmp_g = self.lam_p_g_k
    //       self.lam_tmp_x = self.lam_p_x_k
 
    // create corrected gradient here -----------------------------
    casadi_copy(d->z_feas, nx_, d->z_tmp);
    casadi_axpy(nx_, -1., d_nlp->z, d->z_tmp);
    casadi_copy(d->gf, nx_, d->gf_feas);
    // In case of SQP we need to multiply with
    if (use_sqp_) {
      casadi_mv(d->Bk, Hsp_, d->z_tmp, d->gf_feas, true);
    }
 
    // create bounds of correction QP -----------------------------
    // upper bounds of constraints
    casadi_copy(d_nlp->ubz + nx_, ng_, d->ubdz_feas + nx_);
    casadi_axpy(ng_, -1., d->z_feas + nx_, d->ubdz_feas + nx_);
 
    // lower bounds of constraints
    casadi_copy(d_nlp->lbz + nx_, ng_, d->lbdz_feas + nx_);
    casadi_axpy(ng_, -1., d->z_feas + nx_, d->lbdz_feas + nx_);
 
    // lower bounds of variables
    // lbp = cs.fmax(-self.tr_rad_k*self.tr_scale_mat_inv_k @
    //                       cs.DM.ones(self.nx, 1) - (self.x_tmp-self.x_k),
    //                       self.lbx - self.x_tmp)
 
    casadi_copy(d_nlp->lbz, nx_, d->lbdz_feas);
    casadi_clip_min(d->lbdz_feas, nx_, -tr_rad, d->tr_mask);
    // DM(std::vector<double>(d->lbdz_feas,d->lbdz_feas+nx_)).to_file("lbx_feas_part1_part1.mtx");
    // casadi_fill(d->lbdz_feas, nx_, -tr_rad);
    casadi_axpy(nx_, -1., d->z_feas, d->lbdz_feas);
    casadi_axpy(nx_, 1., d_nlp->z, d->lbdz_feas);
    // DM(std::vector<double>(d->lbdz_feas,d->lbdz_feas+nx_)).to_file("lbx_feas_part1.mtx");
 
 
    casadi_copy(d_nlp->lbz, nx_, d->z_tmp);
    casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    // DM(std::vector<double>(d->z_tmp,d->z_tmp+nx_)).to_file("lbx_feas_part2.mtx");
 
    // comparison of both vectors
    casadi_vector_fmax(nx_, d->z_tmp, d->lbdz_feas, d->lbdz_feas);
    // DM(std::vector<double>(d->lbdz_feas,d->lbdz_feas+nx_)).to_file("lbx_feas_end.mtx");
 
 
    // upper bounds of variables
    //       ubp = cs.fmin(self.tr_rad_k*self.tr_scale_mat_inv_k @
    //                     cs.DM.ones(self.nx, 1) - (self.x_tmp-self.x_k),
    //                     self.ubx - self.x_tmp)
 
 
    casadi_copy(d_nlp->ubz, nx_, d->ubdz_feas);
    casadi_clip_max(d->ubdz_feas, nx_, tr_rad, d->tr_mask);
    // DM(std::vector<double>(d->ubdz_feas,d->ubdz_feas+nx_)).to_file("ubx_feas_part1_part1.mtx");
    // casadi_fill(d->ubdz_feas, nx_, tr_rad);
    casadi_axpy(nx_, -1., d->z_feas, d->ubdz_feas);
    // DM(std::vector<double>(d->z_feas,d->z_feas+nx_)).to_file("ubx_feas_part1_zfeas.mtx");
    casadi_axpy(nx_, 1., d_nlp->z, d->ubdz_feas);
 
    // DM(std::vector<double>(d->ubdz_feas,d->ubdz_feas+nx_)).to_file("ubx_feas_part1.mtx");
 
    casadi_copy(d_nlp->ubz, nx_, d->z_tmp);
    casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    // DM(std::vector<double>(d->z_tmp,d->z_tmp+nx_)).to_file("ubx_feas_part2.mtx");
    // comparison of both vectors
    casadi_vector_fmin(nx_, d->z_tmp, d->ubdz_feas, d->ubdz_feas);
    // DM(std::vector<double>(d->ubdz_feas,d->ubdz_feas+nx_)).to_file("ubx_feas_end.mtx");
 
 
    // std::string suffix = str(j);
    // DM(std::vector<double>(d_nlp->lbz,d_nlp->lbz+nx_+ng_)).to_file("nlp_lbz"+suffix+".mtx");
    // DM(std::vector<double>(d_nlp->ubz,d_nlp->ubz+nx_+ng_)).to_file("nlp_ubz"+suffix+".mtx");
    // DM(std::vector<double>(d->lbdz_feas,d->lbdz_feas+nx_+ng_)).to_file("lbz"+suffix+".mtx");
    // DM(std::vector<double>(d->ubdz_feas,d->ubdz_feas+nx_+ng_)).to_file("ubz"+suffix+".mtx");
    // DM(std::vector<double>(d->z_feas,d->z_feas+nx_+ng_)).to_file("z_feas"+suffix+".mtx");
    // DM(std::vector<double>(d->dx_feas,d->dx_feas+nx_)).to_file("dx_feas"+suffix+".mtx");
    // copy back d->dx back to d->dx_feas
    // casadi_copy(d->dx, nx_, d->x_tmp);
 
    //prepare step_inf_norm
    // DM(std::vector<double>(d->dx_feas,d->dx_feas+nx_)).to_file("dx_input.mtx");
    // DM(std::vector<double>(d->gf_feas,d->gf_feas+nx_)).to_file("gf_feas.mtx");
    // DM(std::vector<double>(d->lbdz_feas, d->lbdz_feas+nx_)).to_file("lb_var_correction.mtx");
    // DM(std::vector<double>(d->ubdz_feas, d->ubdz_feas+nx_)).to_file("ub_var_correction.mtx");
    // DM(std::vector<double>(d->lbdz_feas+nx_,
    //    d->lbdz_feas+nx_+ng_)).to_file("lba_correction.mtx");
    // DM(std::vector<double>(d->ubdz_feas+nx_,
    //    d->ubdz_feas+nx_+ng_)).to_file("uba_correction.mtx");
    // DM(std::vector<double>(d->dlam_feas, d->dlam_feas+nx_)).to_file("lam_x.mtx");
    // DM(std::vector<double>(d->dlam_feas+nx_, d->dlam_feas+nx_+ng_)).to_file("lam_g.mtx");
 
    if (use_sqp_) {
      solve_QP(m, d->Bk, d->gf_feas, d->lbdz_feas, d->ubdz_feas,
        d->Jk, d->dx_feas, d->dlam_feas, 0);
    } else {
      solve_LP(m, d->gf_feas, d->lbdz_feas, d->ubdz_feas,
        d->Jk, d->dx_feas, d->dlam_feas, 0);
    }
 
     // put definition of ret out of loop
 
    // DM(std::vector<double>(d->dx_feas,d->dx_feas+nx_)).to_file("dx_feas.mtx");
    // uout() << "Feas QP step: " << std::vector<double>(d->dx_feas, d->dx_feas+nx_) << std::endl;
    //MISSING: Depending on the result terminate program
 
    //MISSING: Calculate the step_inf_norm
    // self.step_inf_norm = cs.fmax(
    //             cs.norm_inf(self.p_k),
    //             cs.fmax(
    //                     cs.norm_inf(self.lam_tmp_g-self.lam_p_g_k),
    //                     cs.norm_inf(self.lam_tmp_x-self.lam_p_x_k)
    //                     )
    //             )
 
    // TODO(david) the python code has a bug and does not consider the step in lambda
    // here!!
    // casadi_copy(d->dlam_feas, nx_+ng_, d->z_tmp);
    // casadi_axpy(nx_+ng_, -1.0, d->dlam, d->z_tmp);
    step_inf_norm = casadi_masked_norm_inf(nx_, d->dx_feas, d->tr_mask);
 
    //       self.x_tmp = self.x_tmp + p_tmp
    //       self.g_tmp = self.__eval_g(self.x_tmp)  # x_tmp = x_{tmp-1} + p_tmp
 
    if (use_anderson_) {
      anderson_acc_step_update(mem, j);
    } else {
      casadi_axpy(nx_, 1., d->dx_feas, d->z_feas);
    }
 
    // Evaluate g
    m->arg[0] = d->z_feas;
    m->arg[1] = d_nlp->p;
    m->res[0] = d->z_feas + nx_;
    if (calc_function(m, "nlp_g")) {
      uout() << "What does it mean that calc_function fails here??" << std::endl;
    }
 
    //       self.curr_infeas = self.feasibility_measure(self.x_tmp, self.g_tmp)
    //       self.prev_infeas = self.curr_infeas
    prev_infeas = casadi_max_viol(nx_+ng_, d->z_feas, d_nlp->lbz, d_nlp->ubz);
    curr_infeas = prev_infeas;
    //       kappa = self.step_inf_norm/self.prev_step_inf_norm
    //       kappas.append(kappa)
    kappa = step_inf_norm/prev_step_inf_norm;
    //MISSING: as_exac:
 
    // DM(std::vector<double>(d_nlp->z, d_nlp->z+nx_)).to_file("x_k.mtx");
    // DM(std::vector<double>(d->z_feas,d->z_feas+nx_)).to_file("x_tmp.mtx");
    // DM(std::vector<double>(d->dx_feas,d->dx_feas+nx_)).to_file("dx_feas"+suffix+".mtx");
    // DM(std::vector<double>(d->dx,d->dx+nx_)).to_file("p_k.mtx");
 
 
    casadi_copy(d->dx, nx_, d->z_tmp);
    casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    casadi_axpy(nx_, 1., d_nlp->z, d->z_tmp);
    as_exac = casadi_norm_2(nx_, d->z_tmp) / casadi_norm_2(nx_, d->dx);
 
 
    //       as_exac = cs.norm_2(
    //           self.p_k - (self.x_tmp - self.x_k)) / cs.norm_2(self.p_k)
 
    //       if self.verbose:
    //           print("Kappa: ", kappa,
    //                 "Infeasibility", self.feasibility_measure(
    //                           self.x_tmp, self.g_tmp),
    //                 "Asymptotic Exactness: ", as_exac)
    print("%6s %9.10f %14s %9.10f %20s %9.10f\n", "Kappa:", kappa,
    "Infeasibility:", curr_infeas, "AsymptoticExactness:", as_exac);
 
    //       accumulated_as_ex += as_exac
    acc_as_exac += as_exac;
    //       if inner_iter % self.watchdog == 0:
    //           kappa_watch = self.step_inf_norm/watchdog_prev_inf_norm
    //           watchdog_prev_inf_norm = self.step_inf_norm
    //           if self.verbose:
    //               print("kappa watchdog: ", kappa_watch)
    //           if self.curr_infeas < self.feas_tol and as_exac < 0.5:
    //               self.kappa_acceptance = True
    //               break
    if (inner_iter % watchdog_ == 0) {
      kappa_watchdog = step_inf_norm / watchdog_prev_inf_norm;
      watchdog_prev_inf_norm = step_inf_norm;
      print("Kappa watchdog: %9.10f\n", kappa_watchdog);
      if (curr_infeas < feas_tol_ && as_exac < 0.5) {
        // kappa_acceptance = true;
        return 0;
      }
    //           if kappa_watch > self.contraction_acceptance or
    //                   accumulated_as_ex/self.watchdog > 0.5:
    //               self.kappa_acceptance = False
    //               break
      if (kappa_watchdog > contraction_acceptance_value_ || acc_as_exac/watchdog_ > 0.5) {
        // kappa_acceptance = false;
        return -1;
      }
    //           accumulated_as_ex = 0
      acc_as_exac = 0.0;
    }
 
    // Do some saving here??
    //       feasibilities.append(
    //           self.feasibility_measure(self.x_tmp, self.g_tmp))
    //       asymptotic_exactness.append(as_exac)
    //       step_norms.append(cs.norm_inf(p_tmp))
 
    //       self.prev_step_inf_norm = self.step_inf_norm
    //       self.lam_tmp_g = self.lam_p_g_k
    //       self.lam_tmp_x = self.lam_p_x_k
    prev_step_inf_norm = step_inf_norm;
  }
  //maximum iterations reached
  // kappa_acceptance = false;
  return -1;
}
 
int Feasiblesqpmethod::solve(void* mem) const {
    auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
    auto d_nlp = &m->d_nlp;
    auto d = &m->d;
 
    // DM(std::vector<double>(d_nlp->z, d_nlp->z+nx_)).to_file("x0.mtx");
    // Number of SQP iterations
    m->iter_count = 0;
 
    // Reset
    // m->merit_ind = 0;
    // m->sigma = 0.;    // NOTE: Move this into the main optimization loop
    // m->reg = 0;
    int step_accepted = 0;
 
    // Default quadratic model value of objective
    double m_k = -1.0;
 
    double tr_ratio = 0.0;
 
    double tr_rad = tr_rad0_;
    double tr_rad_prev = tr_rad0_;
 
    // transfer the scale vector to the problem
    casadi_copy(get_ptr(tr_scale_vector_), nx_, d->tr_scale_vector);
 
    for (casadi_int i=0;i<nx_;++i) {
      d->tr_mask[i] = d->tr_scale_vector[i]!=0;
    }
 
    // For seeds ---- This is so far needed!!! ------------------
    const double one = 1.;
 
    // Info for printing
    std::string info = "";
 
    casadi_clear(d->dx, nx_);
 
    // ------------------------------------------------------------------------
    // MAIN OPTIMIZATION LOOP
    // ------------------------------------------------------------------------
    while (true) {
      // Evaluate f, g and first order derivative information
      /*m->arg[0] = d_nlp->z;
      m->arg[1] = d_nlp->p;
      m->res[0] = &d_nlp->objective;
      m->res[1] = d->gf;
      m->res[2] = d_nlp->z + nx_;
      m->res[3] = d->Jk;
      switch (calc_function(m, "nlp_jac_fg")) {
        case -1:
          m->return_status = "Non_Regular_Sensitivities";
          m->unified_return_status = SOLVER_RET_NAN;
          if (print_status_)
            print("MESSAGE(feasiblesqpmethod): No regularity of sensitivities at current point.\n");
          return 1;
        case 0:
          break;
        default:
          return 1;
      }*/
      if (m->iter_count == 0) {
        // Evaluate the sensitivities -------------------------------------------
        // Evaluate f
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = &d_nlp->objective;
        if (calc_function(m, "nlp_f")) {
          uout() << "What does it mean that calc_function fails here??" << std::endl;
        }
        // Evaluate g
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = d_nlp->z + nx_;
        if (calc_function(m, "nlp_g")) {
          uout() << "What does it mean that calc_function fails here??" << std::endl;
        }
        // Evaluate grad_f
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = d->gf;
        if (calc_function(m, "nlp_grad_f")) {
          uout() << "What does it mean that calc_function fails here??" << std::endl;
        }
        // Evaluate jac_g
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = d->Jk;
        switch (calc_function(m, "nlp_jac_g")) {
          case -1:
            m->return_status = "Non_Regular_Sensitivities";
            m->unified_return_status = SOLVER_RET_NAN;
            if (print_status_)
              print("MESSAGE(feasiblesqpmethod): "
                    "No regularity of sensitivities at current point.\n");
            return 1;
          case 0:
            break;
          default:
            return 1;
 
        }
        // uout() << "x0: " << *d_nlp->z << std::endl;
        // uout() << "nlp_f: " << d_nlp->objective << std::endl;
        // uout() << "nlp_g: " << *(d_nlp->z + nx_) << std::endl;
 
        if (use_sqp_) {
          if (exact_hessian_) {
            // Update/reset exact Hessian
            m->arg[0] = d_nlp->z;
            m->arg[1] = d_nlp->p;
            m->arg[2] = &one;
            m->arg[3] = d_nlp->lam + nx_;
            m->res[0] = d->Bk;
            if (calc_function(m, "nlp_hess_l")) return 1;
            if (convexify_) {
              ScopedTiming tic(m->fstats.at("convexify"));
              if (convexify_eval(&convexify_data_.config, d->Bk, d->Bk, m->iw, m->w)) return 1;
            }
          } else if (m->iter_count==0) {
            ScopedTiming tic(m->fstats.at("BFGS"));
            // Initialize BFGS
            casadi_fill(d->Bk, Hsp_.nnz(), 1.);
            casadi_bfgs_reset(Hsp_, d->Bk);
          } else {
            ScopedTiming tic(m->fstats.at("BFGS"));
            // Update BFGS
            if (m->iter_count % lbfgs_memory_ == 0) casadi_bfgs_reset(Hsp_, d->Bk);
            // Update the Hessian approximation
            casadi_bfgs(Hsp_, d->Bk, d->dx, d->gLag, d->gLag_old, m->w);
          }
 
          // test if initialization is feasible
          if (casadi_max_viol(nx_ + ng_, d_nlp->z, d_nlp->lbz, d_nlp->ubz) > feas_tol_) {
            if (print_status_)print("MESSAGE(feasiblesqpmethod): "
                "No feasible initialization given! "
                "Find feasible initialization.\n");
            m->return_status = "No_Feasible_Initialization";
            break;
          }
        }
 
      } else if (step_accepted == 0) {
        // Evaluate grad_f
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = d->gf;
        if (calc_function(m, "nlp_grad_f")) {
          uout() << "What does it mean that calc_function fails here??" << std::endl;
        }
        // Evaluate jac_g
        m->arg[0] = d_nlp->z;
        m->arg[1] = d_nlp->p;
        m->res[0] = d->Jk;
        switch (calc_function(m, "nlp_jac_g")) {
          case -1:
            m->return_status = "Non_Regular_Sensitivities";
            m->unified_return_status = SOLVER_RET_NAN;
            if (print_status_)
              print("MESSAGE(feasiblesqpmethod): "
                    "No regularity of sensitivities at current point.\n");
            return 1;
          case 0:
            break;
          default:
            return 1;
        }
        // uout() << "x0: " << *d_nlp->z << std::endl;
        // uout() << "nlp_f: " << d_nlp->objective << std::endl;
        // uout() << "nlp_g: " << *(d_nlp->z + nx_) << std::endl;
 
        if (use_sqp_) {
          if (exact_hessian_) {
            // Update/reset exact Hessian
            m->arg[0] = d_nlp->z;
            m->arg[1] = d_nlp->p;
            m->arg[2] = &one;
            m->arg[3] = d_nlp->lam + nx_;
            m->res[0] = d->Bk;
            if (calc_function(m, "nlp_hess_l")) return 1;
            if (convexify_) {
              ScopedTiming tic(m->fstats.at("convexify"));
              if (convexify_eval(&convexify_data_.config, d->Bk, d->Bk, m->iw, m->w)) return 1;
            }
          } else if (m->iter_count==0) {
            ScopedTiming tic(m->fstats.at("BFGS"));
            // Initialize BFGS
            casadi_fill(d->Bk, Hsp_.nnz(), 1.);
            casadi_bfgs_reset(Hsp_, d->Bk);
          } else {
            ScopedTiming tic(m->fstats.at("BFGS"));
            // Update BFGS
            if (m->iter_count % lbfgs_memory_ == 0) casadi_bfgs_reset(Hsp_, d->Bk);
            // Update the Hessian approximation
            casadi_bfgs(Hsp_, d->Bk, d->dx, d->gLag, d->gLag_old, m->w);
          }
        }
      }
 
      // Evaluate the gradient of the Lagrangian
      casadi_copy(d->gf, nx_, d->gLag);
      casadi_mv(d->Jk, Asp_, d_nlp->lam+nx_, d->gLag, true);
      casadi_axpy(nx_, 1., d_nlp->lam, d->gLag);
 
      // Primal infeasability
      double pr_inf = casadi_max_viol(nx_+ng_, d_nlp->z, d_nlp->lbz, d_nlp->ubz);
      // uout() << "pr_inf: " << pr_inf << std::endl;
      // inf-norm of Lagrange gradient
      // uout() << "grad Lag: " << std::vector<double>(*d->gLag,0,nx_) << std::endl;
      double du_inf = casadi_norm_inf(nx_, d->gLag);
      // uout() << "du_inf: " << du_inf << std::endl;
 
      // inf-norm of step, d->dx is a nullptr???
      // uout() << "HERE!!!!" << *d->dx << std::endl;
      double dx_norminf = casadi_norm_inf(nx_, d->dx);
 
      // uout() << "objective value: " << d_nlp->objective << std::endl;
      // Printing information about the actual iterate
      if (print_iteration_) {
        // if (m->iter_count % 10 == 0) print_iteration();
        print_iteration();
        print_iteration(m->iter_count, d_nlp->objective, m_k, tr_ratio,
                        pr_inf, du_inf, dx_norminf, m->reg, tr_rad_prev, info);
        info = "";
      }
      tr_rad_prev = tr_rad;
 
      // Callback function
      if (callback(m)) {
        if (print_status_) print("WARNING(feasiblesqpmethod): Aborted by callback...\n");
        m->return_status = "User_Requested_Stop";
        break;
      }
 
      // Checking convergence criteria
      // Where is the complementarity condition??
      // if (m->iter_count >= min_iter_ && pr_inf < tol_pr_ && du_inf < tol_du_) {
      //   if (print_status_)
      //     print("MESSAGE(feasiblesqpmethod): "
      //           "Convergence achieved after %d iterations\n", m->iter_count);
      //   m->return_status = "Solve_Succeeded";
      //   m->success = true;
      //   break;
      // }
 
      if (m->iter_count >= max_iter_) {
        if (print_status_) {
          print("MESSAGE(feasiblesqpmethod): Maximum number of iterations reached.\n");
        }
        m->return_status = "Maximum_Iterations_Exceeded";
        m->unified_return_status = SOLVER_RET_LIMITED;
        break;
      }
 
      // Formulate the QP
      // Define lower bounds
      casadi_copy(d_nlp->lbz, nx_+ng_, d->lbdz);
      casadi_axpy(nx_+ng_, -1., d_nlp->z, d->lbdz);
      casadi_clip_min(d->lbdz, nx_, -tr_rad, d->tr_mask);
      // uout() << "lbdz: " << std::vector<double>(d->lbdz, d->lbdz+nx_) << std::endl;
 
 
      // Define upper bounds
      casadi_copy(d_nlp->ubz, nx_+ng_, d->ubdz);
      casadi_axpy(nx_+ng_, -1., d_nlp->z, d->ubdz);
      casadi_clip_max(d->ubdz, nx_, tr_rad, d->tr_mask);
      // uout() << "ubdz: " << std::vector<double>(d->ubdz, d->ubdz+nx_) << std::endl;
 
      // Initial guess
      casadi_copy(d_nlp->lam, nx_+ng_, d->dlam);
      //casadi_clear(d->dx, nx_);
 
      // Increase counter
      m->iter_count++;
 
      // DM(std::vector<double>(d->gf,d->gf+nx_)).to_file("gf.mtx");
      // DM(std::vector<double>(d->lbdz, d->lbdz+nx_)).to_file("lb_var.mtx");
      // DM(std::vector<double>(d->ubdz, d->ubdz+nx_)).to_file("ub_var.mtx");
      // DM(std::vector<double>(d->lbdz+nx_, d->lbdz+nx_+ng_)).to_file("lba.mtx");
      // DM(std::vector<double>(d->ubdz+nx_, d->ubdz+nx_+ng_)).to_file("uba.mtx");
      // // DM(std::vector<double>(d->Bk, d->Bk+Hsp_.nnz())).to_file("Bk.mtx");
      // DM(std::vector<double>(d->Jk, d->Jk+Asp_.nnz())).to_file("Jk.mtx");
      // DM(std::vector<double>(d->dlam, d->dlam+nx_)).to_file("lam_x.mtx");
      // DM(std::vector<double>(d->dx, d->dx+nx_)).to_file("dx_input.mtx");
      // DM(std::vector<double>(d->dlam+nx_, d->dlam+nx_+ng_)).to_file("lam_g.mtx");
 
      int ret = 0;
      // Solve the QP
      if (use_sqp_) {
        ret = solve_QP(m, d->Bk, d->gf, d->lbdz, d->ubdz, d->Jk,
                 d->dx, d->dlam, 0);
      } else {
        ret = solve_LP(m, d->gf, d->lbdz, d->ubdz, d->Jk,
                 d->dx, d->dlam, 0);
      }
 
      // DM(std::vector<double>(d->dx,d->dx+nx_)).to_file("dx_out.mtx");
      // Eval quadratic model and check for convergence
      m_k = eval_m_k(mem);
      if (fabs(m_k) < optim_tol_) {
        if (print_status_)
          print("MESSAGE(feasiblesqpmethod): "
                "Optimal Point Found? Quadratic model is zero. "
                "After %d iterations\n", m->iter_count-1);
        m->return_status = "Solve_Succeeded";
        m->success = true;
        break;
      }
 
      // uout() << "QP step: " << std::vector<double>(d->dx, d->dx+nx_) << std::endl;
      // Detecting indefiniteness
      if (use_sqp_) {
        double gain = casadi_bilin(d->Bk, Hsp_, d->dx, d->dx);
        if (gain < 0) {
          if (print_status_) print("WARNING(feasiblesqpmethod): Indefinite Hessian detected\n");
        }
      }
 
      // Do the feasibility iterations here
      ret = feasibility_iterations(mem, tr_rad);
 
      // Check if step was accepted or not
      if (ret < 0) {
        uout() << "Rejected inner iterates" << std::endl;
        // uout() << casadi_masked_norm_inf(nx_, d->dx, d->tr_mask) << std::endl;
 
        tr_rad = 0.5 * casadi_masked_norm_inf(nx_, d->dx, d->tr_mask);
      } else {
        // Evaluate f
        m->arg[0] = d->z_feas;
        m->arg[1] = d_nlp->p;
        m->res[0] = &d->f_feas;
        if (calc_function(m, "nlp_f")) {
          uout() << "What does it mean that calc_function fails here??" << std::endl;
        }
        tr_ratio = eval_tr_ratio(d_nlp->objective, d->f_feas, m_k);
        tr_update(mem, tr_rad, tr_ratio);
        if (tr_rad < feas_tol_) {
          if (print_status_) print("MESSAGE(feasiblesqpmethod): "
            "Trust-region radius smaller than feasibility!! "
            "Abort!!.\n");
          m->return_status = "Trust_Region_Radius_Becomes_Too_Small";
          break;
        }
 
        step_accepted = step_update(mem, tr_ratio);
      }
 
      if (!exact_hessian_) {
        // Evaluate the gradient of the Lagrangian with the old x but new lam (for BFGS)
        casadi_copy(d->gf, nx_, d->gLag_old);
        casadi_mv(d->Jk, Asp_, d_nlp->lam+nx_, d->gLag_old, true);
        casadi_axpy(nx_, 1., d_nlp->lam, d->gLag_old);
      }
    }
 
    return 0;
  }
 
  void Feasiblesqpmethod::print_iteration() const {
    print("%4s %9s %14s %9s %9s %9s %9s %7s %5s %7s\n",
          "iter", "m_k", "objective", "tr_ratio", "inf_pr",
          "inf_du", "||d||", "lg(rg)", "tr_rad", "info");
  }
 
  void Feasiblesqpmethod::print_iteration(casadi_int iter, double obj,
                                  double m_k, double tr_ratio,
                                  double pr_inf, double du_inf,
                                  double dx_norm, double rg,
                                  double tr_rad,
                                  std::string info) const {
    print("%4d %9.2e %14.6e %9.2e %9.2e %9.2e %9.2e ",
      iter, m_k, obj, tr_ratio, pr_inf, du_inf, dx_norm);
    if (rg>0) {
      print("%7.2f ", log10(rg));
    } else {
      print("%7s ", "-");
    }
 
    print("%9.5e", tr_rad);
    // if (!ls_success) {
    //   print("F");
    // } else {
    //   print (" ");
    // }
 
    print(" - ");
    print(info.c_str());
    print("\n");
  }
 
  int Feasiblesqpmethod::solve_LP(FeasiblesqpmethodMemory* m, const double* g,
                           const double* lbdz, const double* ubdz, const double* A,
                           double* x_opt, double* dlam, int mode) const {
    ScopedTiming tic(m->fstats.at("QP"));
    // Inputs
    std::fill_n(m->arg, qpsol_.n_in(), nullptr);
    // double lol;
    m->arg[CONIC_H] = nullptr;
    m->arg[CONIC_G] = g;
    m->arg[CONIC_X0] = x_opt;
    m->arg[CONIC_LAM_X0] = dlam;
    m->arg[CONIC_LAM_A0] = dlam + nx_;
    m->arg[CONIC_LBX] = lbdz;
    m->arg[CONIC_UBX] = ubdz;
    m->arg[CONIC_A] = A;
    m->arg[CONIC_LBA] = lbdz+nx_;
    m->arg[CONIC_UBA] = ubdz+nx_;
 
    // Outputs
    std::fill_n(m->res, qpsol_.n_out(), nullptr);
    m->res[CONIC_X] = x_opt;
    m->res[CONIC_LAM_X] = dlam;
    m->res[CONIC_LAM_A] = dlam + nx_;
    double obj;
    m->res[CONIC_COST] = &obj;
    // m->res[CONIC_COST] = nullptr;
 
 
    // Solve the QP
    qpsol_(m->arg, m->res, m->iw, m->w);
 
    if (verbose_) print("QP solved\n");
    return 0;
  }
 
  int Feasiblesqpmethod::solve_QP(FeasiblesqpmethodMemory* m, const double* H, const double* g,
                           const double* lbdz, const double* ubdz, const double* A,
                           double* x_opt, double* dlam, int mode) const {
    ScopedTiming tic(m->fstats.at("QP"));
    // Inputs
    std::fill_n(m->arg, qpsol_.n_in(), nullptr);
    m->arg[CONIC_H] = H;
    m->arg[CONIC_G] = g;
    m->arg[CONIC_X0] = x_opt;
    m->arg[CONIC_LAM_X0] = dlam;
    m->arg[CONIC_LAM_A0] = dlam + nx_;
    m->arg[CONIC_LBX] = lbdz;
    m->arg[CONIC_UBX] = ubdz;
    m->arg[CONIC_A] = A;
    m->arg[CONIC_LBA] = lbdz+nx_;
    m->arg[CONIC_UBA] = ubdz+nx_;
 
    // Outputs
    std::fill_n(m->res, qpsol_.n_out(), nullptr);
    m->res[CONIC_X] = x_opt;
    m->res[CONIC_LAM_X] = dlam;
    m->res[CONIC_LAM_A] = dlam + nx_;
    double obj;
    m->res[CONIC_COST] = &obj;
    // m->res[CONIC_COST] = nullptr;
 
 
    // Solve the QP
    qpsol_(m->arg, m->res, m->iw, m->w);
 
    if (verbose_) print("QP solved\n");
    return 0;
  }
 
void Feasiblesqpmethod::codegen_declarations(CodeGenerator& g) const {
    Nlpsol::codegen_declarations(g);
    g.add_dependency(get_function("nlp_grad_f"));
    g.add_dependency(get_function("nlp_jac_g"));
    g.add_dependency(get_function("nlp_g"));
    g.add_dependency(get_function("nlp_f"));
    if (exact_hessian_) g.add_dependency(get_function("nlp_hess_l"));
    g.add_dependency(qpsol_);
  }
 
  void Feasiblesqpmethod::codegen_body(CodeGenerator& g) const {
    g.add_auxiliary(CodeGenerator::AUX_FEASIBLESQPMETHOD);
    codegen_body_enter(g);
    // From nlpsol
    g.local("m_p", "const casadi_real", "*");
    g.init_local("m_p", g.arg(NLPSOL_P));
    g.local("m_f", "casadi_real");
    g.local("m_f_feas", "casadi_real");
    g.copy_default(g.arg(NLPSOL_X0), nx_, "d_nlp.z", "0", false);
    g.copy_default(g.arg(NLPSOL_LAM_X0), nx_, "d_nlp.lam", "0", false);
    g.copy_default(g.arg(NLPSOL_LAM_G0), ng_, "d_nlp.lam+"+str(nx_), "0", false);
    g.copy_default(g.arg(NLPSOL_LBX), nx_, "d_nlp.lbz", "-casadi_inf", false);
    g.copy_default(g.arg(NLPSOL_UBX), nx_, "d_nlp.ubz", "casadi_inf", false);
    g.copy_default(g.arg(NLPSOL_LBG), ng_, "d_nlp.lbz+"+str(nx_),
      "-casadi_inf", false);
    g.copy_default("d_nlp.ubg", ng_, "d_nlp.ubz+"+str(nx_),
      "casadi_inf", false);
    casadi_assert(exact_hessian_, "Codegen implemented for exact Hessian only.", false);
 
    // auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
    // auto d_nlp = &m->d_nlp;
    // auto d = &m->d;
    // Define the problem struct and problem data struct here
    g.local("d", "struct casadi_feasiblesqpmethod_data");
    g.local("p", "struct casadi_feasiblesqpmethod_prob");
 
    g << "d.prob = &p;\n";
    g << "p.sp_h = " << g.sparsity(Hsp_) << ";\n";
    g << "p.sp_a = " << g.sparsity(Asp_) << ";\n";
    g << "p.nlp = &p_nlp;\n";
    g << "casadi_feasiblesqpmethod_init(&d, &iw, &w, " << sz_anderson_memory_ << ");\n";
 
    g.local("m_w", "casadi_real", "*");
    g << "m_w = w;\n";
    g.local("m_iw", "casadi_int", "*");
    g << "m_iw = iw;\n";
    g.local("m_arg", "const casadi_real", "**");
    g.init_local("m_arg", "arg+" + str(NLPSOL_NUM_IN));
    g.local("m_res", "casadi_real", "**");
    g.init_local("m_res", "res+" + str(NLPSOL_NUM_OUT));
 
    // g.local("ret", "int");
    g.local("ret", "casadi_int");
 
    // Number of SQP iterations
    // m->iter_count = 0;
    g.local("iter_count", "casadi_int");
    g.init_local("iter_count", "0");
 
    // Reset
    // int step_accepted = 0;
    g.local("step_accepted", "casadi_int");
    g.init_local("step_accepted", "0");
 
    // Default quadratic model value of objective
    // double m_k = -1.0;
    g.local("m_k", "casadi_real");
    g.init_local("m_k", "-1.0");
 
    // double tr_ratio = 0.0;
    g.local("tr_ratio", "casadi_real");
    g.init_local("tr_ratio", "0.0");
 
    // double tr_rad = tr_rad0_;
    // double tr_rad_prev = tr_rad0_;
    g.local("tr_rad", "casadi_real");
    g << "tr_rad = " << tr_rad0_ << ";\n";
    g.local("tr_rad_prev", "casadi_real");
    g << "tr_rad_prev = " << tr_rad0_ << ";\n";
 
    // transfer the scale vector to the problem
    // casadi_copy(get_ptr(tr_scale_vector_), nx_, d->tr_scale_vector);
    //transfer the scale vector to the problem
    g << g.copy(g.constant(tr_scale_vector_), nx_, "d.tr_scale_vector") << "\n";
    // g << g.copy("casadi_tr_scale_vector_", nx_, "d.tr_scale_vector") << "\n";
 
    // for (casadi_int i=0;i<nx_;++i) {
    //   d->tr_mask[i] = d->tr_scale_vector[i]!=0;
    // }
    g << "for (casadi_int i = 0; i < " << nx_ << "; ++i) {\n";
    g << "d.tr_mask[i] = d.tr_scale_vector != 0;\n";
    g << "}\n";
 
 
    // HERE STARTS THE MAIN OPTIMIZATION LOOP ---------------------------------
    // For seeds ---- This is so far needed!!! ------------------
    // const double one = 1.;
    g.local("one", "const casadi_real");
    g.init_local("one", "1");
 
    // Info for printing
    // string info = "";
 
    // casadi_clear(d->dx, nx_);
    g << g.clear("d.dx", nx_) << "\n";
 
    // ------------------------------------------------------------------------
    // MAIN OPTIMIZATION LOOP
    // ------------------------------------------------------------------------
    // while (true) {
    g.comment("MAIN OPTIMIZATION LOOP");
    g << "while (1) {\n";
    //   if(m->iter_count == 0) {
      g << "if (iter_count == 0) {;\n";
        // Evaluate the sensitivities -------------------------------------------
        // Evaluate f
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = &d_nlp->f;
        // if (calc_function(m, "nlp_f")) {
        //   uout() << "What does it mean that calc_function fails here??" << std::endl;
        // }
        g.comment("Evaluate f");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = &m_f;\n";
        std::string nlp_f = g(get_function("nlp_f"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_f + ") return 1;\n";
        g << "if (" + nlp_f + ") return 10;\n";
 
        // Evaluate g
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = d_nlp->z + nx_;
        // if (calc_function(m, "nlp_g")) {
        //   uout() << "What does it mean that calc_function fails here??" << std::endl;
        // }
        g.comment("Evaluate g");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = d_nlp.z+" + str(nx_) + ";\n";
        std::string nlp_g = g(get_function("nlp_g"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_g + ") return 1;\n";
        g << "if (" + nlp_g + ") return 20;\n";
 
        // Evaluate grad_f
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = d->gf;
        // if (calc_function(m, "nlp_grad_f")) {
        //   uout() << "What does it mean that calc_function fails here??" << std::endl;
        // }
        g.comment("Evaluate grad f");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = d.gf;\n";
        std::string nlp_grad_f = g(get_function("nlp_grad_f"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_grad_f + ") return 1;\n";
        g << "if (" + nlp_grad_f + ") return 30;\n";
 
        // Evaluate jac_g
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = d->Jk;
        // switch (calc_function(m, "nlp_jac_g")) {
        //   case -1:
        //     m->return_status = "Non_Regular_Sensitivities";
        //     m->unified_return_status = SOLVER_RET_NAN;
        //     if (print_status_)
        //       print("MESSAGE(feasiblesqpmethod): "
        //             "No regularity of sensitivities at current point.\n");
        //     return 1;
        //   case 0:
        //     break;
        //   default:
        //     return 1;
 
        // }
        g.comment("Evaluate jac g");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = d.Jk;\n";
        std::string nlp_jac_g = g(get_function("nlp_jac_g"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_jac_g + ") return 1;\n";
        g << "if (" + nlp_jac_g + ") return 40;\n";
 
        // if (use_sqp_) {
        //   if (exact_hessian_) {
        //     // Update/reset exact Hessian
        //     m->arg[0] = d_nlp->z;
        //     m->arg[1] = d_nlp->p;
        //     m->arg[2] = &one;
        //     m->arg[3] = d_nlp->lam + nx_;
        //     m->res[0] = d->Bk;
        //     if (calc_function(m, "nlp_hess_l")) return 1;
        //     if (convexify_) {
        //       ScopedTiming tic(m->fstats.at("convexify"));
        //       if (convexify_eval(&convexify_data_.config, d->Bk, d->Bk, m->iw, m->w)) return 1;
        //     }
        //   } else if (m->iter_count==0) {
        //     ScopedTiming tic(m->fstats.at("BFGS"));
        //     // Initialize BFGS
        //     casadi_fill(d->Bk, Hsp_.nnz(), 1.);
        //     casadi_bfgs_reset(Hsp_, d->Bk);
        //   } else {
        //     ScopedTiming tic(m->fstats.at("BFGS"));
        //     // Update BFGS
        //     if (m->iter_count % lbfgs_memory_ == 0) casadi_bfgs_reset(Hsp_, d->Bk);
        //     // Update the Hessian approximation
        //     casadi_bfgs(Hsp_, d->Bk, d->dx, d->gLag, d->gLag_old, m->w);
        //   }
        g.comment("Just exact Hessian implemented, GN would be possible!");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_arg[2] = &one;\n";
        g << "m_arg[3] = d_nlp.lam+" + str(nx_) + ";\n";
        g << "m_res[0] = d.Bk;\n";
        std::string nlp_hess_l = g(get_function("nlp_hess_l"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_hess_l + ") return 1;\n";
        g << "if (" + nlp_hess_l + ") return 70;\n";
 
        // }
        // test if initialization is feasible
        // if (casadi_max_viol(nx_ + ng_, d_nlp->z, d_nlp->lbz, d_nlp->ubz) > feas_tol_) {
        //   if (print_status_) print("MESSAGE(feasiblesqpmethod): "
        //       "No feasible initialization given! "
        //       "Find feasible initialization.\n");
        //   m->return_status = "No_Feasible_Initialization";
        //   break;
        // }
        std::string  viol = g.max_viol(nx_+ ng_, "d_nlp.z", "d_nlp.lbz", "d_nlp.ubz");
        g << "if (" << viol << "> " << feas_tol_ << ") {\n";
        g << "printf(\"MESSAGE(feasiblesqpmethod): "
             "No feasible initialization given! Find feasible initialization.\\n\");\n";
        g << "break;\n";
        g << "}\n";
 
      // } else if (step_accepted == 0) {
      g << "} else if (step_accepted == 0) {\n";
        // Evaluate grad_f
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = d->gf;
        // if (calc_function(m, "nlp_grad_f")) {
        //   uout() << "What does it mean that calc_function fails here??" << std::endl;
        // }
        g.comment("Evaluate grad f");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = d.gf;\n";
        nlp_grad_f = g(get_function("nlp_grad_f"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_grad_f + ") return 1;\n";
        g << "if (" + nlp_grad_f + ") return 50;\n";
 
        // Evaluate jac_g
        // m->arg[0] = d_nlp->z;
        // m->arg[1] = d_nlp->p;
        // m->res[0] = d->Jk;
        // switch (calc_function(m, "nlp_jac_g")) {
        //   case -1:
        //     m->return_status = "Non_Regular_Sensitivities";
        //     m->unified_return_status = SOLVER_RET_NAN;
        //     if (print_status_)
        //       print("MESSAGE(feasiblesqpmethod): "
        //             "No regularity of sensitivities at current point.\n");
        //     return 1;
        //   case 0:
        //     break;
        //   default:
        //     return 1;
        // }
        g.comment("Evaluate jac g");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = d.Jk;\n";
        nlp_jac_g = g(get_function("nlp_jac_g"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_jac_g + ") return 1;\n";
        g << "if (" + nlp_jac_g + ") return 60;\n";
 
        // if (use_sqp_) {
        //   if (exact_hessian_) {
        //     // Update/reset exact Hessian
        //     m->arg[0] = d_nlp->z;
        //     m->arg[1] = d_nlp->p;
        //     m->arg[2] = &one;
        //     m->arg[3] = d_nlp->lam + nx_;
        //     m->res[0] = d->Bk;
        //     if (calc_function(m, "nlp_hess_l")) return 1;
        //     if (convexify_) {
        //       ScopedTiming tic(m->fstats.at("convexify"));
        //       if (convexify_eval(&convexify_data_.config, d->Bk, d->Bk, m->iw, m->w)) return 1;
        //     }
        //   } else if (m->iter_count==0) {
        //     ScopedTiming tic(m->fstats.at("BFGS"));
        //     // Initialize BFGS
        //     casadi_fill(d->Bk, Hsp_.nnz(), 1.);
        //     casadi_bfgs_reset(Hsp_, d->Bk);
        //   } else {
        //     ScopedTiming tic(m->fstats.at("BFGS"));
        //     // Update BFGS
        //     if (m->iter_count % lbfgs_memory_ == 0) casadi_bfgs_reset(Hsp_, d->Bk);
        //     // Update the Hessian approximation
        //     casadi_bfgs(Hsp_, d->Bk, d->dx, d->gLag, d->gLag_old, m->w);
        //   }
        // }
        g.comment("Just exact Hessian implemented, GN would be possible!");
        g << "m_arg[0] = d_nlp.z;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_arg[2] = &one;\n";
        g << "m_arg[3] = d_nlp.lam+" + str(nx_) + ";\n";
        g << "m_res[0] = d.Bk;\n";
        nlp_hess_l = g(get_function("nlp_hess_l"), "m_arg", "m_res", "m_iw", "m_w");
        // g << "if (" + nlp_hess_l + ") return 1;\n";
        g << "if (" + nlp_hess_l + ") return 70;\n";
 
      // }
      g << "}\n";
 
      // // Evaluate the gradient of the Lagrangian
      // casadi_copy(d->gf, nx_, d->gLag);
      // casadi_mv(d->Jk, Asp_, d_nlp->lam+nx_, d->gLag, true);
      // casadi_axpy(nx_, 1., d_nlp->lam, d->gLag);
      g.comment("Evaluate the gradient of the Lagrangian");
      g << g.copy("d.gf", nx_, "d.gLag") << "\n";
      g << g.mv("d.Jk", Asp_, "d_nlp.lam+"+str(nx_), "d.gLag", true) << "\n";
      g << g.axpy(nx_, "1.0", "d_nlp.lam", "d.gLag") << "\n";
 
      // Primal infeasability
      // double pr_inf = casadi_max_viol(nx_+ng_, d_nlp->z, d_nlp->lbz, d_nlp->ubz);
      g.comment("Primal infeasability");
      g.local("pr_inf", "casadi_real");
      g << "pr_inf = " << g.max_viol(nx_+ng_, "d_nlp.z", "d_nlp.lbz", "d_nlp.ubz") << ";\n";
 
      // inf-norm of Lagrange gradient
      // double du_inf = casadi_norm_inf(nx_, d->gLag);
      g.comment("inf-norm of lagrange gradient");
      g.local("du_inf", "casadi_real");
      g << "du_inf = " << g.norm_inf(nx_, "d.gLag") << ";\n";
 
      // inf-norm of step, d->dx is a nullptr???
      // double dx_norminf = casadi_norm_inf(nx_, d->dx);
      g.comment("inf-norm of step");
      g.local("dx_norminf", "casadi_real");
      g << "dx_norminf = " << g.norm_inf(nx_, "d.dx") << ";\n";
 
      // Printing information about the actual iterate
      // if (print_iteration_) {
      //   // if (m->iter_count % 10 == 0) print_iteration();
      //   print_iteration();
      //   print_iteration(m->iter_count, d_nlp->f, m_k, tr_ratio,
      //                   pr_inf, du_inf, dx_norminf, m->reg, tr_rad_prev, info);
      //   info = "";
      // }
      g << "if (" << print_iteration_ << ") {\n";
        g << "printf(\"%4s %9s %14s %9s %9s %9s %9s %5s\\n\", "
             "\"iter\", \"m_k\", \"objective\", \"tr_ratio\", "
             "\"inf_pr\",\"inf_du\", \"||d||\", \"tr_rad\");\n";
        g << "printf(\"%4lld %9.2e %14.6e %9.2e %9.2e %9.2e %9.2e %5.2e\\n\", "
             "iter_count, m_k, m_f, tr_ratio, pr_inf, du_inf, dx_norminf, tr_rad_prev);";
      g << "}\n";
 
      // tr_rad_prev = tr_rad;
      g << "tr_rad_prev = tr_rad;\n";
 
      // // Callback function NOT IMPLEMENTED IN CODEGEN
      // if (callback(m)) {
      //   if (print_status_) print("WARNING(feasiblesqpmethod): Aborted by callback...\n");
      //   m->return_status = "User_Requested_Stop";
      //   break;
      // }
 
      // Checking convergence criteria
      // Where is the complementarity condition??
      // if (m->iter_count >= min_iter_ && pr_inf < tol_pr_ && du_inf < tol_du_) {
      //   if (print_status_)
      //     print("MESSAGE(feasiblesqpmethod): "
      //            "Convergence achieved after %d iterations\n", m->iter_count);
      //   m->return_status = "Solve_Succeeded";
      //   m->success = true;
      //   break;
      // }
 
 
      g << "if (iter_count >= " << max_iter_ << ") {\n";
        g << "if (" << print_status_ << ") {\n";
        g << g.printf("MESSAGE(feasiblesqpmethod): "
                      "Maximum number of iterations reached.\\n") << "\n";
        g << "break;\n";
        g << "}\n";
      g << "}\n";
 
      // Formulate the QP
      // Define lower bounds
      // casadi_copy(d_nlp->lbz, nx_+ng_, d->lbdz);
      // casadi_axpy(nx_+ng_, -1., d_nlp->z, d->lbdz);
      // casadi_clip_min(d->lbdz, nx_, -tr_rad, d->tr_mask);
      g.comment("Formulate the QP");
      g.comment("Define the lower bounds");
      g << g.copy("d_nlp.lbz", nx_+ng_, "d.lbdz") << "\n";
      g << g.axpy(nx_+ng_, "-1.0", "d_nlp.z", "d.lbdz") << "\n";
      g << g.clip_min("d.lbdz", nx_, "-tr_rad", "d.tr_mask") << "\n";
 
 
      // Define upper bounds
      // casadi_copy(d_nlp->ubz, nx_+ng_, d->ubdz);
      // casadi_axpy(nx_+ng_, -1., d_nlp->z, d->ubdz);
      // casadi_clip_max(d->ubdz, nx_, tr_rad, d->tr_mask);
      g.comment("Define the upper bounds");
      g << g.copy("d_nlp.ubz", nx_+ng_, "d.ubdz") << "\n";
      g << g.axpy(nx_+ng_, "-1.0", "d_nlp.z", "d.ubdz") << "\n";
      g << g.clip_max("d.ubdz", nx_, "tr_rad", "d.tr_mask") << "\n";
 
      // // Initial guess
      // casadi_copy(d_nlp->lam, nx_+ng_, d->dlam);
      g.comment("Initial guess");
      g << g.copy("d_nlp.lam", nx_+ng_, "d.dlam") << "\n";
 
      // Increase counter
      // m->iter_count++;
      g.comment("Increase counter");
      g << "++iter_count;\n";
 
      // int ret = 0;
      // g << "ret = 0;\n";
 
      // Solve the QP
      // if (use_sqp_) {
      //   ret = solve_QP(m, d->Bk, d->gf, d->lbdz, d->ubdz, d->Jk,
      //            d->dx, d->dlam, 0);
      // } else {
      //   ret = solve_LP(m, d->gf, d->lbdz, d->ubdz, d->Jk,
      //            d->dx, d->dlam, 0);
      // }
      g.comment("Solve the QP");
      codegen_qp_solve(g, "d.Bk", "d.gf", "d.lbdz", "d.ubdz", "d.Jk", "d.dx", "d.dlam", 0);
 
      // // Eval quadratic model and check for convergence
      // m_k = eval_m_k(mem);
 
      g.comment("Eval quadratic model and check for convergence");
      codegen_eval_m_k(g);
 
      g.comment("Checking convergence criteria");
      g << "if (fabs(m_k) < " << optim_tol_ << ") {\n";
      g << "printf(\"MESSAGE(feasiblesqpmethod): Optimal Point Found? "
           "Quadratic model is zero. After %lld iterations.\\n\", iter_count-1);\n";
      g << "break;\n";
      g << "}\n";
 
      // uout() << "QP step: " << std::vector<double>(d->dx, d->dx+nx_) << std::endl;
      // Detecting indefiniteness
      // if (use_sqp_) {
      //   double gain = casadi_bilin(d->Bk, Hsp_, d->dx, d->dx);
      //   if (gain < 0) {
      //     if (print_status_) print("WARNING(feasiblesqpmethod): Indefinite Hessian detected\n");
      //   }
      // }
      g.comment("Detecting indefiniteness");
      g.comment("TBD");
 
      // Do the feasibility iterations here
      // ret = feasibility_iterations(mem, tr_rad);
      g.comment("Do the feasibility iterations here");
      codegen_feasibility_iterations(g, "tr_rad");
 
      g << "if (ret < 0) {\n";
        g << "printf(\"Rejected inner iterates\\n\");\n";
        g << "tr_rad = 0.5*" << g.masked_norm_inf(nx_, "d.dx", "d.tr_mask") << ";\n";
      g << "} else {\n";
        g.comment("Evaluate f");
        g << "m_arg[0] = d.z_feas;\n";
        g << "m_arg[1] = m_p;\n";
        g << "m_res[0] = &m_f_feas;\n";
        nlp_f = g(get_function("nlp_f"), "m_arg", "m_res", "m_iw", "m_w");
        g << "if (" + nlp_f + ") return 1;\n";
 
        codegen_eval_tr_ratio(g, "m_f", "m_f_feas", "m_k");
        codegen_tr_update(g, "tr_rad", "tr_ratio");
 
        g << "if (tr_rad < "<< feas_tol_ << ") {\n";
          g << "if (" << print_status_ << ") {\n";
            g << "printf(\"MESSAGE: Trust-Region radius smaller than feasibilty!!\\n\");\n";
          g << "}\n";
          g << "break;";
        g << "}\n";
 
        codegen_step_update(g, "tr_ratio");
      g.comment("Close the step acceptance loop");
      g << "}\n";
 
      // if (!exact_hessian_) {
      //   // Evaluate the gradient of the Lagrangian with the old x but new lam (for BFGS)
      //   casadi_copy(d->gf, nx_, d->gLag_old);
      //   casadi_mv(d->Jk, Asp_, d_nlp->lam+nx_, d->gLag_old, true);
      //   casadi_axpy(nx_, 1., d_nlp->lam, d->gLag_old);
      // }
    // }
  //   g << "}\n";
      // g << "if (!" << exact_hessian_ << ") {\n";
      // g << g.copy("d.gf", nx_, "d.gLag_old") << "\n";
      // g << g.mv("d.Jk", Asp_, "d_nlp.lam+" + str(nx_), "d.gLag_old", true) << ";\n";
      // g << g.axpy(nx_, "1.", "d_nlp.lam", "d.gLag_old") << "\n";
      // g << "}\n";
 
  //   return 0;
  // }
  //Close next loop
      // g << "}\n";
 
      // g << "return 0;\n"; // Do we need this??
    // Close the loop optimization problem
    g.comment("Close the loop optimization problem");
    g << "}\n";
 
    if (bound_consistency_) {
      g << g.bound_consistency(nx_+ng_, "d_nlp.z", "d_nlp.lam", "d_nlp.lbz", "d_nlp.ubz") << ";\n";
    }
    g.copy_check("d_nlp.z", nx_, g.res(NLPSOL_X), false, true);
    g.copy_check("d_nlp.z+" + str(nx_), ng_, g.res(NLPSOL_G), false, true);
    g.copy_check("d_nlp.lam", nx_, g.res(NLPSOL_LAM_X), false, true);
    g.copy_check("d_nlp.lam+"+str(nx_), ng_, g.res(NLPSOL_LAM_G), false, true);
    g.copy_check("d_nlp.lam_p", np_, g.res(NLPSOL_LAM_P), false, true);
    g.copy_check("&m_f", 1, g.res(NLPSOL_F), false, true);
  }
 
 
  void Feasiblesqpmethod::codegen_qp_solve(CodeGenerator& cg,
              const std::string&  H, const std::string& g,
              const std::string&  lbdz, const std::string& ubdz,
              const std::string&  A, const std::string& x_opt,
              const std::string&  dlam, int mode) const {
    for (casadi_int i=0;i<qpsol_.n_in();++i) cg << "m_arg[" << i << "] = 0;\n";
    cg << "m_arg[" << CONIC_H << "] = " << H << ";\n";
    cg << "m_arg[" << CONIC_G << "] = " << g << ";\n";
    cg << "m_arg[" << CONIC_X0 << "] = " << x_opt << ";\n";
    cg << "m_arg[" << CONIC_LAM_X0 << "] = " << dlam << ";\n";
    cg << "m_arg[" << CONIC_LAM_A0 << "] = " << dlam << "+" << nx_ << ";\n";
    cg << "m_arg[" << CONIC_LBX << "] = " << lbdz << ";\n";
    cg << "m_arg[" << CONIC_UBX << "] = " << ubdz << ";\n";
    cg << "m_arg[" << CONIC_A << "] = " << A << ";\n";
    cg << "m_arg[" << CONIC_LBA << "] = " << lbdz << "+" << nx_ << ";\n";
    cg << "m_arg[" << CONIC_UBA << "] = " << ubdz << "+" << nx_ << ";\n";
    for (casadi_int i=0;i<qpsol_.n_out();++i) cg << "m_res[" << i << "] = 0;\n";
    cg << "m_res[" << CONIC_X << "] = " << x_opt << ";\n";
    cg << "m_res[" << CONIC_LAM_X << "] = " << dlam << ";\n";
    cg << "m_res[" << CONIC_LAM_A << "] = " << dlam << "+" << nx_ << ";\n";
    std::string flag = cg(qpsol_, "m_arg", "m_res", "m_iw", "m_w");
    cg << "ret = " << flag << ";\n";
    cg << "if (ret == -1000) return -1000;\n"; // equivalent to raise Exception
  }
 
  void Feasiblesqpmethod::codegen_tr_update(CodeGenerator& cg,
      const std::string& tr_rad, const std::string& tr_ratio) const {
    cg << "if (tr_ratio < " << tr_eta1_ << ") {\n";
    cg << "tr_rad = " << tr_alpha1_ <<"*" << cg.masked_norm_inf(nx_, "d.dx", "d.tr_mask") << ";\n";
    std::string tol = "fabs(" + cg.masked_norm_inf(nx_, "d.dx", "d.tr_mask") + " - tr_rad)";
    cg << "} else if (tr_ratio > " << tr_eta2_ << " && " << tol << " < " << optim_tol_ << " ) {\n";
    cg << "tr_rad = " << cg.fmin(str(tr_alpha2_)+"*tr_rad", str(tr_rad_max_)) << ";\n";
    cg << "}\n";
    cg.comment("else: keep trust-region as it is....");
  }
 
  void Feasiblesqpmethod::codegen_eval_m_k(CodeGenerator& cg) const {
  // auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  // auto d = &m->d;
  // if (use_sqp_) {
  //   return 0.5*casadi_bilin(d->Bk, Hsp_, d->dx, d->dx) + casadi_dot(nx_, d->gf, d->dx);
  // } else {
  //   return casadi_dot(nx_, d->gf, d->dx);
  // }
  cg << "m_k = 0.5*" << cg.bilin("d.Bk", Hsp_, "d.dx", "d.dx")
     << "+" << cg.dot(nx_, "d.gf", "d.dx") << ";\n";
}
 
  void Feasiblesqpmethod::codegen_eval_tr_ratio(CodeGenerator& cg,
      const std::string& val_f, const std::string& val_f_corr, const std::string& val_m_k) const {
    // return (val_f - val_f_corr) / (-val_m_k);
    cg << "tr_ratio = (" + val_f + "-" + val_f_corr + ") / (-" + val_m_k + ");\n";
  }
 
  void Feasiblesqpmethod::codegen_step_update(CodeGenerator& cg,
    const std::string& tr_ratio) const {
  // auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
  // auto d_nlp = &m->d_nlp;
  // auto d = &m->d;
 
  // if (tr_ratio > tr_acceptance_) {
  //   // This is not properly implemented yet: d_nlp->z_old = d_mlp->z;
  //   casadi_copy(d->z_feas, nx_ + ng_, d_nlp->z);
  //   d_nlp->f = d->f_feas;
  //   casadi_copy(d->dlam_feas, nx_ + ng_, d_nlp->lam);
 
  //   uout() << "ACCEPTED" << std::endl;
  //   return 0;
  // } else {
  //   uout() << "REJECTED" << std::endl;
  //   return -1;
  // }
  cg << "if(" + tr_ratio + ">" << tr_acceptance_ << ") {\n";
    cg << cg.copy("d.z_feas", nx_ + ng_, "d_nlp.z") << "\n";
    cg << "m_f = m_f_feas;\n";
    cg << cg.copy("d.dlam_feas", nx_ + ng_, "d_nlp.lam") << "\n";
    cg << "printf(\"ACCEPTED\\n\");\n";
    cg << "ret = 0;\n";
  cg << "} else {\n";
    cg << "printf(\"REJECTED\\n\");\n";
    cg << "ret = -1;\n";
  cg << "}\n";
}
 
  void Feasiblesqpmethod::codegen_feasibility_iterations(CodeGenerator& cg,
    const std::string& tr_rad) const {
  // cg.local("ret", "casadi_int");
  cg.init_local("ret", "0");
 
  // casadi_copy(d->dx, nx_, d->dx_feas);
  cg << cg.copy("d.dx", nx_, "d.dx_feas") << "\n";
 
  // casadi_copy(d->dlam, nx_ + ng_, d->dlam_feas);
  cg << cg.copy("d.dlam", nx_, "d.dlam_feas") << "\n";
 
  // Why do we do this at the moment??
  // casadi_copy(d->dlam, nx_+ng_, d->z_tmp);
  // casadi_axpy(nx_+ng_, -1.0, d_nlp->lam, d->z_tmp);
  cg << cg.copy("d.dlam", nx_+ng_, "d.z_tmp") << "\n";
  cg << cg.axpy(nx_+ng_, "-1.0", "d_nlp.lam", "d.z_tmp") << "\n";
 
  // this is in solve in fslp.py
  // double step_inf_norm = casadi_masked_norm_inf(nx_, d->dx, d->tr_mask);
  // double prev_step_inf_norm = step_inf_norm;
  cg.local("step_inf_norm", "casadi_real");
  cg << "step_inf_norm = " << cg.masked_norm_inf(nx_, "d.dx", "d.tr_mask") << ";\n";
  cg.local("prev_step_inf_norm", "casadi_real");
  cg << "prev_step_inf_norm = step_inf_norm;\n";
  // cg.init_local("prev_step_inf_norm", "step_inf_norm");
 
  // self.x_tmp = self.x_k + p_tmp
  // casadi_copy(d_nlp->z, nx_+ng_, d->z_feas);
  // casadi_axpy(nx_, 1., d->dx_feas, d->z_feas);
  cg << cg.copy("d_nlp.z", nx_+ng_, "d.z_feas") << "\n";
  cg << cg.axpy(nx_, "1.0", "d.dx_feas", "d.z_feas") << "\n";
 
 
  // if (use_anderson_) {
  //   // anderson_acc_init_memory(mem, d->dx_feas, d->z_feas);
  //   anderson_acc_init_memory(mem, d->dx_feas, d_nlp->z);
  // }
  // cg << "if (" << use_anderson_ << ") {\n";
  // cg << cg.codegen_anderson_acc_init_memory(cg, "d.dx_feas", "d_nlp.z");
  // cg << "}\n";
 
  // Evaluate g
  //   self.g_tmp = self.__eval_g(self.x_tmp)
  // m->arg[0] = d->z_feas;
  // m->arg[1] = d_nlp->p;
  // m->res[0] = d->z_feas + nx_;
  // if (calc_function(m, "nlp_g")) {
  //   uout() << "What does it mean that calc_function fails here??" << std::endl;
  // }
  cg.comment("Evaluate g");
  cg << "m_arg[0] = d.z_feas;\n";
  cg << "m_arg[1] = m_p;\n";
  cg << "m_res[0] = d.z_feas+" + str(nx_) + ";\n";
  std::string nlp_g = cg(get_function("nlp_g"), "m_arg", "m_res", "m_iw", "m_w");
  // cg << "if (" + nlp_g + ") return 1;\n";
  cg << "if (" + nlp_g + ") return 100;\n";
 
 
  // int inner_iter = 0;
  cg.local("inner_iter", "casadi_int");
  cg.init_local("inner_iter", "0");
 
  // double prev_infeas = casadi_max_viol(nx_+ng_, d->z_feas, d_nlp->lbz, d_nlp->ubz);
  // double curr_infeas = prev_infeas;
  cg.local("prev_infeas", "casadi_real");
  cg << "prev_infeas =" << cg.max_viol(nx_+ng_, "d.z_feas", "d_nlp.lbz", "d_nlp.ubz") << ";\n";
  cg.local("curr_infeas", "casadi_real");
  cg << "curr_infeas = prev_infeas;\n";
 
 
  // Calculate asymptotic exactness of current step
  // casadi_copy(d->dx, nx_, d->z_tmp);
  // casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
  // casadi_axpy(nx_, 1., d_nlp->z, d->z_tmp);
  // double as_exac = casadi_norm_2(nx_, d->z_tmp) / casadi_norm_2(nx_, d->dx);
  cg << cg.copy("d.dx", nx_, "d.z_tmp") << "\n";
  cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.z_tmp") << "\n";
  cg << cg.axpy(nx_, "1.0", "d_nlp.z", "d.z_tmp") << "\n";
  cg.local("as_exac", "casadi_real");
  cg << "as_exac =" << cg.norm_2(nx_, "d.z_tmp") << "/" << cg.norm_2(nx_, "d.dx") << ";\n";
 
  // double kappa_watchdog = 0.0;
  // double kappa = 0.0;
  // double acc_as_exac = 0.0;
  cg.local("kappa_watchdog", "casadi_real");
  cg.init_local("kappa_watchdog", "0.0");
  cg.local("kappa", "casadi_real");
  cg.init_local("kappa", "0.0");
  cg.local("acc_as_exac", "casadi_real");
  cg << "acc_as_exac = 0.0;\n";
 
  // double watchdog_prev_inf_norm = prev_step_inf_norm; // until here everything is correct!
  cg.local("watchdog_prev_inf_norm", "casadi_real");
  // cg.init_local("watchdog_prev_inf_norm", "prev_step_inf_norm");
  cg << "watchdog_prev_inf_norm = prev_step_inf_norm;\n";
 
  // for (int j=0; j<max_inner_iter_; ++j) {
  //   if (curr_infeas < feas_tol_) {
  //     inner_iter = j;
  //     // kappa_acceptance = true;
  //     if (as_exac < 0.5) {
  //       return 0;
  //     } else {
  //       return -1;
  //     }
  //   } else if (j>0 && (curr_infeas > 1.0 || as_exac > 1.0)) {
  //     // kappa_acceptance = false;
  //     return -1;
  //   }
  cg << "for (int j=0;j<" << max_inner_iter_ << "; ++j) {\n";
    cg << "if (curr_infeas < " << feas_tol_ << ") {\n";
      cg << "inner_iter = j;\n";
      cg << "if (as_exac < 0.5) {\n";
        cg << "ret = 0; \n";
        cg << "break; \n";
      cg << "} else {\n";
        cg << "ret = -1;\n";
        cg << "break; \n";
      cg << "}\n";
    cg << "} else if (j>0 && (curr_infeas > 1.0 || as_exac > 1.0)) {\n";
      cg << "ret = -1;\n";
      cg << "break; \n";
    cg << "}\n";
 
 
    // inner_iter = j+1;
    cg << "inner_iter =j+1;\n";
 
    // create corrected gradient here -----------------------------
    // casadi_copy(d->z_feas, nx_, d->z_tmp);
    // casadi_axpy(nx_, -1., d_nlp->z, d->z_tmp);
    // casadi_copy(d->gf, nx_, d->gf_feas);
    cg << cg.copy("d.z_feas", nx_, "d.z_tmp") << "\n";
    cg << cg.axpy(nx_, "-1.0", "d_nlp.z", "d.z_tmp") << "\n";
    cg << cg.copy("d.gf", nx_, "d.gf_feas") << "\n";
    // In case of SQP we need to multiply with
    // if (use_sqp_) {
    //   casadi_mv(d->Bk, Hsp_, d->z_tmp, d->gf_feas, true);
    // }
    cg.comment("Just SQP implemented so far!");
    // cg << "if (" << use_sqp_ << ") {\n";
    cg << cg.mv("d.Bk", Hsp_, "d.z_tmp", "d.gf_feas", true) << "\n";
    // cg << "}\n";
 
    // create bounds of correction QP -----------------------------
    // upper bounds of constraints
    // casadi_copy(d_nlp->ubz + nx_, ng_, d->ubdz_feas + nx_);
    // casadi_axpy(ng_, -1., d->z_feas + nx_, d->ubdz_feas + nx_);
    cg << cg.copy("d_nlp.ubz+"+str(nx_), ng_, "d.ubdz_feas+"+str(nx_)) << "\n";
    cg << cg.axpy(ng_, "-1.0", "d.z_feas+"+str(nx_), "d.ubdz_feas+"+str(nx_)) << "\n";
 
    // lower bounds of constraints
    // casadi_copy(d_nlp->lbz + nx_, ng_, d->lbdz_feas + nx_);
    // casadi_axpy(ng_, -1., d->z_feas + nx_, d->lbdz_feas + nx_);
    cg << cg.copy("d_nlp.lbz+"+str(nx_), ng_, "d.lbdz_feas+"+str(nx_)) << "\n";
    cg << cg.axpy(ng_, "-1.0", "d.z_feas+"+str(nx_), "d.lbdz_feas+"+str(nx_)) << "\n";
 
 
    // casadi_copy(d_nlp->lbz, nx_, d->lbdz_feas);
    // casadi_clip_min(d->lbdz_feas, nx_, -tr_rad, d->tr_mask);
    cg << cg.copy("d_nlp.lbz", nx_, "d.lbdz_feas") << "\n";
    cg << cg.clip_min("d.lbdz_feas", nx_, "-tr_rad", "d.tr_mask") << "\n";
 
 
    // casadi_axpy(nx_, -1., d->z_feas, d->lbdz_feas);
    // casadi_axpy(nx_, 1., d_nlp->z, d->lbdz_feas);
    cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.lbdz_feas") << "\n";
    cg << cg.axpy(nx_, "1.0", "d_nlp.z", "d.lbdz_feas") << "\n";
 
 
    // casadi_copy(d_nlp->lbz, nx_, d->z_tmp);
    // casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    cg << cg.copy("d_nlp.lbz", nx_, "d.z_tmp") << "\n";
    cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.z_tmp") << "\n";
 
    // comparison of both vectors
    // casadi_vector_fmax(nx_, d->z_tmp, d->lbdz_feas, d->lbdz_feas);
    cg << cg.vector_fmax(nx_, "d.z_tmp", "d.lbdz_feas", "d.lbdz_feas");
 
    // casadi_copy(d_nlp->ubz, nx_, d->ubdz_feas);
    // casadi_clip_max(d->ubdz_feas, nx_, tr_rad, d->tr_mask);
    cg << cg.copy("d_nlp.ubz", nx_, "d.ubdz_feas") << "\n";
    cg << cg.clip_max("d.ubdz_feas", nx_, "tr_rad", "d.tr_mask") << ";\n";
 
    // casadi_axpy(nx_, -1., d->z_feas, d->ubdz_feas);
    // casadi_axpy(nx_, 1., d_nlp->z, d->ubdz_feas);
    cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.ubdz_feas") << "\n";
    cg << cg.axpy(nx_, "1.0", "d_nlp.z", "d.ubdz_feas") << "\n";
 
 
    // casadi_copy(d_nlp->ubz, nx_, d->z_tmp);
    // casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    // casadi_vector_fmin(nx_, d->z_tmp, d->ubdz_feas, d->ubdz_feas);
    cg << cg.copy("d_nlp.ubz", nx_, "d.z_tmp") << "\n";
    cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.z_tmp") << "\n";
    cg << cg.vector_fmin(nx_, "d.z_tmp", "d.ubdz_feas", "d.ubdz_feas");
 
    // if (use_sqp_) {
    //   int ret = solve_QP(m, d->Bk, d->gf_feas, d->lbdz_feas, d->ubdz_feas,
    //     d->Jk, d->dx_feas, d->dlam_feas, 0);
    // } else {
    //   int ret = solve_LP(m, d->gf_feas, d->lbdz_feas, d->ubdz_feas,
    //     d->Jk, d->dx_feas, d->dlam_feas, 0);
    // }
    cg.comment("Just SQP implemented. Solve the feasible QP");
    codegen_qp_solve(cg, "d.Bk", "d.gf_feas", "d.lbdz_feas", "d.ubdz_feas",
                         "d.Jk", "d.dx_feas", "d.dlam_feas", 0);
 
 
    // step_inf_norm = casadi_masked_norm_inf(nx_, d->dx_feas, d->tr_mask);
    cg << "step_inf_norm = " << cg.masked_norm_inf(nx_, "d.dx_feas", "d.tr_mask") << ";\n";
 
    // if (use_anderson_) {
    //   anderson_acc_step_update(mem, j);
    // } else {
    //   casadi_axpy(nx_, 1., d->dx_feas, d->z_feas);
    // }
    cg.comment("No Anderson Acceleration implemented yet.");
    cg << cg.axpy(nx_, "1.0", "d.dx_feas", "d.z_feas") << "\n";
 
    // Evaluate g
    // m->arg[0] = d->z_feas;
    // m->arg[1] = d_nlp->p;
    // m->res[0] = d->z_feas + nx_;
    // if (calc_function(m, "nlp_g")) {
    //   uout() << "What does it mean that calc_function fails here??" << std::endl;
    // }
    cg.comment("Evaluate g");
    cg << "m_arg[0] = d.z_feas;\n";
    cg << "m_arg[1] = m_p;\n";
    cg << "m_res[0] = d.z_feas+" + str(nx_) + ";\n";
    nlp_g = cg(get_function("nlp_g"), "m_arg", "m_res", "m_iw", "m_w");
    // cg << "if (" + nlp_g + ") return 1;\n";
    cg << "if (" + nlp_g + ") return 100;\n";
 
    // prev_infeas = casadi_max_viol(nx_+ng_, d->z_feas, d_nlp->lbz, d_nlp->ubz);
    // curr_infeas = prev_infeas;
    // kappa = step_inf_norm/prev_step_inf_norm;
    cg << "prev_infeas =" << cg.max_viol(nx_+ng_, "d.z_feas", "d_nlp.lbz", "d_nlp.ubz") << ";\n";
    cg << "curr_infeas = prev_infeas;\n";
    cg << "kappa = step_inf_norm/prev_step_inf_norm;";
 
    // casadi_copy(d->dx, nx_, d->z_tmp);
    // casadi_axpy(nx_, -1., d->z_feas, d->z_tmp);
    // casadi_axpy(nx_, 1., d_nlp->z, d->z_tmp);
    // as_exac = casadi_norm_2(nx_, d->z_tmp) / casadi_norm_2(nx_, d->dx);
    cg << cg.copy("d.dx", nx_, "d.z_tmp") << "\n";
    cg << cg.axpy(nx_, "-1.0", "d.z_feas", "d.z_tmp") << "\n";
    cg << cg.axpy(nx_, "1.0", "d_nlp.z", "d.z_tmp") << "\n";
    cg.local("as_exac", "casadi_real");
    cg << "as_exac =" << cg.norm_2(nx_, "d.z_tmp") << "/" << cg.norm_2(nx_, "d.dx") << ";\n";
 
    cg << "printf(\"Kappa: %9.10f, Infeasibility: %9.10f, "
          "AsymptoticExctness: %9.10f\\n\", kappa, curr_infeas, as_exac);\n";
 
    // acc_as_exac += as_exac;
    cg << "acc_as_exac += as_exac;\n";
 
    // if (inner_iter % watchdog_ == 0) {
    //   kappa_watchdog = step_inf_norm / watchdog_prev_inf_norm;
    //   watchdog_prev_inf_norm = step_inf_norm;
    //   print("Kappa watchdog: %9.10f\n", kappa_watchdog);
    //   if (curr_infeas < feas_tol_ && as_exac < 0.5) {
    //     // kappa_acceptance = true;
    //     return 0;
    //   }
 
    //   if (kappa_watchdog > contraction_acceptance_value_ || acc_as_exac/watchdog_ > 0.5) {
    //     // kappa_acceptance = false;
    //     return -1;
    //   }
    // //           accumulated_as_ex = 0
    //   acc_as_exac = 0.0;
    // }
    cg << "if (inner_iter % " << watchdog_ << "== 0) {\n";
      cg << "kappa_watchdog = step_inf_norm / watchdog_prev_inf_norm;\n";
      cg << "watchdog_prev_inf_norm = step_inf_norm;\n";
      cg << "printf(\"Kappa watchdog: %9.10f\\n\", kappa_watchdog);\n";
      cg << "if (curr_infeas < "<< feas_tol_ << "&& as_exac < 0.5) {\n";
        cg << "ret = 0;\n";
        cg << "break; \n";
      cg << "}\n";
      cg << "if (kappa_watchdog > " << contraction_acceptance_value_ << " || "
         << "acc_as_exac/" << watchdog_ << "> 0.5) {\n";
        cg << "ret = -1;\n";
        cg << "break;\n";
      cg << "}\n";
 
      cg << "acc_as_exac = 0.0;\n";
    cg << "}\n"; //Added
 
    // prev_step_inf_norm = step_inf_norm;
    cg << "prev_step_inf_norm = step_inf_norm;\n";
  // }
  cg << "}\n";
  //maximum iterations reached
  // kappa_acceptance = false;
//   return -1;
// }
  cg << "if (inner_iter >=" << max_inner_iter_ << ") {\n";
  cg << "ret = -1;\n";
  cg << "}\n";
 
  }
 
  Dict Feasiblesqpmethod::get_stats(void* mem) const {
    Dict stats = Nlpsol::get_stats(mem);
    auto m = static_cast<FeasiblesqpmethodMemory*>(mem);
    stats["return_status"] = m->return_status;
    stats["iter_count"] = m->iter_count;
    return stats;
  }
 
  Feasiblesqpmethod::Feasiblesqpmethod(DeserializingStream& s) : Nlpsol(s) {
    int version = s.version("Feasiblesqpmethod", 1, 3);
    s.unpack("Feasiblesqpmethod::qpsol", qpsol_);
    if (version>=3) {
      s.unpack("Feasiblesqpmethod::qpsol_ela", qpsol_ela_);
    }
    s.unpack("Feasiblesqpmethod::exact_hessian", exact_hessian_);
    s.unpack("Feasiblesqpmethod::max_iter", max_iter_);
    s.unpack("Feasiblesqpmethod::min_iter", min_iter_);
    s.unpack("Feasiblesqpmethod::lbfgs_memory", lbfgs_memory_);
    s.unpack("Feasiblesqpmethod::tol_pr_", tol_pr_);
    s.unpack("Feasiblesqpmethod::tol_du_", tol_du_);
    s.unpack("Feasiblesqpmethod::print_header", print_header_);
    s.unpack("Feasiblesqpmethod::print_iteration", print_iteration_);
    s.unpack("Feasiblesqpmethod::print_status", print_status_);
 
    // if (version>=3) {
    //   s.unpack("Feasiblesqpmethod::elastic_mode", elastic_mode_);
    //   s.unpack("Feasiblesqpmethod::gamma_0", gamma_0_);
    //   s.unpack("Feasiblesqpmethod::gamma_max", gamma_max_);
    //   s.unpack("Feasiblesqpmethod::gamma_1_min", gamma_1_min_);
    //   s.unpack("Feasiblesqpmethod::init_feasible", init_feasible_);
    //   s.unpack("Feasiblesqpmethod::so_corr", so_corr_);
    // } else {
    //   // elastic_mode_ = false;
    //   // gamma_0_ = 0;
    //   // gamma_max_ = 0;
    //   // gamma_1_min_ = 0;
    //   init_feasible_ = false;
    //   // so_corr_ = false;
    // }
 
    s.unpack("Feasiblesqpmethod::Hsp", Hsp_);
    if (version==1) {
      Sparsity Hrsp;
      s.unpack("Feasiblesqpmethod::Hrsp", Hrsp);
    }
    s.unpack("Feasiblesqpmethod::Asp", Asp_);
    if (version==1) {
      double convexify_margin;
      s.unpack("Feasiblesqpmethod::convexify_margin", convexify_margin);
      char convexify_strategy;
      s.unpack("Feasiblesqpmethod::convexify_strategy", convexify_strategy);
      casadi_assert(convexify_strategy==0, "deserializtion failed.");
      bool Hsp_project;
      s.unpack("Feasiblesqpmethod::Hsp_project", Hsp_project);
      bool scc_transform;
      s.unpack("Feasiblesqpmethod::scc_transform", scc_transform);
      std::vector<casadi_int> scc_offset;
      s.unpack("Feasiblesqpmethod::scc_offset", scc_offset);
      std::vector<casadi_int> scc_mapping;
      s.unpack("Feasiblesqpmethod::scc_mapping", scc_mapping);
      casadi_int max_iter_eig;
      s.unpack("Feasiblesqpmethod::max_iter_eig", max_iter_eig);
      casadi_int block_size;
      s.unpack("Feasiblesqpmethod::block_size", block_size);
      Sparsity scc_sp;
      s.unpack("Feasiblesqpmethod::scc_sp", scc_sp);
      convexify_ = false;
    }
    if (version>=2) {
      s.unpack("Feasiblesqpmethod::convexify", convexify_);
      if (convexify_) Convexify::deserialize(s, "Feasiblesqpmethod::", convexify_data_);
    }
    set_feasiblesqpmethod_prob();
  }
 
  void Feasiblesqpmethod::serialize_body(SerializingStream &s) const {
    Nlpsol::serialize_body(s);
    s.version("Feasiblesqpmethod", 3);
    s.pack("Feasiblesqpmethod::qpsol", qpsol_);
    // s.pack("Feasiblesqpmethod::qpsol_ela", qpsol_ela_);
    s.pack("Feasiblesqpmethod::exact_hessian", exact_hessian_);
    s.pack("Feasiblesqpmethod::max_iter", max_iter_);
    s.pack("Feasiblesqpmethod::min_iter", min_iter_);
    s.pack("Feasiblesqpmethod::lbfgs_memory", lbfgs_memory_);
    s.pack("Feasiblesqpmethod::tol_pr_", tol_pr_);
    s.pack("Feasiblesqpmethod::tol_du_", tol_du_);
    s.pack("Feasiblesqpmethod::print_header", print_header_);
    s.pack("Feasiblesqpmethod::print_iteration", print_iteration_);
    s.pack("Feasiblesqpmethod::print_status", print_status_);
 
    s.pack("Feasiblesqpmethod::init_feasible", init_feasible_);
    s.pack("Feasiblesqpmethod::Hsp", Hsp_);
    s.pack("Feasiblesqpmethod::Asp", Asp_);
    s.pack("Feasiblesqpmethod::convexify", convexify_);
    if (convexify_) Convexify::serialize(s, "Feasiblesqpmethod::", convexify_data_);
  }
} // namespace casadi