casadi_qrqp.hpp

//
//    MIT No Attribution
//
//    Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
//
//    Permission is hereby granted, free of charge, to any person obtaining a copy of this
//    software and associated documentation files (the "Software"), to deal in the Software
//    without restriction, including without limitation the rights to use, copy, modify,
//    merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
//    permit persons to whom the Software is furnished to do so.
//
//    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//    INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//    PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//    OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//    SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
 
 
// C-REPLACE "fmin" "casadi_fmin"
// C-REPLACE "fmax" "casadi_fmax"
// C-REPLACE "fabs" "casadi_fabs"
// C-REPLACE "std::numeric_limits<T1>::min()" "casadi_real_min"
// C-REPLACE "std::numeric_limits<T1>::infinity()" "casadi_inf"
// C-REPLACE "static_cast<int>" "(int) "
 
// C-REPLACE "casadi_qp_prob<T1>" "struct casadi_qp_prob"
// C-REPLACE "casadi_qp_data<T1>" "struct casadi_qp_data"
 
// SYMBOL "qrqp_prob"
template<typename T1>
struct casadi_qrqp_prob {
  const casadi_qp_prob<T1>* qp;
  // Sparsity patterns
  const casadi_int *sp_at, *sp_kkt;
  // Symbolic QR factorization
  const casadi_int *prinv, *pc, *sp_v, *sp_r;
  // Smallest nonzero number
  T1 dmin;
  // Infinity
  T1 inf;
  // Smallest multiplier treated as inactive for the initial active set
  T1 min_lam;
  // Maximum number of iterations
  casadi_int max_iter;
  // Primal and dual error tolerance
  T1 constr_viol_tol, dual_inf_tol;
};
// C-REPLACE "casadi_qrqp_prob<T1>" "struct casadi_qrqp_prob"
 
// SYMBOL "qrqp_setup"
template<typename T1>
void casadi_qrqp_setup(casadi_qrqp_prob<T1>* p) {
  p->dmin = std::numeric_limits<T1>::min();
  p->inf = std::numeric_limits<T1>::infinity();
  p->min_lam = 0;
  p->max_iter = 1000;
  p->constr_viol_tol = 1e-8;
  p->dual_inf_tol = 1e-8;
}
 
// SYMBOL "qrqp_flag_t"
typedef enum {
  QP_SUCCESS,
  QP_MAX_ITER,
  QP_NO_SEARCH_DIR,
  QP_PRINTING_ERROR
} casadi_qrqp_flag_t;
 
// SYMBOL "qrqp_data"
template<typename T1>
struct casadi_qrqp_data {
  // Problem structure
  const casadi_qrqp_prob<T1>* prob;
  // Problem structure
  casadi_qp_data<T1>* qp;
  // Cost
  T1 f;
  // Solver status
  casadi_qrqp_flag_t status;
  // Vectors
  T1 *lbz, *ubz, *z, *infeas, *tinfeas, *sens, *lam, *w, *dz, *dlam;
  casadi_int *iw, *neverzero, *neverlower, *neverupper, *lincomb;
  // Numeric QR factorization
  T1 *nz_at, *nz_kkt, *beta, *nz_v, *nz_r;
  // Message buffer
  const char *msg;
  // Message index
  casadi_int msg_ind;
  // Stepsize
  T1 tau;
  // Singularity
  casadi_int sing;
  // Do we already have a search direction?
  int has_search_dir;
  // Smallest diagonal value for the QR factorization
  T1 mina;
  casadi_int imina;
  // Primal and dual error, corresponding index
  T1 pr, du, epr, edu;
  casadi_int ipr, idu;
  // Pending active-set change
  casadi_int index, sign;
  // Feasibility restoration active-set change
  casadi_int r_index, r_sign;
  // Iteration
  casadi_int iter;
};
// C-REPLACE "casadi_qrqp_data<T1>" "struct casadi_qrqp_data"
 
 
// SYMBOL "qrqp_work"
template<typename T1>
void casadi_qrqp_work(const casadi_qrqp_prob<T1>* p, casadi_int* sz_arg, casadi_int* sz_res,
    casadi_int* sz_iw, casadi_int* sz_w) {
  // Local variables
  casadi_int nnz_a, nnz_kkt, nnz_v, nnz_r;
  casadi_qp_work(p->qp, sz_arg, sz_res, sz_iw, sz_w);
  // Get matrix number of nonzeros
  nnz_a = p->qp->sp_a[2+p->qp->sp_a[1]];
  nnz_kkt = p->sp_kkt[2+p->sp_kkt[1]];
  nnz_v = p->sp_v[2+p->sp_v[1]];
  nnz_r = p->sp_r[2+p->sp_r[1]];
  // Temporary work vectors
  *sz_w = casadi_max(*sz_w, p->qp->nz); // casadi_project, tau memory
  *sz_iw = casadi_max(*sz_iw, p->qp->nz); // casadi_trans, tau type, allzero
  *sz_w = casadi_max(*sz_w, 2*p->qp->nz); // casadi_qr
  // Persistent work vectors
  *sz_w += nnz_kkt; // kkt
  *sz_w += p->qp->nz; // z=[xk,gk]
  *sz_w += p->qp->nz; // lbz
  *sz_w += p->qp->nz; // ubz
  *sz_w += p->qp->nz; // lam
  *sz_w += p->qp->nz; // dz
  *sz_w += p->qp->nz; // dlam
  *sz_w += casadi_max(nnz_v+nnz_r, nnz_kkt); // [v,r] or trans(kkt)
  *sz_w += p->qp->nz; // beta
  *sz_w += nnz_a; // trans(a)
  *sz_w += p->qp->nx; // infeas
  *sz_w += p->qp->nx; // tinfeas
  *sz_w += p->qp->nz; // sens
  *sz_iw += p->qp->nz; // neverzero
  *sz_iw += p->qp->nz; // neverupper
  *sz_iw += p->qp->nz; // neverlower
  *sz_iw += p->qp->nz; // lincomb
}
 
// SYMBOL "qrqp_set_work"
template<typename T1>
void casadi_qrqp_set_work(casadi_qrqp_data<T1>* d, const T1*** arg, T1*** res,
    casadi_int** iw, T1** w) {
  (void)arg; (void)res;
  // Local variables
  casadi_int nnz_a, nnz_kkt, nnz_v, nnz_r;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Get matrix number of nonzeros
  nnz_a = p->qp->sp_a[2+p->qp->sp_a[1]];
  nnz_kkt = p->sp_kkt[2+p->sp_kkt[1]];
  nnz_v = p->sp_v[2+p->sp_v[1]];
  nnz_r = p->sp_r[2+p->sp_r[1]];
  d->nz_kkt = *w; *w += nnz_kkt;
  d->z = *w; *w += p->qp->nz;
  d->lbz = *w; *w += p->qp->nz;
  d->ubz = *w; *w += p->qp->nz;
  d->lam = *w; *w += p->qp->nz;
  d->dz = *w; *w += p->qp->nz;
  d->dlam = *w; *w += p->qp->nz;
  d->nz_v = *w; *w += casadi_max(nnz_v+nnz_r, nnz_kkt);
  d->beta = *w; *w += p->qp->nz;
  d->nz_at = *w; *w += nnz_a;
  d->infeas = *w; *w += p->qp->nx;
  d->tinfeas = *w; *w += p->qp->nx;
  d->sens = *w; *w += p->qp->nz;
  d->neverzero = *iw; *iw += p->qp->nz;
  d->neverupper = *iw; *iw += p->qp->nz;
  d->neverlower = *iw; *iw += p->qp->nz;
  d->lincomb = *iw; *iw += p->qp->nz;
  d->w = *w;
  d->iw = *iw;
 
  d->nz_r = d->nz_v + nnz_v;
}
 
// SYMBOL "qrqp_reset"
template<typename T1>
int casadi_qrqp_reset(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Reset variables corresponding to previous iteration
  d->msg = 0;
  d->tau = 0.;
  d->sing = 0;
  // Correct lam if needed, determine permitted signs
  for (i=0; i<p->qp->nz; ++i) {
    // Permitted signs for lam
    d->neverzero[i] = d->lbz[i] == d->ubz[i];
    d->neverupper[i] = d->ubz[i] == p->inf;
    d->neverlower[i] = d->lbz[i] == -p->inf;
    if (d->neverzero[i] && d->neverupper[i] && d->neverlower[i]) return 1;
    // Small enough lambdas are treated as inactive
    if (!d->neverzero[i] && fabs(d->lam[i]) < p->min_lam) d->lam[i] = 0.;
    // Prevent illegal active sets
    if (d->neverzero[i] && d->lam[i] == 0.) {
      d->lam[i] = d->neverupper[i]
                || d->z[i]-d->lbz[i] <= d->ubz[i]-d->z[i] ? -p->dmin : p->dmin;
    } else if (d->neverupper[i] && d->lam[i]>0.) {
      d->lam[i] = d->neverzero[i] ? -p->dmin : 0.;
    } else if (d->neverlower[i] && d->lam[i]<0.) {
      d->lam[i] = d->neverzero[i] ? p->dmin : 0.;
    }
  }
  // Transpose A
  casadi_trans(d->qp->a, p->qp->sp_a, d->nz_at, p->sp_at, d->iw);
  // No pending active-set change
  d->index = -2;
  d->sign = 0;
  // No restoration index
  d->r_index = -2;
  d->r_sign = 0;
  // Reset iteration counter
  d->iter = 0;
  return 0;
}
 
// SYMBOL "qrqp_pr"
template<typename T1>
void casadi_qrqp_pr(casadi_qrqp_data<T1>* d) {
  // Calculate largest constraint violation
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  d->pr = 0;
  d->ipr = -1;
  for (i=0; i<p->qp->nz; ++i) {
    if (d->z[i] > d->ubz[i]+d->pr) {
      d->pr = d->z[i]-d->ubz[i];
      d->ipr = i;
    } else if (d->z[i] < d->lbz[i]-d->pr) {
      d->pr = d->lbz[i]-d->z[i];
      d->ipr = i;
    }
  }
}
 
// SYMBOL "qrqp_du"
template<typename T1>
void casadi_qrqp_du(casadi_qrqp_data<T1>* d) {
  // Calculate largest constraint violation
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  d->du = 0;
  d->idu = -1;
  for (i=0; i<p->qp->nx; ++i) {
    if (d->infeas[i] > d->du) {
      d->du = d->infeas[i];
      d->idu = i;
    } else if (d->infeas[i] < -d->du) {
      d->du = -d->infeas[i];
      d->idu = i;
    }
  }
}
 
// SYMBOL "qrqp_du_check"
template<typename T1>
int casadi_qrqp_du_check(casadi_qrqp_data<T1>* d, casadi_int i) {
  // Local variables
  casadi_int k;
  T1 new_du;
  const casadi_int *at_colind, *at_row;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // AT sparsity
  at_colind = p->sp_at + 2;
  at_row = at_colind + p->qp->na + 1;
  // Maximum infeasibility from setting from setting lam[i]=0
  if (i<p->qp->nx) {
    new_du = fabs(d->infeas[i]-d->lam[i]);
  } else {
    new_du = 0.;
    for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
      new_du = fmax(new_du, fabs(d->infeas[at_row[k]]-d->nz_at[k]*d->lam[i]));
    }
  }
  return new_du <= d->du;
}
 
// SYMBOL "qrqp_du_index"
template<typename T1>
void casadi_qrqp_du_index(casadi_qrqp_data<T1>* d) {
  // Try to improve dual feasibility by removing a constraint
  // Local variables
  casadi_int i, s;
  T1 best_sens;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Find the best lam[i] to make zero
  d->index = -1;
  best_sens = -1;
  for (i = 0; i < p->qp->nz; ++i) {
    // Skip if no dual infeasibility sensitivity
    if (d->sens[i] == 0.) continue;
    // Is the constraint enforced?
    if (d->lam[i] == 0) {
      // We're enforcing constraints
      s = d->sens[i] > 0 ? 1 : -1;
      // Make sure that enforcing the constraint is possible
      if (s > 0 ? d->neverupper[i] : d->neverlower[i]) continue;
    } else {
      // We're removing constraints
      s = 0;
      // Make sure that it's a constraint that can be removed
      if (d->neverzero[i]) continue;
      // If variable influences du, make sure sign is right
      if (d->lam[i] > 0. ? d->sens[i] > 0. : d->sens[i] < 0.) continue;
      // Skip if maximum infeasibility increases
      if (!casadi_qrqp_du_check(d, i)) continue;
    }
    // Check if best so far
    if (fabs(d->sens[i]) > best_sens) {
      best_sens = fabs(d->sens[i]);
      d->index = i;
      d->sign = s;
    }
  }
  // Accept, if any
  if (d->index >= 0) {
    if (d->sign > 0) {
      d->msg = "Enforced ubz to reduce |du|";
    } else if (d->sign < 0) {
      d->msg = "Enforced lbz to reduce |du|";
    } else if (d->lam[d->index] > 0) {
      d->msg = "Dropped ubz to reduce |du|";
    } else {
      d->msg = "Dropped lbz to reduce |du|";
    }
    d->msg_ind = d->index;
  }
}
 
// SYMBOL "qrqp_pr_index"
template<typename T1>
void casadi_qrqp_pr_index(casadi_qrqp_data<T1>* d) {
  // Try to improve primal feasibility by adding a constraint
  if (d->lam[d->ipr] == 0.) {
    // Add the most violating constraint
    if (d->z[d->ipr] < d->lbz[d->ipr]) {
      d->sign = -1;
      d->msg = "Added lbz to reduce |pr|";
    } else {
      d->sign = 1;
      d->msg = "Added ubz to reduce |pr|";
    }
    d->msg_ind = d->ipr;
    d->index = d->ipr;
  } else {
    // No improvement possible
    d->index = -1;
  }
}
 
// SYMBOL "qrqp_kkt"
template<typename T1>
void casadi_qrqp_kkt(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i, k;
  const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row,
                   *kkt_colind, *kkt_row;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Extract sparsities
  a_row = (a_colind = p->qp->sp_a+2) + p->qp->nx + 1;
  at_row = (at_colind = p->sp_at+2) + p->qp->na + 1;
  h_row = (h_colind = p->qp->sp_h+2) + p->qp->nx + 1;
  kkt_row = (kkt_colind = p->sp_kkt+2) + p->qp->nz + 1;
  // Reset w to zero
  casadi_clear(d->w, p->qp->nz);
  // Loop over rows of the (transposed) KKT
  for (i=0; i<p->qp->nz; ++i) {
    // Copy row of KKT to w
    if (i<p->qp->nx) {
      if (d->lam[i]==0) {
        for (k=h_colind[i]; k<h_colind[i+1]; ++k) d->w[h_row[k]] = d->qp->h[k];
        for (k=a_colind[i]; k<a_colind[i+1]; ++k) d->w[p->qp->nx+a_row[k]] = d->qp->a[k];
      } else {
        d->w[i] = 1.;
      }
    } else {
      if (d->lam[i]==0) {
        d->w[i] = -1.;
      } else {
        for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
          d->w[at_row[k]] = d->nz_at[k];
        }
      }
    }
    // Copy row to KKT, zero out w
    for (k=kkt_colind[i]; k<kkt_colind[i+1]; ++k) {
      d->nz_kkt[k] = d->w[kkt_row[k]];
      d->w[kkt_row[k]] = 0;
    }
  }
}
 
// SYMBOL "qrqp_kkt_vector"
template<typename T1>
void casadi_qrqp_kkt_vector(casadi_qrqp_data<T1>* d, T1* kkt_i, casadi_int i) {
  // Local variables
  casadi_int k;
  const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Extract sparsities
  a_row = (a_colind = p->qp->sp_a+2) + p->qp->nx + 1;
  at_row = (at_colind = p->sp_at+2) + p->qp->na + 1;
  h_row = (h_colind = p->qp->sp_h+2) + p->qp->nx + 1;
  // Reset kkt_i to zero
  casadi_clear(kkt_i, p->qp->nz);
  // Copy sparse entries
  if (i<p->qp->nx) {
    for (k=h_colind[i]; k<h_colind[i+1]; ++k) kkt_i[h_row[k]] = d->qp->h[k];
    for (k=a_colind[i]; k<a_colind[i+1]; ++k) kkt_i[p->qp->nx+a_row[k]] = d->qp->a[k];
  } else {
    for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
      kkt_i[at_row[k]] = -d->nz_at[k];
    }
  }
  // Add diagonal entry
  kkt_i[i] -= 1.;
}
 
// SYMBOL "qrqp_kkt_dot"
template<typename T1>
T1 casadi_qrqp_kkt_dot(casadi_qrqp_data<T1>* d, const T1* v, casadi_int i) {
  // Local variables
  casadi_int k;
  const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row;
  T1 r;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Extract sparsities
  a_row = (a_colind = p->qp->sp_a + 2) + p->qp->nx + 1;
  at_row = (at_colind = p->sp_at + 2) + p->qp->na + 1;
  h_row = (h_colind = p->qp->sp_h + 2) + p->qp->nx + 1;
  // Scalar product with the diagonal
  r = v[i];
  // Scalar product with the sparse entries
  if (i < p->qp->nx) {
    for (k=h_colind[i]; k<h_colind[i+1]; ++k) r -= v[h_row[k]] * d->qp->h[k];
    for (k=a_colind[i]; k<a_colind[i+1]; ++k) r -= v[p->qp->nx+a_row[k]] * d->qp->a[k];
  } else {
    for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
      r += v[at_row[k]] * d->nz_at[k];
    }
  }
  return r;
}
 
// SYMBOL "qrqp_kkt_residual"
template<typename T1>
void casadi_qrqp_kkt_residual(casadi_qrqp_data<T1>* d, T1* r) {
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  for (i=0; i<p->qp->nz; ++i) {
    if (d->lam[i]>0.) {
      r[i] = d->ubz[i]-d->z[i];
    } else if (d->lam[i]<0.) {
      r[i] = d->lbz[i]-d->z[i];
    } else if (i<p->qp->nx) {
      r[i] = d->lam[i]-d->infeas[i];
    } else {
      r[i] = d->lam[i];
    }
  }
}
 
// SYMBOL "qrqp_zero_blocking"
template<typename T1>
int casadi_qrqp_zero_blocking(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  T1 dz_max = 0;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Look for violated constraints that are not improving
  for (i = 0; i < p->qp->nz; ++i) {
    if (d->dz[i] < -dz_max && d->lbz[i] - d->z[i] >= d->epr) {
      dz_max = -d->dz[i];
      d->index = i;
      d->sign = -1;
      d->msg = "lbz violated with zero step";
      d->msg_ind = d->index;
    } else if (d->dz[i] > dz_max && d->z[i] - d->ubz[i] >= d->epr) {
      dz_max = d->dz[i];
      d->index = i;
      d->sign = 1;
      d->msg = "ubz violated with zero step";
      d->msg_ind = d->index;
    }
  }
  return dz_max > 0;
}
 
// SYMBOL "qrqp_primal_blocking"
template<typename T1>
void casadi_qrqp_primal_blocking(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  T1 trial_z;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Check if violation with tau=0 and not improving
  if (casadi_qrqp_zero_blocking(d)) {
    d->tau = 0.;
    return;
  }
  // Loop over all primal variables
  for (i = 0; i < p->qp->nz; ++i) {
    if (d->dz[i] == 0.) continue; // Skip zero steps
    // Trial primal step
    trial_z = d->z[i] + d->tau * d->dz[i];
    if (d->dz[i] < 0 && trial_z < d->lbz[i] - d->epr) {
      // Trial would increase maximum infeasibility
      d->tau = (d->lbz[i] - d->epr - d->z[i]) / d->dz[i];
      d->index = d->lam[i] < 0. ? -1 : i;
      d->sign = -1;
      d->msg = "Enforcing lbz";
      d->msg_ind = i;
    } else if (d->dz[i] > 0 && trial_z > d->ubz[i] + d->epr) {
      // Trial would increase maximum infeasibility
      d->tau = (d->ubz[i] + d->epr - d->z[i]) / d->dz[i];
      d->index = d->lam[i] > 0. ? -1 : i;
      d->sign = 1;
      d->msg = "Enforcing ubz";
      d->msg_ind = i;
    }
    if (d->tau <= 0) return;
  }
}
 
// SYMBOL "qrqp_dual_breakpoints"
template<typename T1>
casadi_int casadi_qrqp_dual_breakpoints(casadi_qrqp_data<T1>* d, T1* tau_list,
                                      casadi_int* ind_list, T1 tau) {
  // Local variables
  casadi_int i, n_tau, loc, next_ind, tmp_ind, j;
  T1 trial_lam, new_tau, next_tau, tmp_tau;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Dual feasibility is piecewise linear. Start with one interval [0,tau]:
  tau_list[0] = tau;
  ind_list[0] = -1; // no associated index
  n_tau = 1;
  // Find the taus corresponding to lam crossing zero and insert into list
  for (i=0; i<p->qp->nz; ++i) {
    if (d->dlam[i]==0.) continue; // Skip zero steps
    if (d->lam[i]==0.) continue; // Skip inactive constraints
    // Trial dual step
    trial_lam = d->lam[i] + tau*d->dlam[i];
    // Skip if no sign change
    if (d->lam[i]>0 ? trial_lam>=0 : trial_lam<=0) continue;
    // Location of the sign change
    new_tau = -d->lam[i]/d->dlam[i];
    // Where to insert the w[i]
    for (loc=0; loc<n_tau-1; ++loc) {
      if (new_tau<tau_list[loc]) break;
    }
    // Insert element
    n_tau++;
    next_tau=new_tau;
    next_ind=i;
    for (j=loc; j<n_tau; ++j) {
      tmp_tau = tau_list[j];
      tau_list[j] = next_tau;
      next_tau = tmp_tau;
      tmp_ind = ind_list[j];
      ind_list[j] = next_ind;
      next_ind = tmp_ind;
    }
  }
  return n_tau;
}
 
// SYMBOL "qrqp_dual_blocking"
template<typename T1>
casadi_int casadi_qrqp_dual_blocking(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i, n_tau, j, k, du_index;
  T1 tau_k, dtau, new_infeas, tau1, infeas, tinfeas;
  const casadi_int *at_colind, *at_row;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Extract sparsities
  at_row = (at_colind = p->sp_at+2) + p->qp->na + 1;
  // Dual feasibility is piecewise linear in tau. Get the intervals:
  n_tau = casadi_qrqp_dual_breakpoints(d, d->w, d->iw, d->tau);
  // No dual blocking yet
  du_index = -1;
  // How long step can we take without exceeding e?
  tau_k = 0.;
  for (j=0; j<n_tau; ++j) {
    // Distance to the next tau (may be zero)
    dtau = d->w[j] - tau_k;
    // Check if maximum dual infeasibility gets exceeded
    for (k=0; k<p->qp->nx; ++k) {
      // Get infeasibility and infeasibility tangent
      infeas  = d->infeas[k];
      tinfeas  = d->tinfeas[k];
      // Make sure tinfeas>0
      if (fabs(tinfeas)<1e-14) {
        // Skip
        continue;
      } else if (tinfeas<0) {
        // Switch signs
        infeas *= -1;
        tinfeas *= -1;
      }
      // Tentative new infeasibility
      new_infeas = infeas + dtau*tinfeas;
      // Does infeasibility get exceeded
      if (new_infeas > d->edu) {
        // Sign change and exceeded
        tau1 = fmax(tau_k, tau_k + (d->edu - infeas)/tinfeas);
        if (tau1 < d->tau) {
          // Enforce dual blocking constraint
          d->tau = tau1;
          du_index = k;
        }
      }
    }
    // Update infeasibility
    casadi_axpy(p->qp->nx, fmin(d->tau - tau_k, dtau), d->tinfeas, d->infeas);
    // Stop here if dual blocking constraint
    if (du_index>=0) return du_index;
    // Continue to the next tau
    tau_k = d->w[j];
    // Get component, break if last
    i = d->iw[j];
    if (i<0) break;
    // Update sign or tinfeas
    if (!d->neverzero[i]) {
      // lam becomes zero, update the infeasibility tangent
      if (i<p->qp->nx) {
        // Set a lam_x to zero
        d->tinfeas[i] -= d->dlam[i];
      } else {
        // Set a lam_a to zero
        for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
          d->tinfeas[at_row[k]] -= d->nz_at[k]*d->dlam[i];
        }
      }
    }
  }
  return du_index;
}
 
// SYMBOL "qrqp_take_step"
template<typename T1>
void casadi_qrqp_take_step(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Get current sign
  for (i=0; i<p->qp->nz; ++i) d->iw[i] = d->lam[i]>0. ? 1 : d->lam[i]<0 ? -1 : 0;
  // Take primal-dual step
  casadi_axpy(p->qp->nz, d->tau, d->dz, d->z);
  casadi_axpy(p->qp->nz, d->tau, d->dlam, d->lam);
  // Update sign
  for (i=0; i<p->qp->nz; ++i) {
    // Allow sign changes for certain components
    if (d->neverzero[i] && (d->iw[i]<0 ? d->lam[i]>0 : d->lam[i]<0)) {
      d->iw[i]=-d->iw[i];
    }
    // Ensure correct sign
    switch (d->iw[i]) {
      case -1: d->lam[i] = fmin(d->lam[i], -p->dmin); break;
      case  1: d->lam[i] = fmax(d->lam[i],  p->dmin); break;
      case  0: d->lam[i] = 0.; break;
    }
  }
}
 
// SYMBOL "qrqp_flip_check"
template<typename T1>
int casadi_qrqp_flip_check(casadi_qrqp_data<T1>* d) {
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Calculate the difference between unenforced and enforced column index
  casadi_qrqp_kkt_vector(d, d->dlam, d->index);
  // Calculate the difference between old and new column index
  if (d->sign == 0) casadi_scal(p->qp->nz, -1., d->dlam);
  // Try to find a linear combination of the new columns
  casadi_qr_solve(d->dlam, 1, 0, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
    p->prinv, p->pc, d->w);
  // If dlam[index]!=1, new columns must be linearly independent
  if (fabs(d->dlam[d->index]-1.) >= 1e-12) return 0;
  // Next, find a linear combination of the new rows
  casadi_clear(d->dz, p->qp->nz);
  d->dz[d->index] = 1;
  casadi_qr_solve(d->dz, 1, 1, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
    p->prinv, p->pc, d->w);
  // Normalize dlam, dz
  casadi_scal(p->qp->nz, 1./sqrt(casadi_dot(p->qp->nz, d->dlam, d->dlam)), d->dlam);
  casadi_scal(p->qp->nz, 1./sqrt(casadi_dot(p->qp->nz, d->dz, d->dz)), d->dz);
  // KKT system will be singular
  return 1;
}
 
// SYMBOL "qrqp_factorize"
template<typename T1>
void casadi_qrqp_factorize(casadi_qrqp_data<T1>* d) {
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Do we already have a search direction due to lost singularity?
  if (d->has_search_dir) {
    d->sing = 1;
    return;
  }
  // Construct the KKT matrix
  casadi_qrqp_kkt(d);
  // QR factorization
  casadi_qr(p->sp_kkt, d->nz_kkt, d->w, p->sp_v, d->nz_v, p->sp_r,
            d->nz_r, d->beta, p->prinv, p->pc);
  // Check singularity
  d->sing = casadi_qr_singular(&d->mina, &d->imina, d->nz_r, p->sp_r, p->pc, 1e-12);
}
 
// SYMBOL "qrqp_expand_step"
template<typename T1>
void casadi_qrqp_expand_step(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Calculate change in Lagrangian gradient
  casadi_clear(d->dlam, p->qp->nx);
  casadi_mv(d->qp->h, p->qp->sp_h, d->dz, d->dlam, 0); // gradient of the objective
  casadi_mv(d->qp->a, p->qp->sp_a, d->dz + p->qp->nx, d->dlam, 1); // gradient of the Lagrangian
  // Step in lam[:nx]
  casadi_scal(p->qp->nx, -1., d->dlam);
  // For inactive constraints, lam(x) step is zero
  for (i = 0; i < p->qp->nx; ++i) if (d->lam[i] == 0.) d->dlam[i] = 0.;
  // Step in lam[nx:]
  casadi_copy(d->dz+p->qp->nx, p->qp->na, d->dlam + p->qp->nx);
  // Step in z[nx:]
  casadi_clear(d->dz + p->qp->nx, p->qp->na);
  casadi_mv(d->qp->a, p->qp->sp_a, d->dz, d->dz + p->qp->nx, 0);
  // Avoid steps that are nonzero due to numerics
  for (i = 0; i < p->qp->nz; ++i) if (fabs(d->dz[i]) < 1e-14) d->dz[i] = 0.;
  // Tangent of the dual infeasibility at tau=0
  casadi_clear(d->tinfeas, p->qp->nx);
  casadi_mv(d->qp->h, p->qp->sp_h, d->dz, d->tinfeas, 0);
  casadi_mv(d->qp->a, p->qp->sp_a, d->dlam + p->qp->nx, d->tinfeas, 1);
  casadi_axpy(p->qp->nx, 1., d->dlam, d->tinfeas);
}
 
// SYMBOL "qrqp_pr_direction"
template<typename T1>
int casadi_qrqp_pr_direction(casadi_qrqp_data<T1>* d) {
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  for (i=0; i<p->qp->nz; ++i) {
    if (d->lbz[i] - d->z[i] >= d->epr) {
      // Prevent further violation of lower bound
      if (d->dz[i] < 0 || d->dlam[i] > 0) return 1;
    } else if (d->z[i] - d->ubz[i] >= d->epr) {
      // Prevent further violation of upper bound
      if (d->dz[i] > 0 || d->dlam[i] < 0) return 1;
    }
  }
  return 0;
}
 
// SYMBOL "qrqp_du_direction"
template<typename T1>
int casadi_qrqp_du_direction(casadi_qrqp_data<T1>* d) {
  casadi_int i;
  const casadi_qrqp_prob<T1>* p = d->prob;
  for (i=0; i<p->qp->nx; ++i) {
    // Prevent further increase in dual infeasibility
    if (d->infeas[i] <= -d->edu && d->tinfeas[i] < -1e-12) {
      return 1;
    } else if (d->infeas[i] >= d->edu && d->tinfeas[i] > 1e-12) {
      return 1;
    }
  }
  return 0;
}
 
// SYMBOL "qrqp_enforceable"
template<typename T1>
int casadi_qrqp_enforceable(casadi_qrqp_data<T1>* d, casadi_int i, casadi_int s) {
  // Local variables
  casadi_int k;
  const casadi_int *at_colind, *at_row;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Can always enforce if not at bound
  if (fabs(d->infeas[i]) < d->edu) return 1;
  // AT sparsity
  at_colind = p->sp_at + 2;
  at_row = at_colind + p->qp->na + 1;
  // Can we set lam[i] := s*DMIN without exceeding edu?
  if (i<p->qp->nx) {
    return (s < 0) == (d->infeas[i] > 0);
  } else {
    for (k=at_colind[i-p->qp->nx]; k<at_colind[i-p->qp->nx+1]; ++k) {
      if (d->nz_at[k] > 0) {
        if ((s > 0) == (d->infeas[at_row[k]] > 0)) return 0;
      } else if (d->nz_at[k] < 0) {
        if ((s < 0) == (d->infeas[at_row[k]] > 0)) return 0;
      }
    }
    return 1;
  }
}
 
// SYMBOL "qrqp_singular_step"
// C-REPLACE "static_cast<T1*>(0)" "0"
template<typename T1>
int casadi_qrqp_singular_step(casadi_qrqp_data<T1>* d) {
  // Local variables
  T1 tau_test, tau;
  casadi_int nnz_kkt, nk, k, i, best_k, best_neg, neg;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Find the columns that take part in any linear combination
  for (i = 0; i < p->qp->nz; ++i) d->lincomb[i] = 0;
  for (k = 0; k < d->sing; ++k) {
    if (!d->has_search_dir) {
      casadi_qr_colcomb(d->dlam, d->nz_r, p->sp_r, p->pc, 1e-12, k);
    }
    for (i = 0; i < p->qp->nz; ++i) if (fabs(d->dlam[i]) >= 1e-12) d->lincomb[i]++;
  }
 
  if (d->has_search_dir) {
    // One, given search direction
    nk = 1;
  } else {
    // QR factorization of the transpose
    casadi_trans(d->nz_kkt, p->sp_kkt, d->nz_v, p->sp_kkt, d->iw);
    nnz_kkt = p->sp_kkt[2+p->qp->nz]; // kkt_colind[nz]
    casadi_copy(d->nz_v, nnz_kkt, d->nz_kkt);
    casadi_qr(p->sp_kkt, d->nz_kkt, d->w, p->sp_v, d->nz_v, p->sp_r, d->nz_r,
              d->beta, p->prinv, p->pc);
    // For all nullspace vectors
    nk = casadi_qr_singular(static_cast<T1*>(0), 0, d->nz_r, p->sp_r, p->pc, 1e-12);
  }
  // Best flip
  best_k = best_neg = -1;
  tau = p->inf;
  for (k=0; k<nk; ++k) {
    if (!d->has_search_dir) {
      // Get a linear combination of the rows in kkt
      casadi_qr_colcomb(d->dz, d->nz_r, p->sp_r, p->pc, 1e-12, k);
    }
    // Which constraints can be flipped in order to increase rank?
    for (i=0; i<p->qp->nz; ++i) {
      d->iw[i] = d->lincomb[i] && fabs(casadi_qrqp_kkt_dot(d, d->dz, i)) > 1e-12;
    }
    // Calculate step, dz and dlam
    casadi_qrqp_expand_step(d);
    // Try both positive and negative direction
    for (neg = 0; neg < 2; ++neg) {
      // Negate direction
      if (neg) {
        casadi_scal(p->qp->nz, -1., d->dz);
        casadi_scal(p->qp->nz, -1., d->dlam);
        casadi_scal(p->qp->nx, -1., d->tinfeas);
      }
      // Make sure primal infeasibility doesn't exceed limits
      if (casadi_qrqp_pr_direction(d)) continue;
      // Make sure dual infeasibility doesn't exceed limits
      if (casadi_qrqp_du_direction(d)) continue;
      // Loop over potential active set changes
      for (i=0; i<p->qp->nz; ++i) {
        // Skip if no rank increase
        if (!d->iw[i]) continue;
        // Enforced or not?
        if (d->lam[i]==0.) {
          if (d->z[i] <= d->ubz[i] && (d->z[i] >= d->lbz[i] ?
              d->dz[i] < -1e-12 : d->dz[i] > 1e-12)) {
            // Enforce lower bound?
            if (!d->neverlower[i]
                && (tau_test = (d->lbz[i] - d->z[i]) / d->dz[i]) < tau
                && casadi_qrqp_enforceable(d, i, -1)) {
              tau = tau_test;
              d->r_index = i;
              d->r_sign = -1;
              best_k = k;
              best_neg = neg;
            }
          } else if (d->z[i] >= d->lbz[i] && (d->z[i] <= d->ubz[i] ?
              d->dz[i] > 1e-12 : d->dz[i] < -1e-12)) {
            // Enforce upper bound?
            if (!d->neverupper[i]
                && (tau_test = (d->ubz[i] - d->z[i]) / d->dz[i]) < tau
                && casadi_qrqp_enforceable(d, i, 1)) {
              tau = tau_test;
              d->r_index = i;
              d->r_sign = 1;
              best_k = k;
              best_neg = neg;
            }
          }
        } else if (!d->neverzero[i]) {
          // Drop a constraint?
          if (d->lam[i] > 0 ? d->dlam[i] < -1e-12 : d->dlam[i] > 1e-12) {
            if ((tau_test = -d->lam[i] / d->dlam[i]) < tau) {
              tau = tau_test;
              d->r_index = i;
              d->r_sign = 0;
              best_k = k;
              best_neg = neg;
            }
          }
        }
      }
    }
  }
  // Can we restore feasibility?
  if (d->r_index < 0) return 1;
  // Recalculate direction, if needed
  if (--k != best_k) {
    // Need to recalculate direction
    casadi_qr_colcomb(d->dz, d->nz_r, p->sp_r, p->pc, 1e-12, best_k);
    casadi_qrqp_expand_step(d);
    if (best_neg) tau *= -1;
  } else if (--neg != best_neg) {
    // No need to recalculate, but opposite direction
    tau *= -1;
  }
  // Scale step so that that tau=1 corresponds to a full step
  casadi_scal(p->qp->nz, tau, d->dz);
  casadi_scal(p->qp->nz, tau, d->dlam);
  casadi_scal(p->qp->nx, tau, d->tinfeas);
  return 0;
}
 
// SYMBOL "qrqp_calc_step"
template<typename T1>
int casadi_qrqp_calc_step(casadi_qrqp_data<T1>* d) {
  // Local variables
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Reset returns
  d->r_index = -1;
  d->r_sign = 0;
  // Handle singularity
  if (d->sing) return casadi_qrqp_singular_step(d);
  // Negative KKT residual
  casadi_qrqp_kkt_residual(d, d->dz);
  // Solve to get step in z[:nx] and lam[nx:]
  casadi_qr_solve(d->dz, 1, 1, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
                  p->prinv, p->pc, d->w);
  // Have step in dz[:nx] and dlam[nx:]. Calculate complete dz and dlam
  casadi_qrqp_expand_step(d);
  // Successful return
  return 0;
}
 
// SYMBOL "qrqp_calc_sens"
template<typename T1>
void casadi_qrqp_calc_sens(casadi_qrqp_data<T1>* d, casadi_int i) {
  // Local variables
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Calculate sensitivities in decreasing dual infeasibility index i
  casadi_clear(d->sens, p->qp->nz);
  if (i >= 0) {
    d->sens[i] = d->infeas[i] > 0 ? -1. : 1.;
    casadi_mv(d->qp->a, p->qp->sp_a, d->sens, d->sens + p->qp->nx, 0);
  }
}
 
// SYMBOL "qrqp_calc_dependent"
template<typename T1>
void casadi_qrqp_calc_dependent(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int i;
  T1 r;
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Calculate f
  d->f = casadi_bilin(d->qp->h, p->qp->sp_h, d->z, d->z)/2.
       + casadi_dot(p->qp->nx, d->z, d->qp->g);
  // Calculate z[nx:]
  casadi_clear(d->z+p->qp->nx, p->qp->na);
  casadi_mv(d->qp->a, p->qp->sp_a, d->z, d->z+p->qp->nx, 0);
  // Calculate gradient of the Lagrangian
  casadi_copy(d->qp->g, p->qp->nx, d->infeas);
  casadi_mv(d->qp->h, p->qp->sp_h, d->z, d->infeas, 0);
  casadi_mv(d->qp->a, p->qp->sp_a, d->lam+p->qp->nx, d->infeas, 1);
  // Calculate lam[:nx] without changing the sign accidentally, dual infeasibility
  for (i=0; i<p->qp->nx; ++i) {
    // No change if zero
    if (d->lam[i]==0) continue;
    // lam[i] with no sign restrictions
    r = -d->infeas[i];
    if (d->lam[i]>0) {
      if (d->neverzero[i] && !d->neverlower[i]) {
        d->lam[i] = r==0 ? p->dmin : r; // keep sign if r==0
      } else {
        d->lam[i] = fmax(r, p->dmin); // no sign change
      }
    } else {
      if (d->neverzero[i] && !d->neverupper[i]) {
        d->lam[i] = r==0 ? -p->dmin : r; // keep sign if r==0
      } else {
        d->lam[i] = fmin(r, -p->dmin); // no sign change
      }
    }
    // Update dual infeasibility
    d->infeas[i] += d->lam[i];
  }
  // Calculate primal and dual error
  casadi_qrqp_pr(d);
  casadi_qrqp_du(d);
  // Acceptable primal and dual error
  d->epr = fmax(d->pr, (0.5 * p->constr_viol_tol / p->dual_inf_tol) * d->du);
  d->edu = fmax(d->du, (0.5 * p->dual_inf_tol / p->constr_viol_tol) * d->pr);
  // Sensitivity in decreasing |du|
  casadi_qrqp_calc_sens(d, d->idu);
}
 
// SYMBOL "qrqp_linesearch"
template<typename T1>
void casadi_qrqp_linesearch(casadi_qrqp_data<T1>* d) {
  // Local variables
  casadi_int du_index;
  // Start with a full step and no active set change
  d->sign = 0;
  d->index = -1;
  d->tau = 1.;
  // Find largest possible step without exceeding acceptable |pr|
  casadi_qrqp_primal_blocking(d);
  // Find largest possible step without exceeding acceptable |du|
  du_index = casadi_qrqp_dual_blocking(d);
  // Take primal-dual step, avoiding accidental sign changes for lam
  casadi_qrqp_take_step(d);
  // Handle dual blocking constraints
  if (du_index >= 0) {
    // Sensititivity in decreasing du_index
    casadi_qrqp_calc_sens(d, du_index);
    // Find corresponding index
    casadi_qrqp_du_index(d);
  }
}
 
// SYMBOL "qrqp_flip"
template<typename T1>
void casadi_qrqp_flip(casadi_qrqp_data<T1>* d) {
  // Local variables
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Try to restore regularity if possible
  if (d->index == -1 && d->r_index >= 0) {
    if (d->r_sign != 0 || casadi_qrqp_du_check(d, d->r_index)) {
      d->index = d->r_index;
      d->sign = d->r_sign;
      if (d->sign > 0) {
        d->msg = "Enforced ubz for regularity";
      } else if (d->sign < 0) {
        d->msg = "Enforced lbz for regularity";
      } else if (d->lam[d->index] > 0) {
        d->msg = "Dropped ubz for regularity";
      } else {
        d->msg = "Dropped lbz for regularity";
      }
      d->msg_ind = d->index;
    }
  }
  // If nonsingular and nonzero error, try to flip a constraint
  if (!d->sing && d->index == -1) {
    if (d->pr * p->dual_inf_tol >= p->constr_viol_tol * d->du) {
      // Improve primal feasibility if dominating
      if (d->pr >= p->constr_viol_tol) casadi_qrqp_pr_index(d);
    } else {
      // Improve dual feasibility if dominating
      if (d->du >= p->dual_inf_tol) casadi_qrqp_du_index(d);
    }
  }
  // No search direction given by default
  d->has_search_dir = 0;
  // If a constraint was added
  if (d->index >= 0) {
    // Detect singularity before it happens and get nullspace vectors
    if (!d->sing) d->has_search_dir = casadi_qrqp_flip_check(d);
    // Perform the active-set change
    d->lam[d->index] = d->sign==0 ? 0 : d->sign > 0 ? p->dmin : -p->dmin;
    // Recalculate primal and dual infeasibility
    casadi_qrqp_calc_dependent(d);
  }
}
 
// SYMBOL "qrqp_prepare"
template<typename T1>
int casadi_qrqp_prepare(casadi_qrqp_data<T1>* d) {
  // Local variables
  const casadi_qrqp_prob<T1>* p = d->prob;
  // Calculate dependent quantities
  casadi_qrqp_calc_dependent(d);
  // Make an active set change
  casadi_qrqp_flip(d);
  // Form and factorize the KKT system
  casadi_qrqp_factorize(d);
  // Termination message
  if (!d->sing && d->index == -1) {
    d->status = QP_SUCCESS;
    d->msg = "Converged";
    d->msg_ind = -2;
    return 1;
  } else if (d->iter >= p->max_iter) {
    d->status = QP_MAX_ITER;
    d->msg = "Max iter";
    d->msg_ind = -2;
    return 1;
  } else if (!d->sing && d->ipr < 0 && d->idu < 0) {
    d->status = QP_SUCCESS;
    d->msg = "No primal or dual error";
    d->msg_ind = -2;
    return 1;
  } else {
    // Keep iterating
    return 0;
  }
}
 
// SYMBOL "qrqp_iterate"
template<typename T1>
int casadi_qrqp_iterate(casadi_qrqp_data<T1>* d) {
  // Reset message flag
  d->msg = 0;
  // Start a new iteration
  d->iter++;
  // Calculate search direction
  if (casadi_qrqp_calc_step(d)) {
    d->status = QP_NO_SEARCH_DIR;
    return 1;
  }
  // Line search in the calculated direction
  casadi_qrqp_linesearch(d);
  // Keep iterating
  return 0;
}
 
// SYMBOL "qrqp_print_header"
template<typename T1>
int casadi_qrqp_print_header(casadi_qrqp_data<T1>* d, char* buf, size_t buf_sz) {
#ifdef CASADI_SNPRINTF
  int flag;
  // Print to string
  flag = CASADI_SNPRINTF(buf, buf_sz, "%5s %5s %9s %9s %5s %9s %5s %9s %5s %9s  %4s",
          "Iter", "Sing", "fk", "|pr|", "con", "|du|", "var",
          "min_R", "con", "last_tau", "Note");
  // Check if error
  if (flag < 0) {
    d->status = QP_PRINTING_ERROR;
    return 1;
  }
#else
  if (buf_sz) buf[0] = '\0';
#endif
  // Successful return
  return 0;
}
 
// SYMBOL "qrqp_print_colcomb"
template<typename T1>
int casadi_qrqp_print_colcomb(casadi_qrqp_data<T1>* d, char* buf, size_t buf_sz, casadi_int j) {
#ifdef CASADI_SNPRINTF
  casadi_int num_size, n_print, i, k, buf_offset, val;
  size_t b;
  const casadi_qrqp_prob<T1>* p = d->prob;
  casadi_qr_colcomb(d->dlam, d->nz_r, p->sp_r, p->pc, 1e-12, j);
 
  // Determine max printing size
  num_size = 1;
  val = p->qp->nz-1;
  while (val) {
    val/=10;
    num_size++;
  }
 
  if (buf_sz<=4) return 1;
 
  // How many numbers can be printed?
  // Need some extra space for '...'
  // and null
  n_print = (buf_sz-4)/num_size;
 
  // Clear buffer
  for (b=0;b<buf_sz;++b) buf[b]=' ';
 
  buf_offset = 0;
  for (i=0;i<p->qp->nz;++i) {
    if (fabs(d->dlam[i]) >= 1e-12) {
      if (n_print==0) {
        buf[buf_sz-4] = '.';
        buf[buf_sz-3] = '.';
        buf[buf_sz-2] = '.';
        buf[buf_sz-1] = '\0';
        return 1;
      }
      n_print--;
      CASADI_SNPRINTF(buf+buf_offset, num_size, "%d", static_cast<int>(i));
      // Clear null chars
      for (k=0;k<num_size;++k) {
        if (buf[buf_offset+k]=='\0') buf[buf_offset+k] = ' ';
      }
      buf_offset += num_size;
    }
  }
  buf[buf_sz-1] = '\0';
#else
  if (buf_sz) buf[0] = '\0';
#endif
  // Successful return
  return 0;
}
 
// SYMBOL "qrqp_print_iteration"
template<typename T1>
int casadi_qrqp_print_iteration(casadi_qrqp_data<T1>* d, char* buf, int buf_sz) {
#ifdef CASADI_SNPRINTF
  int flag;
  // Print iteration data without note to string
  flag = CASADI_SNPRINTF(buf, buf_sz,
    "%5d %5d %9.2g %9.2g %5d %9.2g %5d %9.2g %5d %9.2g  ",
    static_cast<int>(d->iter), static_cast<int>(d->sing), d->f, d->pr,
    static_cast<int>(d->ipr), d->du, static_cast<int>(d->idu),
    d->mina, static_cast<int>(d->imina), d->tau);
  // Check if error
  if (flag < 0) {
    d->status = QP_PRINTING_ERROR;
    return 1;
  }
  // Rest of buffer reserved for iteration note
  buf += flag;
  buf_sz -= flag;
  // Print iteration note, if any
  if (d->msg) {
    if (d->msg_ind > -2) {
      flag = CASADI_SNPRINTF(buf, buf_sz, "%s, i=%d", d->msg, static_cast<int>(d->msg_ind));
    } else {
      flag = CASADI_SNPRINTF(buf, buf_sz, "%s", d->msg);
    }
    // Check if error
    if (flag < 0) {
      d->status = QP_PRINTING_ERROR;
      return 1;
    }
  }
#else
  if (buf_sz) buf[0] = '\0';
#endif
  // Successful return
  return 0;
}