text: #define {PREFIX}_DEBUG_ALL_MODE_NO
  /** Second order cone program solver based on a primal barrier interior point
    * method.
    */ 
  
  #ifdef {PREFIX}_DEBUG_ALL_MODE
  #include "stdio.h"
  #endif
  
  #include <string.h>  /* TODO For? */
  
  #include <stdbool.h>
  #include "{prefix}mtxops.h"
  #include "{prefix}pbm.h"
  
  /* #include <{prefix}usefull.h> */  /* TODO implement {prefix}usefull.h */
  #include "{prefix}pbmsolve.h"
  
  /* static functions declaration */
  
  static void print_mtx(real_t *m, int r, int c) {{
      for (int i=0; i<r; i++) {{
          printf("\n%d ", i);
          for (int j=0; j<c; j++) {{
              printf("%.1e, ", m[i*c+j]);
          }}
      }}
      printf("\n");
      return;
  }}
    
  static bool {prefix}_pbm_is_close_to_zero(real_t n, real_t tol) {{
          if ((n < tol) && (n > -tol)) {{
              return true;
          }} else {{
              return false;
          }}
  }}
  
  static real_t {prefix}_pbm_get_scaling_factor(real_t *num, real_t *den, uint32_t nb_ineq) {{
      real_t v = 0.;
      real_t tol = 1e-99;  /* Tolerance for zero */
  
      for (uint32_t i=0; i<nb_ineq; i++) {{
          if ({prefix}_pbm_is_close_to_zero(num[i], tol)) {{
              continue;
          }}
          /* If true, you'll get a division by ~0, it should be avoided */
          if ({prefix}_pbm_is_close_to_zero(den[i], tol)) {{
              if (den[i] > 0) {{
                  den[i] = tol;
              }} else {{
                  den[i] = -tol;
              }}
              printf("WARNING: division by zero in scaling factor\n");
          }}
          v = (num[i]/den[i] > v) ? num[i]/den[i] : v;
      }}
      return v;
  }}
  
  static void {prefix}_pbm_form_h(const struct {prefix}_pbm *pbm) {{
      uint32_t ulen = pbm->optvar_seqlen;
      for (uint32_t i=0; i<ulen; i++) {{
          pbm->P_of_z->h[i] = -pbm->u_lb[i];
          pbm->P_of_z->h[ulen+i] = pbm->u_ub[i];
  
      }}
      uint32_t vlen = ((pbm->P_of_z->nb_lin_constr) - (2*ulen)) / 2;
      for (uint32_t i=0; i< vlen; i++) {{
          pbm->P_of_z->h[2*ulen + i] = -pbm->v_lb[i];
          pbm->P_of_z->h[2*ulen+vlen+i] = pbm->v_ub[i];
      }}
  }}
  
  static void {prefix}_pbm_get_valid(real_t *z_valid, real_t *tmp_otpseq_optseq,
                                     real_t *tmp_optseq,
                                     const real_t *P, const real_t *P_T, const real_t *h,
                                     const uint32_t optseq, const uint32_t nb_ineq,
                                     real_t *tmp3_optseq, real_t *tmp4_optseq,
                                     real_t *tmp1_optvec_optvec, real_t *tmp2_optvec_optvec,
                                     real_t *tmp5_nb_of_constr);
  
  static void form_d(
                  real_t *d, const real_t *P, const real_t *h, const real_t *z,
                  const uint32_t rowsP, const uint32_t colsP);
  
  static void form_dsoft(
                  real_t *dsoft, real_t *diag_d_soft, real_t *rd_soft,
                  real_t *Phi_soft,
                  real_t *tmp,
                  const real_t *roh, const real_t *z,
                  const real_t *Psoft, const real_t *Psoft_T, const real_t *hsoft,
                  const real_t *Fusoft, const real_t *Fxsoft, const real_t *Ffsoft,
                  const uint32_t rowsFusoft, const uint32_t control_veclen,
                  const uint32_t rowsFfsoft, const uint32_t state_veclen,
                  const uint32_t horizon, const struct {prefix}_pbm *pbm);
  
  static void form_diag_d_sq(
                  real_t *diag_d_sq, const real_t *d, const uint32_t dim);
  
  static void form_Phi(
                  real_t *Phi, real_t *help, real_t *tmp_Phi,
                  const struct {prefix}_pbm *pbm,
                  const real_t *H, const real_t *P_T, const real_t *P,
                  const struct {prefix}_pbm_P_hat *P_hat,
                  const real_t *d, const real_t *diag_d_sq,
                  const real_t kappa,
                  const uint32_t optvar_seqlen, const uint32_t nb_of_ueq_constr);
  
  static void {prefix}_residual_norm(
                  real_t *f,
                  const real_t *r_d, const real_t* r_p,
                  const uint32_t optvar_seqlen, const uint32_t dual_seqlen);
  
  static void {prefix}_bt_line_search_new(
                  real_t *good_step, const struct {prefix}_pbm *pbm);
  
  static void {prefix}_bt_line_search(
                  real_t *good_step, const struct {prefix}_pbm *pbm);
  
  static void {prefix}_pbm_warm_start(
                  const struct {prefix}_pbm *pbm);
  
  static void {prefix}_multiply_H_z(
                  real_t *product, const real_t *H, const real_t *z,
                  const struct {prefix}_pbm *pbm);
  
  static void {prefix}_multiply_P_z(
                  real_t *product, const real_t *P, const real_t *z,
                  const struct {prefix}_pbm *pbm);
  
  static void {prefix}_multiply_P_T_d(
                  real_t *product, const real_t *P_T, const real_t *z,
                  const struct {prefix}_pbm *pbm);
  
  static uint32_t {prefix}_better_step_size(
                  const struct {prefix}_pbm *pbm, real_t f1, real_t f2);
  
  /* external functions definition */
  
  void {prefix}_pbm_test_get_valid(const struct {prefix}_pbm *pbm)
  {{
      {prefix}_pbm_get_valid(pbm->z_opt, pbm->tmp4_mtx_optvar_optvar,
                             pbm->tmp1_optvar_seqlen, pbm->P, pbm->P_T,
                             pbm->h, pbm->optvar_seqlen,
                             pbm->nb_of_ueq_constr,
                             pbm->tmp2_optvar_seqlen, pbm->tmp6_optvar_seqlen,
                             pbm->tmp8_L_Phi, pbm->tmp9_L_Phi_T,
                             pbm->tmp5_nb_of_constr);
  }}
  
  /* Returns z_valid that satisfies: P*z_valid < h 
   * In general, this is done by solving the linear system P*z_valid = h - eps*h
   * Where eps is a small enough number.
   * The linear system is reformulated as:
   * P^T * (P * z_valid - h) = - P^T * eps*h 
   * 
   */
  void {prefix}_pbm_get_valid(real_t *z_valid, real_t *P_T_P, real_t *P_T_h,
                              const real_t *P, const real_t *P_T, const real_t *h,
                              const uint32_t optseq, const uint32_t nb_ineq,
                              real_t *tmp3_os, real_t *tmp4_os,
                              real_t *tmp1_ov_ov, real_t *tmp2_ov_ov,
                              real_t *tmp5_nb_c)
  {{
      uint32_t i;
      /* TODO tmp_os_x_os for sparse date or use block structure. */
      //real_t tmp1[HHMPC_OS*HHMPC_OS]; /* = tmp1_ov_ov; TODO */
      //real_t tmp2[HHMPC_OS*HHMPC_OS]; /* = tmp2_ov_ov; TODO */
      real_t *tmp1 = tmp1_ov_ov;
      real_t *tmp2 = tmp2_ov_ov;
      real_t *tmp3 = tmp3_os;
      real_t *z_delta = tmp4_os;
      real_t *tmp4 = tmp5_nb_c;
      real_t *tmp5 = tmp1;  /* We reuse tmp1, renamed for clarity */
  
      /* Get violation lines of the inequality constraints. 
         tmp4[i] = max(0, (P*z_valid - h)[i])
         if tmp4[i] == 0, tmp4[i] += eps  (this makes strictly feasible)
      */
      real_t eps = 0.0001;  /* TODO: compute according to problem */
      {prefix}_mtx_multiply_mtx_vec(tmp4, P, z_valid, nb_ineq, optseq);
      {prefix}_mtx_substract_direct(tmp4, h, nb_ineq, 1);
      for (i = 0; i < nb_ineq; i++) {{
          tmp4[i] = (tmp4[i] < 0.) ? 0. : tmp4[i] + eps;
      }}
      
      /* We want to find the change in z (z_delta) that violates the constrainst 
      to later substract it from z_valid. We have:
      P*z_delta = tpm4
      Solve the following equivalent system for z_delta:
      P^T * P * z_delta = P^T * tmp4 
  
      z_delta[i] == 0, if tpm4[i] == 0 (i.e. no change in z_valid)
      */
      {prefix}_mtx_multiply_mtx_vec(P_T_h, P_T, tmp4, optseq, nb_ineq);
      {prefix}_mtx_multiply_mtx_mtx(P_T_P, P_T, P, optseq, nb_ineq, optseq);
      for (i = 0; i < optseq; i++) {{
              P_T_P[i*optseq+i] += 0.0001;
      }}
      solveBlock(z_delta, tmp1, tmp2, P_T_P,
                  optseq, P_T_h, 1, tmp3);
  
      /* Finally compute dh = P*z_delta
       * and scale it by v = max((eps+h[i])/dh[i]) (for d[i]!=0)
       * to finally compute:
       * z_valid = z_valid - v*z_delta
       */
      {prefix}_mtx_multiply_mtx_vec(tmp5, P, z_delta, nb_ineq, optseq);
  
      real_t v = {prefix}_pbm_get_scaling_factor(tmp4, tmp5, nb_ineq);
  
      {prefix}_mtx_scale_direct(z_delta, v, optseq, 1);
      
      {prefix}_mtx_substract_direct(z_valid, z_delta, optseq, 1);
  }}
  
  /* Minimize MPC qp/qcqp/socp using primal barrier interior method */
  void {prefix}_pbm_solve_problem(const struct {prefix}_pbm *pbm)
  {{
      uint32_t i, j;
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      real_t *t_solve_dual_seqlen = pbm->tmp2_dual_seqlen;
      real_t *t_L_Y = pbm->tmp8_L_Y;
      real_t *t_L_Y_T = pbm->tmp9_L_Y_T;
      real_t *eye_nm = pbm->eye_optvar_veclen;
      real_t *eye_n = pbm->eye_state_veclen;
      real_t f;
  #if {PREFIX}_PBM_PRB_COND
      real_t delta_to_zero = 0.01 /* pos */, delta_to_zero_lin = 0.05;
  #endif
  /*//     printf("g = ");
  //     print_mtx(pbm->g, 5, 1);
  //     printf("H = ");
  //     print_mtx(pbm->H, 5, 5);*/
  #if {PREFIX}_PBM_PRB_COND/*
  //     for (i = 0; i < 5; i++){{
  //         for (j = 0; j < 5; j++){{
  //             pbm->H[i*5+j] *= 500.;
  //         }}
  //     }}*//* TODO Daten korrigieren? */
      for (i = 0; i < pbm->optvar_seqlen; i++){{
              pbm->g[i] *= 2.;
      }}
  #endif
  /*     printf(" inner step %d ", inner); */
      /*Check if initial value is valid*/
  /*     printf("%d\n", {prefix}_pbm_check_valid(pbm, pbm->z_ini)); */
      
      /*Take initial value*/
      memcpy(pbm->z_opt, pbm->z_ini, pbm->sizeof_optvar_seqlen);
      memcpy(pbm->v_opt, pbm->v_ini, pbm->sizeof_dual_seqlen);
      
      /* Calculate Kappa in every time_step */
      {prefix}_pbm_calc_kappa(pbm->kappa, pbm, pbm->z_opt);
  #ifdef {PREFIX}_DEBUG_ALL_MODE
      printf("calculated kappa = %.20f\n", pbm->kappa[0]);
  #endif    
        
      {prefix}_pbm_form_h(pbm);
      memcpy(pbm->P_of_z->h_hat, pbm->P_of_z->h, sizeof(real_t) * pbm->P_of_z->nb_lin_constr);
  #if 0
  #ifdef HHMPC_SOCPCONDTEST    
  /*     {prefix}_pbm_get_valid_trick(pbm);*/  /* Trick we used before */
  #ifdef HHMPC_SOCPCONDTEST5
      {prefix}_pbm_get_positiv(pbm, &delta_to_zero);  /* Only ensure, that c*z +d > 0 */
  #endif
  #ifndef HHMPC_SOCPCONDTEST5
      {prefix}_pbm_get_valid_lin_constr(pbm, &delta_to_zero_lin);
  #endif
  #endif
  #endif/*
  
  #ifdef HHMPC_SOCPCONDTEST    */
      /*
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
      {prefix}_pbm_get_positiv(pbm, &delta_to_zero);
      */
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
      {prefix}_pbm_get_valid(pbm->z_opt, pbm->tmp4_mtx_optvar_optvar,
                             pbm->tmp1_optvar_seqlen, pbm->P, pbm->P_T,
                             pbm->h, pbm->optvar_seqlen,
                             pbm->nb_of_ueq_constr,
                             pbm->tmp2_optvar_seqlen, pbm->tmp6_optvar_seqlen,
                             pbm->tmp8_L_Phi, pbm->tmp9_L_Phi_T,
                             pbm->tmp5_nb_of_constr);/*
  #endif*/
  /*     print_mtx(pbm->h, 1, pbm->nb_of_ueq_constr); */
      /*Improve z for a fixed number of steps j_in*/
      for (i = 0; i < *(pbm->j_in); i++) {{  /*  TODO here error, compiling for MC */
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
          /*Check if initial value is valid*/
          if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
              printf("%d return, j = %d", {prefix}_pbm_check_valid(pbm, pbm->z_opt), i);
  /*//             while ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
  //                 pbm->z_opt[{prefix}_pbm_check_valid(pbm, pbm->z_opt)] *= 1.1;
  //                 {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
  //                        t_solve_optvar_seqlen, t_optvar_seqlen);
  //             }}
  //             uint32_t z = {prefix}_pbm_check_valid(pbm, pbm->z_opt);
  //             if (pbm->z_opt[z]>0){{
  //                 pbm->z_opt[z] = 0.04;
  //                 pbm->z_ini[z] = 0.04; 
  //             }}
  //             if (pbm->z_opt[z]<0){{
  //                 pbm->z_opt[z] = 0.04;
  //                 pbm->z_ini[z] = 0.04; 
  //             }}
  //             real_t h_low[5];
  //             real_t z_low[5];
  //             for (j = 0; j < 5; j++){{
  //                 h_low[j] = pbm->h[j+pbm->P_of_z->nb_lin_constr] - 0.001;
  //             }}
  //             fwd_subst(z_low, pbm->P+5*pbm->P_of_z->nb_lin_constr, 5, h_low, 1);
  // //             print_mtx(z_low, 5, 1);
  //             printf("%d return", {prefix}_pbm_check_valid(pbm, z_low));
  //             HIER
  //             return;*/
          }}
  /*         printf("%d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt)); */
          {prefix}_form_pbm_matrices(pbm->d, pbm->diag_d_sq, pbm->dsoft,
                                     pbm->diag_d_soft, pbm->Phi, pbm);
          
          /* Calculate the residual */
          residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
          {prefix}_residual_norm(&f, pbm->r_d, pbm->r_p, pbm->optvar_seqlen, pbm->dual_seqlen);
  /*//         print_mtx(pbm->r_p, pbm->dual_seqlen, 1);
  //         printf("res_norm = %f\n", f);
  //         print_mtx(pbm->Phi, pbm->optvar_seqlen, pbm->optvar_seqlen); */
          /* Solve system of linear equations to obtain the step direction */
          
          /* regularization (+epsilon*I) */
          for (j = 0; j < pbm->optvar_seqlen; j++){{
              pbm->Phi[j*pbm->optvar_seqlen+j] += pbm->reg[0];
          }}
          solve_sysofleq(pbm->delta_z, pbm->delta_v, pbm->Phi, pbm->r_d, pbm->r_p,
                         pbm->C, pbm->C_T, pbm->A, pbm->A_T, pbm->B, pbm->B_T,
                         pbm->A_B, pbm->A_B_T, pbm->H,
                         pbm->state_veclen, pbm->optvar_veclen-pbm->state_veclen, /* TODO m einführen */
                         pbm->horizon,
                         eye_nm, eye_n,
                         t_solve_optvar_seqlen,
                         pbm->tmp2_optvar_seqlen,
                         t_solve_dual_seqlen,
                         t_L_Y, t_L_Y_T,
                         pbm->tmp_phibl1, pbm->tmp_phibl2,
                         pbm->tmp_phibl3,
                         pbm->tmpQbl, pbm->tmpYbl,
                         pbm->tmp10,
                         pbm->tmp8_L_Phi, pbm->tmp9_L_Phi_T);
          /* undo regularization (-epsilon*I) */
          for (j = 0; j < pbm->optvar_seqlen; j++){{
              pbm->Phi[j*pbm->optvar_seqlen+j] -= pbm->reg[0];
          }}
  
  /*         print_mtx(pbm->delta_z, pbm->optvar_seqlen, 1); */
          {prefix}_pbm_iterative_refinement(pbm);
  
          /* Find best step size (0...1] */
  /*         real_t test = 0.; */
           {prefix}_bt_line_search_new(pbm->st_size, pbm);
  /*//         test = pbm->st_size[0];
  //         printf("new st_size = %.8f\n", pbm->st_size[0]);
  //         bt_line_search(pbm->st_size, pbm);
  //         if(smpl_abs(test-pbm->st_size[0]) > 0.00){{
  //             HIER
  //             printf("new st_size = %.8f\n", test);
  //             printf("old st_size = %.8f\n", pbm->st_size[0]);
  //         }}
  //         pbm->st_size[0] = test;
  //         printf("old st_size = %.8f\n", pbm->st_size[0]); */
          
          /* Update z */
          {prefix}_mtx_scale_direct(pbm->delta_z, pbm->st_size[0],
                                  pbm->optvar_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->z_opt, pbm->delta_z,
                                pbm->optvar_seqlen, 1);
          {prefix}_mtx_scale_direct(pbm->delta_v, pbm->st_size[0],
                                  pbm->dual_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->v_opt, pbm->delta_v,
                                pbm->dual_seqlen, 1);
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
             t_solve_optvar_seqlen, t_optvar_seqlen);
          if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
              printf("No valid step possible: return");
              return;
          }}
  /*//         print_mtx(pbm->delta_z, pbm->optvar_seqlen, 1);
  //         print_mtx(pbm->z_opt, pbm->optvar_veclen, 1);
  //         print_mtx(pbm->v_opt, pbm->dual_seqlen, 1); */
          if (f <= 1e-12){{
  /*//             printf("break, res_norm = %f\n", f);
  //             break; */
          }}
  /*         pbm->kappa[0] *= 0.8; */
      }}
  /*     pbm->kappa[0] = 95.; */
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
             t_solve_optvar_seqlen, t_optvar_seqlen);
      form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt,
             pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
      form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
      residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
      {prefix}_residual_norm(&f, pbm->r_d, pbm->r_p, pbm->optvar_seqlen, pbm->dual_seqlen);
     /* printf("res_norm = %.11f\n", f);*/
      /* Update x_k (und andere Parameter) */
      if (pbm->conf->warm_start) {{
          {prefix}_pbm_warm_start(pbm);
      }}
  /*     print_mtx(pbm->z_opt, pbm->optvar_veclen, 1);
  //     print_mtx(pbm->r_p, pbm->state_veclen, 1); */
      /*
      memcpy(pbm->z_ini, pbm->z_opt, pbm->sizeof_optvar_seqlen);
      memcpy(pbm->v_ini, pbm->v_opt, pbm->sizeof_dual_seqlen);*/
  }}
  
  /* TODO Describe external function definitions */
  uint32_t {prefix}_pbm_check_positiv(
                  const struct {prefix}_pbm *pbm, const real_t *z_check)
  {{
      uint32_t i;
      real_t *tmp_os = pbm->tmp1_optvar_seqlen;
      
      for (i = 0; i < pbm->P_of_z->nb_socc; i++){{
          {prefix}_mtx_multiply_mtx_vec(tmp_os, pbm->P_of_z->socc[i]->c, z_check,
                                      1, pbm->optvar_seqlen);
          if (tmp_os[0] + pbm->P_of_z->socc[i]->d[0] < 0){{
              /*
              printf("c = %f %f, d = %f \n", pbm->P_of_z->socc[i]->c[0], pbm->P_of_z->socc[i]->c[1], pbm->P_of_z->socc[i]->d[0]);
              printf("A = %f %f %f %f, b = %f %f\n", pbm->P_of_z->socc[i]->A[0], pbm->P_of_z->socc[i]->A[1],pbm->P_of_z->socc[i]->A[2], pbm->P_of_z->socc[i]->A[3], pbm->P_of_z->socc[i]->b[0], pbm->P_of_z->socc[i]->b[1]);
              */
              return i;    
          }}
      }}
      
      return -1;
  }}
  
  uint32_t {prefix}_pbm_check_valid(
                  const struct {prefix}_pbm *pbm, const real_t *z_check)
  {{
      uint32_t i;
      real_t *help1 = pbm->tmp5_nb_of_constr;
      
      {prefix}_multiply_P_z(help1, pbm->P, z_check, pbm);
  
      {prefix}_mtx_substract_direct(help1, pbm->h, pbm->nb_of_ueq_constr, 1);
  /*//     print_mtx(help1, pbm->nb_of_ueq_constr, 1);
  //     printf("check:\n");*/
  #ifdef HHMPC_QPTEST
      for (i = 0; i < 2; i++){{
  /*         printf("%f\n", help1[i]); */
          if (help1[i] > 0) {{return i;}}
      }}
      for (i = 3; i < pbm->nb_of_ueq_constr; i++){{
  /*         printf("%f\n", help1[i]); */
          if (help1[i] > 0) {{return i;}}
      }}
  #endif
  #ifdef HHMPC_QPSMALLTEST
      for (i = 0; i < 2; i++){{
  /*         printf("%f\n", help1[i]); */
          if (help1[i] >= 0.) {{return i;}}
      }}
  /* //         printf("%f\n", help1[i]);
  //     if (help1[i] > 0.) {{
  //         print_mtx(help1, pbm->nb_of_ueq_constr, 1);
  //         return i;
  //     }} */
      for (i = 3; i < pbm->nb_of_ueq_constr; i++){{
  /*         printf("%f\n", help1[i]); */
          if (help1[i] >= 0.) {{return i;}}
      }}
  #endif
  #if {PREFIX}_PBM_PRB_COND
  
      /*
  //     
  //     print_mtx(pbm->P, pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
  //     print_mtx(pbm->h, 1, pbm->nb_of_ueq_constr);
  //     
  //     print_mtx(pbm->z_opt, 1, pbm->optvar_seqlen);
  // 
  //     real_t help01[pbm->P_of_z->nb_socc];
  //     real_t help02[pbm->P_of_z->nb_socc];
  //     for (i = 0; i < pbm->P_of_z->nb_socc; i++){{
  // //         printf("%d\n", i); 
  //         {prefix}_mtx_multiply_mtx_vec(help01, pbm->P_of_z->socc[i]->A, pbm->z_opt, pbm->P_of_z->nb_socc, 5);
  //         {prefix}_mtx_add_direct(help01, pbm->P_of_z->socc[i]->b, pbm->P_of_z->nb_socc, 1);
  //         {prefix}_mtx_multiply_mtx_vec(help02, help01, help01, 1, pbm->P_of_z->nb_socc);
  // //         printf("Au+b = %f", help02[0]);
  // //         print_mtx(help01, 5, 1);
  //         {prefix}_mtx_multiply_mtx_vec(help01, pbm->P_of_z->socc[i]->c, pbm->z_opt, 1, pbm->P_of_z->nb_socc);
  //         {prefix}_mtx_add_direct(help01, pbm->P_of_z->socc[i]->d, pbm->P_of_z->nb_socc, 1);
  // //         printf("cu+d = %f \n", help01[0]);*/
  /*         if (help01[0] < help02[0]) {{*//*print_mtx(help1, pbm->nb_of_ueq_constr, 1); printf("%d\n", i);*/ /*return i;}}*/
  /*// //         print_mtx(help01, 5, 1);
  //     }} */
      if ({prefix}_pbm_check_positiv(pbm, z_check) + 1){{
          return pbm->nb_of_ueq_constr;
      }}else{{
          for (i = 0; i < pbm->nb_of_ueq_constr; i++){{/*
              printf("%f\n", help1[i]);*/
              if (help1[i] >= 0.0) {{return i;}}
          }}
      }}
  #endif
  #ifdef HHMPC_SOCPTEST
      for (i = 0; i < pbm->nb_of_ueq_constr; i++){{
  //         printf("%f\n", help1[i]);
          if (help1[i] >= 0.) {{return i;}}
      }}
  #endif
  #ifdef HHMPC_SOCPSOFTTEST
      for (i = 0; i < pbm->nb_of_ueq_constr; i++){{
  //         printf("%f\n", help1[i]);
          if (help1[i] >= 0.) {{return i;}}
      }}
  #endif
  #ifdef HHMPC_SOCPHARDTEST
      for (i = 0; i < 2; i++){{
  //         printf("%f\n", help1[i]);
          if (help1[i] > 0.) {{return i;}}
      }}
      for (i = 3; i < pbm->nb_of_ueq_constr; i++){{
  //         printf("%f\n", help1[i]);
          if (help1[i] > 0.) {{return i;}}
      }}
  #endif
      return -1;
  }}
  
  /* Ensure, that c*z +d > 0. So we can use generalized inequalities*/
  void {prefix}_pbm_get_positiv(const struct {prefix}_pbm *pbm, real_t *delta)
  {{
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      uint32_t i, k;
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
          t_solve_optvar_seqlen, t_optvar_seqlen);
      
      if ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1){{
  /*//         print_mtx(pbm->z_opt, 5, 1);
  //         printf("corrected pos z_opt[0] %d\n", {prefix}_pbm_check_positiv(pbm, pbm->z_opt));*/
          for (k = 0; k < pbm->P_of_z->nb_socc; k++){{
              if (k == {prefix}_pbm_check_positiv(pbm, pbm->z_opt)){{
                  pbm->z_opt[k] = delta[0] - pbm->P_of_z->socc[k]->d[0];
                  for (i = 0; i < k; i++){{
                      pbm->z_opt[k] -= pbm->P_of_z->socc[k]->c[i]*pbm->z_opt[i];
                  }}
                  pbm->z_opt[k] /= pbm->P_of_z->socc[k]->c[k];
              }}
          }}
  /*//         print_mtx(pbm->z_opt, 5, 1);
  // //         if ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1){{
  // //             printf("not corrected pos z_opt[0] enough %d\n", {prefix}_pbm_check_positiv(pbm, pbm->z_opt));
  // //             return;
  // //         }}*/
      }}
  }}
  
  /* Ensure, that P*z > h */
  void {prefix}_pbm_get_valid_lin_constr(
                  const struct {prefix}_pbm *pbm, real_t *delta)
  {{
      /* P für condensiertes Problem hat hinsichtlich der Zeilen, die für die
       * Einhaltung der UNB für die Zustände verantwortlich sind Dreiecksgestalt 
       * Deswegen hier jede zweite Zeile der zweiten Hälfte von P (upper bounds)
       * verwenden, um sicherzustellen, dass z valide ist */
      
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      uint32_t i, k;
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
          t_solve_optvar_seqlen, t_optvar_seqlen);
  /*//     printf("1corrected pos z_opt[0] %d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt));
  //     pbm->z_opt[0] = pbm->P_of_z->h[60] - delta[0];
  //     pbm->z_opt[0] /= pbm->P_of_z->P[60*HHMPC_OS];
  //     printf("2corrected pos z_opt[0] %d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt));
  //     delta[0] = (pbm->P_of_z->h[pbm->P_of_z->nb_lin_constr/2] + pbm->P_of_z->h[0])/2.;*/
  
      if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
  /*         print_mtx(pbm->z_opt, 30, 1); */
          printf("corrected valid z_opt[0] %d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt));
          for (k = 0; k < 1; k++){{
  /*             if (pbm->P_of_z->nb_lin_constr/2 + 2*k == {prefix}_pbm_check_valid(pbm, pbm->z_opt)){{ */
                  pbm->z_opt[k] = pbm->P_of_z->h[pbm->P_of_z->nb_lin_constr/2 + 2*k] - delta[0];
                  for (i = 0; i < k; i = i+2){{
                      pbm->z_opt[k] -= pbm->P_of_z->P[(pbm->P_of_z->nb_lin_constr/2 + 2*k)*pbm->optvar_seqlen + i]*pbm->z_opt[i];
                  }}
                  pbm->z_opt[k] /= pbm->P_of_z->P[(pbm->P_of_z->nb_lin_constr/2 + 2*k)*pbm->optvar_seqlen + k];
  /*             }} */
          }}
      }}
      if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
  /*         print_mtx(pbm->z_opt, 30, 1); */
          printf("corrected valid z_opt[0] %d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt));
          for (k = 0; k < pbm->optvar_seqlen; k++){{
  /*             if ((pbm->P_of_z->nb_lin_constr/2 + 2*k) == {prefix}_pbm_check_valid(pbm, pbm->z_opt)){{ */
                  pbm->z_opt[k] = pbm->P_of_z->h[pbm->P_of_z->nb_lin_constr/2 + 2*k] - delta[0];
                  for (i = 0; i < k; i = i+2){{
                      pbm->z_opt[k] -= pbm->P_of_z->P[(pbm->P_of_z->nb_lin_constr/2 + 2*k)*pbm->optvar_seqlen + i]*pbm->z_opt[i];
                  }}
                  pbm->z_opt[k] /= pbm->P_of_z->P[(pbm->P_of_z->nb_lin_constr/2 + 2*k)*pbm->optvar_seqlen + k];
  /*             }} */
          }}
      
  /*//         print_mtx(pbm->z_opt, 30, 1);
  // //         if ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1){{
  // //             printf("not corrected pos z_opt[0] enough %d\n", {prefix}_pbm_check_positiv(pbm, pbm->z_opt));
  // //             return;
  // //         }} */
      
      }}
  }}
  
  void {prefix}_pbm_get_valid_trick(const struct {prefix}_pbm* pbm)
  {{
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      
      
  /*// //     i = 1;
  // //     {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
  // //             t_solve_optvar_seqlen, t_optvar_seqlen);
  // //     printf("%d\n", {prefix}_pbm_check_valid(pbm, pbm->z_opt)+1);
  // //     while ({prefix}_pbm_check_valid(pbm, pbm->z_opt) >= pbm->P_of_z->nb_lin_constr){{
  // // //         if (pbm->P_of_z->socc[0]->c[0] * pbm->z_opt[0] <
  // // //                 (0.002*i*i - pbm->P_of_z->socc[0]->d[0])){{
  // //             printf("corrected %d z_opt[0]\n", i);
  // //             pbm->z_opt[0] = 
  // //                 (0.001*i*i - pbm->P_of_z->socc[0]->d[0]) / pbm->P_of_z->socc[0]->c[0];
  // //             print_mtx(pbm->z_opt, 5, 1);
  // // //         }}
  // //         {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
  // //             t_solve_optvar_seqlen, t_optvar_seqlen);
  // //         i++;
  // //     }} */
      
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
              t_solve_optvar_seqlen, t_optvar_seqlen);
          if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
          
  /*//             if ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1){{
  //                 print_mtx(pbm->z_opt, 5, 1);
  //                 printf("corrected pos z_opt[0]\n");
  //                 pbm->z_opt[0] = (-1 * pbm->P_of_z->socc[0]->d[0]) / pbm->P_of_z->socc[0]->c[0];
  //                 print_mtx(pbm->z_opt, 5, 1);
  //             }}
  //             real_t tmp[5];
  //             print_mtx(pbm->P_of_z->socc[4]->A, 5, 5);
  //             print_mtx(pbm->P_of_z->socc[4]->b, 1, 5);
  //             {prefix}_mtx_multiply_mtx_vec(tmp, pbm->P_of_z->socc[4]->A, pbm->z_opt, 5, 5);
  //             print_mtx(tmp, 5, 1);
  //             {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
  //                t_solve_optvar_seqlen, t_optvar_seqlen);
  // //             print_mtx(pbm->P, pbm->nb_of_ueq_constr, 5);
  //             printf("%f\n", pbm->P[pbm->P_of_z->nb_lin_constr*pbm->optvar_seqlen]);
  //             pbm->z_opt[0] = (pbm->h[pbm->P_of_z->nb_lin_constr] - 0.0001) /
  //                             pbm->P[pbm->P_of_z->nb_lin_constr*pbm->optvar_seqlen];
  //             print_mtx(pbm->z_opt, 5, 1); */
                              
              if (pbm->P_of_z->socc[0]->c[0] * pbm->z_opt[0] <
                      (0.01 - pbm->P_of_z->socc[0]->d[0])){{
                  /* printf("corrected 1 z_opt[0]\n"); */
                  pbm->z_opt[0] = 
                      (0.01 - pbm->P_of_z->socc[0]->d[0]) / pbm->P_of_z->socc[0]->c[0];
                  /* print_mtx(pbm->z_opt, 5, 1); */           
              }}
          }}
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
              t_solve_optvar_seqlen, t_optvar_seqlen);
          if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
              if (pbm->P_of_z->socc[0]->c[0] * pbm->z_opt[0] <
                      (0.05 - pbm->P_of_z->socc[0]->d[0])){{
                  /* printf("corrected 2 z_opt[0]\n"); */
                  pbm->z_opt[0] = 
                      (0.05 - pbm->P_of_z->socc[0]->d[0]) / pbm->P_of_z->socc[0]->c[0];
                  /* print_mtx(pbm->z_opt, 5, 1); */            
              }}
          }}
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
              t_solve_optvar_seqlen, t_optvar_seqlen);
          if ({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1){{
              if (pbm->P_of_z->socc[0]->c[0] * pbm->z_opt[0] <
                      (0.2 - pbm->P_of_z->socc[0]->d[0])){{
                  printf("corrected 2 z_opt[0]\n");
                  pbm->z_opt[0] = 
                      (0.2 - pbm->P_of_z->socc[0]->d[0]) / pbm->P_of_z->socc[0]->c[0];
                  /* print_mtx(pbm->z_opt, 5, 1); */           
              }}
          }}
  }}
  
  void {prefix}_pbm_iterative_refinement(const struct {prefix}_pbm *pbm)
  {{
  #if {PREFIX}_PBM_PRB_SPARSE
      real_t *delta_rd = pbm->tmp1_optvar_seqlen;
      real_t *delta_rp = pbm->tmp2_dual_seqlen;
      real_t *delta_delta_v = pbm->tmp7_dual_seqlen;
      real_t *delta_delta_z = pbm->tmp4_mtx_optvar_optvar+pbm->dual_seqlen;
      real_t *tmp1 = pbm->tmp2_optvar_seqlen;
      real_t *tmp2 = pbm->tmp6_optvar_seqlen;
      real_t *tmp3 = pbm->tmp3_state_veclen;
      real_t *tmp4 = pbm->tmp4_mtx_optvar_optvar;
      
      real_t *L_Phi_blocks = pbm->tmp8_L_Phi;
      real_t *L_Phi_T_blocks = pbm->tmp9_L_Phi_T;
      real_t *L_Y = pbm->tmp8_L_Y;
      real_t *L_Y_T = pbm->tmp9_L_Y_T;
  
      {prefix}_multiply_C_T_v(tmp1, pbm->C_T, pbm->delta_v,
                              pbm->state_veclen, pbm->control_veclen, pbm->horizon);
  
      {prefix}_multiply_H_z(delta_rd, pbm->Phi, pbm->delta_z, pbm);
  
      {prefix}_mtx_add_direct(delta_rd, tmp1, pbm->optvar_seqlen, 1);
      {prefix}_mtx_add_direct(delta_rd, pbm->r_d, pbm->optvar_seqlen, 1);
      {prefix}_mtx_scale_direct(delta_rd, -1., pbm->optvar_seqlen, 1);
  
      {prefix}_multiply_C_z(delta_rp, pbm->C, pbm->delta_z,
                            pbm->state_veclen, pbm->control_veclen, pbm->horizon);
  
      {prefix}_mtx_add_direct(delta_rp, pbm->r_p, pbm->dual_seqlen, 1);
      {prefix}_mtx_scale_direct(delta_rp, -1., pbm->dual_seqlen, 1);
     
  /*     print_mtx(delta_rp, pbm->dual_seqlen, 1); */
          
      form_beta(delta_delta_v, L_Phi_blocks, L_Phi_T_blocks, delta_rd, delta_rp,
                pbm->horizon, pbm->C,
                pbm->state_veclen, pbm->optvar_veclen-pbm->state_veclen,
                tmp1, tmp2);
      form_delta_v(delta_delta_v, tmp4, tmp3, 
                   L_Y, L_Y_T, pbm->horizon, pbm->state_veclen);
      form_delta_z(delta_delta_z, tmp1, delta_delta_v,
                   L_Phi_blocks, L_Phi_T_blocks, delta_rd, pbm->C_T, pbm->horizon,
                   pbm->state_veclen, pbm->optvar_veclen-pbm->state_veclen);
      
      
  /*     print_mtx(delta_delta_z, pbm->optvar_seqlen, 1); */
      {prefix}_mtx_substract_direct(pbm->delta_z, delta_delta_z, pbm->optvar_seqlen, 1);
      {prefix}_mtx_substract_direct(pbm->delta_v, delta_delta_v, pbm->dual_seqlen, 1);
  /*     print_mtx(pbm->delta_z, pbm->optvar_seqlen, 1); */
  #endif
  }}
  
  void {prefix}_pbm_update_P(struct {prefix}_pbm_P_hat *P,
                             const uint32_t optvar_seqlen,
                             real_t *tmp1, real_t *tmp2)
  {{
      uint32_t i;
      /* Update matrix P(z) */
      struct {prefix}_pbm_qc *qc_i;
      struct {prefix}_pbm_socc *socc_i;
      /*  TODO P vorher 0 setzen */
      memcpy(P->P_hat, P->P, sizeof(real_t) * P->nb_lin_constr*optvar_seqlen);
      P->P_hat += P->nb_lin_constr*optvar_seqlen;  /* Pointer wird nicht nur lokal verändern */
      
      /* Determine rows for qc */
      for (i = 0; i < P->nb_qc; i++){{
          qc_i = P->qc[i];
          {prefix}_mtx_multiply_mtx_mtx(P->P_hat+qc_i->par_0,
                                      qc_i->par, qc_i->Gamma,
                                      1, qc_i->dimGamma, qc_i->dimGamma);
          {prefix}_mtx_add_direct(P->P_hat+qc_i->par_0,
                                qc_i->beta, 1, qc_i->dimGamma);
          P->P_hat += optvar_seqlen;
      }}
      /* Determine rows for socc */
      for (i = 0; i < P->nb_socc; i++){{
          socc_i = P->socc[i];
          {prefix}_mtx_multiply_mtx_vec(tmp1, socc_i->A, socc_i->par,
                                      socc_i->rowsA, socc_i->colsA);
          {prefix}_mtx_add_direct(tmp1, socc_i->b, 1, socc_i->rowsA);
          {prefix}_mtx_add_direct(tmp1, socc_i->b, 1, socc_i->rowsA);
  
          {prefix}_mtx_multiply_mtx_mtx(P->P_hat+socc_i->par_0,
                                      tmp1, socc_i->A,
                                      1, socc_i->rowsA, socc_i->colsA);
  /*         print_mtx(P->P_hat+socc_i->par_0, 1, socc_i->colsA); */
          {prefix}_mtx_multiply_mtx_vec(tmp1, socc_i->c, socc_i->par,
                                      1, socc_i->colsA);
          tmp1[0] += 2*socc_i->d[0];
          {prefix}_mtx_scale(tmp2, socc_i->c, tmp1[0], socc_i->colsA, 1);
          {prefix}_mtx_substract_direct(P->P_hat+socc_i->par_0,
                                      tmp2, 1, socc_i->colsA);
          P->P_hat += optvar_seqlen;
      }}
  
      P->P_hat -= (P->nb_lin_constr+P->nb_qc + P->nb_socc)*optvar_seqlen;
      {prefix}_mtx_transpose(P->P_hat_T, P->P_hat,
                           P->nb_lin_constr + P->nb_qc + P->nb_socc,
                           optvar_seqlen);
      
  /**************************/
      /* Update matrix P(2*z) */
      memcpy(P->P2_hat, P->P, sizeof(real_t) * P->nb_lin_constr*optvar_seqlen);
      P->P2_hat += P->nb_lin_constr*optvar_seqlen;  /* Pointer wird nicht nur lokal verändern */
      
      /* Determine rows for qc */
      for (i = 0; i < P->nb_qc; i++){{
          qc_i = P->qc[i];
          {prefix}_mtx_multiply_mtx_mtx(P->P2_hat+qc_i->par_0,
                                      qc_i->par, qc_i->Gamma,
                                      1, qc_i->dimGamma, qc_i->dimGamma);
          /* 2*z */
          {prefix}_mtx_scale_direct(P->P2_hat+qc_i->par_0, 2., 1, qc_i->dimGamma);
          {prefix}_mtx_add_direct(P->P2_hat+qc_i->par_0,
                                qc_i->beta, 1, qc_i->dimGamma);
          P->P2_hat += optvar_seqlen;
      }}
      /* Determine rows for socc */
      for (i = 0; i < P->nb_socc; i++){{
          socc_i = P->socc[i];
          {prefix}_mtx_multiply_mtx_vec(tmp1, socc_i->A, socc_i->par,
                                      socc_i->rowsA, socc_i->colsA);
          /* 2*z */
          {prefix}_mtx_scale_direct(tmp1, 2., 1, socc_i->rowsA);
          {prefix}_mtx_add_direct(tmp1, socc_i->b, 1, socc_i->rowsA);
          {prefix}_mtx_add_direct(tmp1, socc_i->b, 1, socc_i->rowsA);
          {prefix}_mtx_multiply_mtx_mtx(P->P2_hat+socc_i->par_0,
                                      tmp1, socc_i->A,
                                      1, socc_i->rowsA, socc_i->colsA);
          {prefix}_mtx_multiply_mtx_vec(tmp1, socc_i->c, socc_i->par,
                                      1, socc_i->colsA);
          /* 2*z */
          tmp1[0] *= 2.;
          tmp1[0] += 2*socc_i->d[0];
          {prefix}_mtx_scale(tmp2, socc_i->c, tmp1[0], socc_i->colsA, 1);
          {prefix}_mtx_substract_direct(P->P2_hat+socc_i->par_0,
                                      tmp2, 1, socc_i->colsA);
          P->P2_hat += optvar_seqlen;
      }}
      
      P->P2_hat -= (P->nb_lin_constr+P->nb_qc + P->nb_socc)*optvar_seqlen;
      {prefix}_mtx_transpose(P->P2_hat_T, P->P2_hat,
                           P->nb_lin_constr + P->nb_qc + P->nb_socc,
                           optvar_seqlen);
  }}
  
  /* Calculate the residum r = (r_d, r_p) */
  void residual(const struct {prefix}_pbm *pbm,
                const real_t *z, const real_t *v, const real_t *d,
                const real_t kappa)
  {{
      real_t *tmp1_os = pbm->tmp1_res_os;
      real_t *tmp2_os = pbm->tmp2_res_os;
      real_t *tmp3_ds = pbm->tmp3_res_ds;
      /* Term kappa*P^T*d */  
      {prefix}_multiply_P_T_d(tmp1_os, pbm->P2_T, d, pbm);
  
      {prefix}_mtx_scale(pbm->r_d, tmp1_os, kappa, pbm->optvar_seqlen, 1);
      /* Add term C^T*v */
      {prefix}_multiply_C_T_v(tmp1_os, pbm->C_T, v,
                              pbm->state_veclen, pbm->control_veclen, pbm->horizon);
  
      {prefix}_mtx_add_direct(pbm->r_d, tmp1_os, pbm->optvar_seqlen, 1);
      /* Add term g */
      {prefix}_mtx_add_direct(pbm->r_d, pbm->g,
                            pbm->optvar_seqlen, 1);
      /* Add term 2*H*(z-z_ref) */
      /* Calculate distance to the reference (z-z_ref) */
      {prefix}_mtx_substract(tmp2_os, z, pbm->zref, pbm->optvar_seqlen, 1);
  
      {prefix}_multiply_H_z(tmp1_os, pbm->H, tmp2_os, pbm);
      
      {prefix}_mtx_scale_direct(tmp1_os, 2., pbm->optvar_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->r_d, tmp1_os, pbm->optvar_seqlen, 1);
      
      {prefix}_mtx_add_direct(pbm->r_d, pbm->r_d_soft, pbm->optvar_seqlen, 1);
  
      {prefix}_multiply_C_z(pbm->r_p, pbm->C, z,
                            pbm->state_veclen, pbm->control_veclen, pbm->horizon);
  
      {prefix}_mtx_substract_direct(pbm->r_p, pbm->b, pbm->dual_seqlen, 1);
  }}
  
  void {prefix}_form_pbm_matrices(real_t *d, real_t *diag_d_sq,
                                  real_t *dsoft, real_t *diag_d_soft,
                                  real_t *Phi,
                                  const struct {prefix}_pbm *pbm)
  {{
      form_d(d, pbm->P, pbm->h, pbm->z_opt,
             pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
  
      form_dsoft(dsoft, diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                 pbm->tmp3_mtx_optvar_nb_of_soft,
                 pbm->roh, pbm->z_opt, 
                 pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                 pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                 pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                 pbm->state_veclen, pbm->horizon, pbm);
      
      form_diag_d_sq(diag_d_sq, d, pbm->nb_of_ueq_constr);
      
      form_Phi(Phi, pbm->tmp3_mtx_optvar_nb_of_ueq, pbm->tmp4_mtx_optvar_optvar,
               pbm, pbm->H,
               pbm->P2_T, pbm->P2, pbm->P_of_z , pbm->d, pbm->diag_d_sq,
               pbm->kappa[0], pbm->optvar_seqlen, pbm->nb_of_ueq_constr);
  }}
  
  void form_Phi(real_t *Phi, real_t *help, real_t *t_Phi,
                const struct {prefix}_pbm *pbm,
                const real_t *H, const real_t *P_T, const real_t *P,
                const struct {prefix}_pbm_P_hat *P_hat,
                const real_t *d, const real_t *diag_d_sq,
                const real_t kappa,
                const uint32_t optvar, const uint32_t nb_of_ueq)
  {{    
      uint32_t i, j, k, pos_d, pos_Phi;
      struct {prefix}_pbm_qc *qc_i;
      struct {prefix}_pbm_socc *socc_i;
  
  #if {PREFIX}_PBM_PRB_SPARSE
      /* Der erste Teilschritt sollte auch bei condensed Problemen funktionieren */
      /* Multiply P_T mit diag_d_sq */
      for (j = 0; j < optvar; j++){{
          for (i = 0; i < nb_of_ueq; i++){{
              help[j*nb_of_ueq+i] = P_T[j*nb_of_ueq+i]*diag_d_sq[i*nb_of_ueq+i];
          }}
      }}
      /* Fast multication of above product with P by use of its structure */
  /*//     for (i = 0; i < pbm->control_veclen; i++){{  
  //         for (k = 0; k < pbm->control_veclen; k++){{ 
  //             {prefix}_mtx_multiply_mtx_vec(Phi+i*optvar+k, help+i*nb_of_ueq, P_T+k*nb_of_ueq, 1, nb_of_ueq);
  //         }}
  //     }}
  //     for (j = 0; j < pbm->horizon-1; j++){{
  //         for (i = pbm->control_veclen + j*pbm->optvar_veclen; i < pbm->control_veclen + (j+1)*pbm->optvar_veclen; i++){{ 
  //             for (k = pbm->control_veclen + j*pbm->optvar_veclen; k < pbm->control_veclen + (j+1)*pbm->optvar_veclen; k++){{ /
  //                 {prefix}_mtx_multiply_mtx_vec(Phi+i*optvar+k, help+i*nb_of_ueq, P_T+k*nb_of_ueq, 1, nb_of_ueq);
  //             }}
  //         }}
  //     }}
  //     for (i = pbm->control_veclen + j*pbm->optvar_veclen; i < (j+1)*pbm->optvar_veclen; i++){{  
  //         for (k = pbm->control_veclen + j*pbm->optvar_veclen; k < (j+1)*pbm->optvar_veclen; k++){{ 
  //             {prefix}_mtx_multiply_mtx_vec(Phi+i*optvar+k, help+i*nb_of_ueq, P_T+k*nb_of_ueq, 1, nb_of_ueq);
  //         }}
  //     }} */
      
      
      for (i = 0; i < pbm->optvar_seqlen; i++){{
          {prefix}_multiply_P_T_d(Phi+i*optvar, P_T, help+i*nb_of_ueq, pbm);
      }}
      
      
      
      
  #endif
  #if {PREFIX}_PBM_PRB_COND
      {prefix}_mtx_multiply_mtx_mtx(help, P_T, diag_d_sq, optvar, nb_of_ueq, nb_of_ueq);
      {prefix}_mtx_multiply_mtx_mtx(Phi, help, P, optvar, nb_of_ueq, optvar);
  #endif
      /* Additional terms for Phi resulting of the second derivative of qc */
      for (i = 0; i < P_hat->nb_qc; i++){{
          qc_i = P_hat->qc[i];
          pos_d = P_hat->nb_lin_constr+i;
          {prefix}_mtx_scale(t_Phi, qc_i->Gamma, 2*d[pos_d],
                           qc_i->dimGamma, qc_i->dimGamma);
          
          for (j = 0; j < qc_i->dimGamma; j++){{
              pos_Phi = (qc_i->par_0 + j)*optvar + qc_i->par_0;
              {prefix}_mtx_add_direct(Phi+pos_Phi, t_Phi+j*qc_i->dimGamma, 1, qc_i->dimGamma);
          }}
      }}
      /* Additional terms for Phi resulting of the second derivative of socc */
      for (i = 0; i < P_hat->nb_socc; i++){{
          socc_i = P_hat->socc[i];
          pos_d = P_hat->nb_lin_constr + P_hat->nb_qc + i;
          {prefix}_mtx_scale(t_Phi, socc_i->AAmcc, 2*d[pos_d],
                           socc_i->colsA, socc_i->colsA);
          
          for (j = 0; j < socc_i->colsA; j++){{
              pos_Phi = (socc_i->par_0 + j)*optvar + socc_i->par_0;
              {prefix}_mtx_add_direct(Phi+pos_Phi, t_Phi+j*socc_i->colsA, 1, socc_i->colsA);
          }}
      }}
      
      {prefix}_mtx_scale_direct(Phi, kappa, optvar, optvar);
      {prefix}_mtx_add_direct(Phi, H, optvar, optvar);  /* statt 2*H */
      {prefix}_mtx_add_direct(Phi, H, optvar, optvar);
      
      {prefix}_mtx_add_direct(Phi, pbm->Phi_soft, optvar, optvar);
  }}
  
  void form_d(real_t *d, const real_t *P, const real_t *h, const real_t *z,
              const uint32_t rowsP, const uint32_t colsP)
  {{
      uint32_t i, j;
      for (i = 0; i < rowsP; i++){{
          d[i] = h[i];
          for (j = 0; j < colsP; j++){{
              d[i] -= P[i*colsP + j]*z[j];
          }}
          d[i] = 1/d[i];
      }}
  }}
  
  void form_dsoft(real_t *ds, real_t *diags, real_t *rd_soft, real_t *Phi_soft,
                  real_t *tmp,
                  const real_t *roh, const real_t *z,
                  const real_t *Psoft, const real_t *Psoft_T, const real_t *hs,
                  const real_t *Fus, const real_t *Fxs, const real_t *Ffs,
                  const uint32_t rowsFus, const uint32_t c_veclen,
                  const uint32_t rowsFfs, const uint32_t s_veclen,
                  const uint32_t T, const struct {prefix}_pbm *pbm)
  {{
  /*//     real_t rd_soft[T*(c_veclen+s_veclen)];
  //     real_t tmp[T*(c_veclen+s_veclen)*(rowsFus*T+rowsFfs)];
  //     real_t Phi_soft[T*(c_veclen+s_veclen)*T*(c_veclen+s_veclen)]; */
  
      
      uint32_t k, i, j;
      for (i = 0; i < rowsFus; i++){{
          ds[i] = hs[i];
          for (j = 0; j < c_veclen; j++){{
              ds[i] -= (Fus+i*c_veclen)[j]*z[j];
          }}
          ds[i] = {prefix}_mtx_smpl_exp(roh[0]*ds[i]);
          diags[i*(rowsFus*T+rowsFfs)+i] = ds[i];
          ds[i] = 1 / (1 + ds[i]);
          diags[i*(rowsFus*T+rowsFfs)+i] = diags[i*(rowsFus*T+rowsFfs)+i] * ds[i] * ds[i];
      }}
      for (k = 1; k < T; k++){{
          for (i = k*rowsFus; i < (k+1)*rowsFus; i++){{
              ds[i] = hs[i];
              for (j = 0; j < s_veclen; j++){{
                  ds[i] -= (Fxs+(i-k*rowsFus)*s_veclen)[j]*(z+k*c_veclen+(k-1)*s_veclen)[j];
              }}
              for (j = 0; j < c_veclen; j++){{
                  ds[i] -= (Fus+(i-k*rowsFus)*c_veclen)[j]*(z+k*c_veclen+(k)*s_veclen)[j];
              }}
              ds[i] = {prefix}_mtx_smpl_exp(roh[0]*ds[i]);
              diags[i*(rowsFus*T+rowsFfs)+i] = ds[i];
              ds[i] = 1 / (1 + ds[i]);
              diags[i*(rowsFus*T+rowsFfs)+i] = diags[i*(rowsFus*T+rowsFfs)+i] * ds[i] * ds[i];
          }}
      }}
      for (i = T*rowsFus; i < T*rowsFus + rowsFfs; i++){{
          ds[i] = hs[i];
          for (j = 0; j < s_veclen; j++){{
              ds[i] -= (Ffs+(i-T*rowsFus)*s_veclen)[j]*(z+T*c_veclen+(T-1)*s_veclen)[j];
          }}
          ds[i] = {prefix}_mtx_smpl_exp(roh[0]*ds[i]);
          diags[i*(rowsFus*T+rowsFfs)+i] = ds[i];
          ds[i] = 1 / (1 + ds[i]);
          diags[i*(rowsFus*T+rowsFfs)+i] = diags[i*(rowsFus*T+rowsFfs)+i] * ds[i] * ds[i];
      }}
  /*//     print_mtx(ds, T*rowsFus+rowsFfs, 1);
  //     print_mtx(diags, T*rowsFus+rowsFfs, T*rowsFus+rowsFfs); */
      
      for (i = 0; i < c_veclen; i++){{
          rd_soft[i] = 0;
          for (j = 0; j < rowsFus; j++){{
              rd_soft[i] += Fus[i+j*c_veclen]*ds[j];
          }}
      }}
      for (k = 1; k < T; k++){{
          for (i = k*c_veclen+(k-1)*s_veclen; i < k*c_veclen+k*s_veclen; i++){{
              rd_soft[i] = 0;
              for (j = 0; j < rowsFus; j++){{
                  rd_soft[i] += Fxs[i-(k*c_veclen+(k-1)*s_veclen)+j*s_veclen]*(ds+k*rowsFus)[j];
              }}
          }}
          for (i = k*c_veclen+(k)*s_veclen; i < (k+1)*c_veclen+k*s_veclen; i++){{
              rd_soft[i] = 0;
              for (j = 0; j < rowsFus; j++){{
                  rd_soft[i] += Fus[i-(k*c_veclen+(k)*s_veclen)+j*c_veclen]*(ds+k*rowsFus)[j];
              }}
          }}
      }}
      for (i = T*c_veclen+(T-1)*s_veclen; i < T*(c_veclen+s_veclen); i++){{
          rd_soft[i] = 0.;
          for (j = 0; j < rowsFfs; j++){{
              rd_soft[i] += Ffs[i-(T*c_veclen+(T-1)*s_veclen)+j*s_veclen] * (ds+T*rowsFus)[j];
          }}
      }}
  /*//     print_mtx(rd_soft, T*(c_veclen + s_veclen), 1);
      
  //     printf("rd_soft stimmt wenn -1 = %d\n", mtx_cmp(rd_soft, rd_soft_ref, 155, 1e-7));
  //     printf("%.17f\t%.17f\n", rd_soft[149], rd_soft_ref[149]); */
      
      
      {prefix}_mtx_multiply_mtx_mtx(tmp, Psoft_T, diags, T*(c_veclen+s_veclen),
                                  rowsFus*T+rowsFfs, rowsFus*T+rowsFfs);
      {prefix}_mtx_multiply_mtx_mtx(Phi_soft, tmp, Psoft, T*(c_veclen+s_veclen),
                                  rowsFus*T+rowsFfs, T*(c_veclen+s_veclen));
      {prefix}_mtx_scale_direct(Phi_soft, roh[0], T*(c_veclen+s_veclen),
                              T*(c_veclen+s_veclen));
      
  /*//     print_mtx(Phi_soft, T*(c_veclen+s_veclen),
  //                             T*(c_veclen+s_veclen));
    
      
  //     printf("Phi_soft stimmt wenn -1 = %d\n", mtx_cmp(Phi_soft, Phi_soft_ref, 155*155, 1e-6));
  //     printf("%.17f\t%.17f\n", Phi_soft[23244], Phi_soft_ref[23244]);
      
      
      
  //     {prefix}_mtx_multiply_mtx_mtx(pbm->tmp_Phi_sft_blk->data, diags, Fus,
  //                                 rowsFus, rowsFus, c_veclen);
      
  //     {prefix}_mtx_multiply_mtx_mtx();
  //     print_mtx(pbm->tmp_Phi_sft_blk->data, rowsFus, c_veclen); */
      
  }}
  
  void form_diag_d_sq(real_t *diag_d_sq, const real_t *d, const uint32_t dim)
  {{
      uint32_t i;
      for (i = 0; i < dim; i++){{
              diag_d_sq[i*dim+i] = d[i]*d[i];
      }}
  }}
  
  void {prefix}_pbm_calc_kappa(real_t *kappa, const struct {prefix}_pbm *pbm, const real_t *z)
  {{
      real_t *tmp1 = pbm->tmp1_optvar_seqlen;
      real_t *tmp3 = pbm->tmp2_optvar_seqlen;
      real_t *tmp2 = pbm->tmp3_state_veclen; 
      
      {prefix}_mtx_substract(tmp3, pbm->z_opt, pbm->zref, pbm->optvar_seqlen, 1);
  
      {prefix}_multiply_H_z(tmp1, pbm->H, pbm->z_opt, pbm);
  
      {prefix}_mtx_multiply_mtx_vec(tmp2, tmp1, pbm->z_opt, 1, pbm->optvar_seqlen);
      {prefix}_mtx_multiply_mtx_vec(kappa, pbm->g, z, 1, pbm->optvar_seqlen);
      kappa[0] = tmp2[0];
      kappa[0] *= 0.01*30/pbm->optvar_seqlen;  /* TODO auf optvar_seqlen umstellen*/
  #ifdef HHMPC_QPSMALLTEST
      kappa[0] /= 24;
  #endif
  #if {PREFIX}_PBM_PRB_COND
      kappa[0] /= 24*3;
  /*     kappa[0] = 0.00008;*/
  #endif
      kappa[0] += 0;    
  /*    printf("calculated kappa = %.20f\n", pbm->kappa[0]);*/
  
      /* -5 N=5, höchstens -6 N=20, -8 N= 30 */
      kappa[0] = (kappa[0] >5e-8)? kappa[0] : 5e-8;  /* -3 statt -5 für QP cond */
   /* N=30: cond 5*1e-9, uncond 5*1e-6*/
  #if {PREFIX}_PBM_PRB_COND
   kappa[0] *=225.15;
  #endif
  }}
  
  /* static functions definition */
  
  void {prefix}_pbm_warm_start(const struct {prefix}_pbm *pbm)
  {{
      real_t *tmp = pbm->tmp3_state_veclen;
      {prefix}_mtx_shift_sequence(pbm->z_ini, pbm->z_opt, pbm->optvar_veclen,
              pbm->optvar_seqlen);
      {prefix}_mtx_multiply_mtx_vec((pbm->z_ini)+pbm->optvar_seqlen-pbm->state_veclen,
                                  pbm->A, (pbm->z_opt)+pbm->optvar_seqlen-pbm->state_veclen,
                                  pbm->state_veclen, pbm->state_veclen);
      {prefix}_mtx_mul_add((pbm->z_ini)+pbm->optvar_seqlen-pbm->state_veclen, tmp,
                                  pbm->B, (pbm->z_opt)+pbm->optvar_seqlen-pbm->optvar_veclen,
                                  pbm->state_veclen, pbm->control_veclen);    
      {prefix}_mtx_scale_direct(pbm->z_ini, 1., pbm->optvar_seqlen, 1);
      {prefix}_mtx_shift_sequence(pbm->v_ini, pbm->v_opt, pbm->state_veclen,
              pbm->dual_seqlen);
      
      return;
  }}
  
  uint32_t {prefix}_better_step_size(
                  const struct {prefix}_pbm *pbm, real_t f_p_g, real_t f_p_alpha)
  {{
      if (({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1) ||
          (f_p_g > f_p_alpha) ||
          ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1) ){{
              return 0;
          }}
          return -1;
  }}
  
  void {prefix}_bt_line_search(real_t *st_size, const struct {prefix}_pbm *pbm)
  {{
      const real_t g_step = 1e-6;
      const real_t alpha = 0.25; /* 0.15; */  /* [0.4] Measure for reduction of function value  */
      const real_t beta = 0.6; /*0.5; */  /* [0.6] Factor to decrease step ervery iteration */
      real_t *help_z = pbm->tmp6_optvar_seqlen;
      real_t *help_v = pbm->tmp7_dual_seqlen;
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      real_t st = 1.;
      
      real_t f_p;
      real_t f_p_g;
      real_t g_in_dir;
      
      st_size[0] = st;
      
      /*Save maintain value*/
      memcpy(help_z, pbm->z_opt, pbm->sizeof_optvar_seqlen);
      memcpy(help_v, pbm->v_opt, pbm->sizeof_dual_seqlen);
      
      /* update P matrices */
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
          form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt,
                 pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
          form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
          residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
      {prefix}_residual_norm(&f_p, pbm->r_d, pbm->r_p,
                    pbm->optvar_seqlen, pbm->dual_seqlen);
      
      {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, g_step, pbm->optvar_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->z_opt, help_z, pbm->optvar_seqlen, 1);
      {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, g_step, pbm->dual_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->v_opt, help_v, pbm->dual_seqlen, 1);
      /* update P matrices */
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
      form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt, pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
      form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
      residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
      {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
                    pbm->optvar_seqlen, pbm->dual_seqlen);
      g_in_dir = (f_p_g - f_p)/g_step;
  /*//     printf("Grad in dir = %.8f\n", g_in_dir);
  //     g_in_dir = g_in_dir <= 0 ? g_in_dir : 0.;
  //     printf("Grad in dir = %.8f\n", g_in_dir);
  //     printf("st size inner = %f\n", st_size[0]); */
      {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, st_size[0],
                       pbm->optvar_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->z_opt, help_z,
                            pbm->optvar_seqlen, 1);
      {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, st_size[0],
                       pbm->dual_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->v_opt, help_v,
                            pbm->dual_seqlen, 1);
      /* update P matrices */
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
      {prefix}_pbm_check_valid(pbm, pbm->z_opt);
      form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt, pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
      form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
      residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
      {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
                    pbm->optvar_seqlen, pbm->dual_seqlen);
      while (({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1) || (f_p_g > (f_p + alpha*st*g_in_dir)) || ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1) )
      {{
          st *= beta;
          if (st < g_step){{st = 0.; break;}}
          
          {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, st,
                           pbm->optvar_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->z_opt, help_z,
                                pbm->optvar_seqlen, 1);
          {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, st,
                           pbm->dual_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->v_opt, help_v,
                                pbm->dual_seqlen, 1);
          /* update P matrices */
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
          form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt,
                 pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
          form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
          residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
          {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
                        pbm->optvar_seqlen, pbm->dual_seqlen);
      }}
      
      /*Load back maintain value*/
      memcpy(pbm->z_opt, help_z, pbm->sizeof_optvar_seqlen);
      memcpy(pbm->v_opt, help_v, pbm->sizeof_dual_seqlen);
      st_size[0] = st;
  }}
  
  void {prefix}_bt_line_search_new(
                  real_t *st_size, const struct {prefix}_pbm *pbm)
  {{
      const real_t g_step = 1e-6;
      const real_t alpha = 0.25; /* 0.15; */  /* [0.4] Measure for reduction of function value  */
      const real_t beta = 0.6; /* 0.5; */  /* [0.6] Factor to decrease step ervery iteration */
      real_t *help_z = pbm->tmp6_optvar_seqlen;
      real_t *help_v = pbm->tmp7_dual_seqlen;
      real_t *t_solve_optvar_seqlen = pbm->tmp1_optvar_seqlen;
      real_t *t_optvar_seqlen = pbm->tmp2_optvar_seqlen;
      real_t st = 1.;
      
      real_t f_p;
      real_t f_p_g;
      real_t g_in_dir;
      
      uint32_t i;
      
      st_size[0] = st;
      
      /*Save maintain value*/
      memcpy(help_z, pbm->z_opt, pbm->sizeof_optvar_seqlen);
      memcpy(help_v, pbm->v_opt, pbm->sizeof_dual_seqlen);
      
      
      {prefix}_residual_norm(&f_p, pbm->r_d, pbm->r_p,
                    pbm->optvar_seqlen, pbm->dual_seqlen);
      
      {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, g_step, pbm->optvar_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->z_opt, help_z, pbm->optvar_seqlen, 1);
      {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, g_step, pbm->dual_seqlen, 1);
      {prefix}_mtx_add_direct(pbm->v_opt, help_v, pbm->dual_seqlen, 1);
      /* update P matrices */
      {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
      form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt, pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
      form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
      residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
      {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
                    pbm->optvar_seqlen, pbm->dual_seqlen);
      g_in_dir = (f_p_g - f_p)/g_step;
  /* //     printf("Grad in dir = %.8f\n", g_in_dir);
  //     g_in_dir = g_in_dir <= 0 ? g_in_dir : 0.;
  //     printf("Grad in dir = %.8f\n", g_in_dir);
  //     printf("st size inner = %f\n", st_size[0]);
  //     {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, st_size[0],
  //                      pbm->optvar_seqlen, 1);
  //     {prefix}_mtx_add_direct(pbm->z_opt, help_z,
  //                           pbm->optvar_seqlen, 1);
  //     {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, st_size[0],
  //                      pbm->dual_seqlen, 1);
  //     {prefix}_mtx_add_direct(pbm->v_opt, help_v,
  //                           pbm->dual_seqlen, 1);
  //      *//* update P matrices *//*
  //     {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
  //                t_solve_optvar_seqlen, t_optvar_seqlen);
  //     {prefix}_pbm_check_valid(pbm, pbm->z_opt);
  //     form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt, pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
  //     form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
  //                    pbm->tmp3_mtx_optvar_nb_of_soft,
  //                    pbm->roh, pbm->z_opt, 
  //                    pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
  //                    pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
  //                    pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
  //                    pbm->state_veclen, pbm->horizon, pbm);
  //     residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
  //     {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
  //                   pbm->optvar_seqlen, pbm->dual_seqlen);
  //     while (({prefix}_pbm_check_valid(pbm, pbm->z_opt)+1) || (f_p_g > (f_p + alpha*st*g_in_dir)) || ({prefix}_pbm_check_positiv(pbm, pbm->z_opt)+1) )
  //     while ({prefix}_better_step_size(pbm, f_p_g, (f_p + alpha*st*g_in_dir))) */
      st = 1.0234904e-6;
      st = 0.0060466176;
      st = 0.0279936;
      st_size[0] = g_step;
      for (i = 0; i < 8; i++)
      {{
          {prefix}_mtx_scale(pbm->z_opt, pbm->delta_z, st,
                           pbm->optvar_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->z_opt, help_z,
                                pbm->optvar_seqlen, 1);
          {prefix}_mtx_scale(pbm->v_opt, pbm->delta_v, st,
                           pbm->dual_seqlen, 1);
          {prefix}_mtx_add_direct(pbm->v_opt, help_v,
                                pbm->dual_seqlen, 1);
          /* update P matrices */
          {prefix}_pbm_update_P(pbm->P_of_z, pbm->optvar_seqlen,
                 t_solve_optvar_seqlen, t_optvar_seqlen);
          form_d(pbm->d, pbm->P, pbm->h, pbm->z_opt,
                 pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
          form_dsoft(pbm->dsoft, pbm->diag_d_soft, pbm->r_d_soft, pbm->Phi_soft,
                     pbm->tmp3_mtx_optvar_nb_of_soft,
                     pbm->roh, pbm->z_opt, 
                     pbm->Psoft, pbm->Psoft_T, pbm->hsoft,
                     pbm->Fusoft, pbm->Fxsoft, pbm->Ffsoft,
                     pbm->rowsFusoft, pbm->control_veclen, pbm->rowsFfsoft,
                     pbm->state_veclen, pbm->horizon, pbm);
          residual(pbm, pbm->z_opt, pbm->v_opt, pbm->d, pbm->kappa[0]);
          {prefix}_residual_norm(&f_p_g, pbm->r_d, pbm->r_p,
                        pbm->optvar_seqlen, pbm->dual_seqlen);
  /*         printf("step %f\n", st); */
          st_size[0] = {prefix}_better_step_size(pbm, f_p_g, (f_p + alpha*st*g_in_dir)) ? st : st_size[0];
          st /= beta;
      }}
      
      /*Load back maintain value*/
      memcpy(pbm->z_opt, help_z, pbm->sizeof_optvar_seqlen);
      memcpy(pbm->v_opt, help_v, pbm->sizeof_dual_seqlen);
  /*     st_size[0] = st; */
  }}
  
  void {prefix}_residual_norm(
                  real_t *f, const real_t *r_d, const real_t *r_p,
                  const uint32_t ld, const uint32_t lp)
  {{
      uint32_t i;
      f[0] = 0.;
      /* Better solution if only norm(r_p) is calculated */
  /*//     for (i = 0; i < ld; i++){{
  //         f[0] += r_d[i]*r_d[i];
  //     }} */
      for (i = 0; i < lp; i++){{
          f[0] += r_p[i]*r_p[i];
      }}
  }}
  
  void {prefix}_multiply_P_T_d(real_t *product, const real_t *P_T, const real_t *d,
                          const struct {prefix}_pbm* pbm)
  {{
  #if {PREFIX}_PBM_PRB_SPARSE
      uint32_t i, j, k;
      for (i = 0; i < pbm->control_veclen; i++){{
          product[i] = 0.;
          for (j = 0; j < HHMPC_NB_ROWSFU; j++){{
              product[i] += P_T[i*HHMPC_NB_LCONSTR+j]*d[j];
          }}
      }}
      for (k = 0; k < pbm->horizon-1; k++){{
          for (i = pbm->control_veclen + k*pbm->optvar_veclen; i < pbm->control_veclen + (k+1)*pbm->optvar_veclen; i++){{
              product[i] = 0.;
              for (j = HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU; j < HHMPC_NB_ROWSFU + (k+1)*HHMPC_NB_ROWSFU; j++){{
                  product[i] += P_T[i*HHMPC_NB_LCONSTR+j]*d[j];
              }}
          }}
      }}
      for (i = pbm->control_veclen + k*pbm->optvar_veclen; i < (k+1)*pbm->optvar_veclen; i++){{
          product[i] = 0.;
          for (j = HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU; j < HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU + HHMPC_NB_ROWSFF; j++){{
              product[i] += P_T[i*HHMPC_NB_LCONSTR+j]*d[j];
          }}
      }}
  #endif
  #if {PREFIX}_PBM_PRB_COND
      {prefix}_mtx_multiply_mtx_vec(product, P_T, d,
                                    pbm->optvar_seqlen, pbm->nb_of_ueq_constr);
  #endif
  }}
  
  void {prefix}_multiply_P_z(real_t *product, const real_t *P, const real_t *z,
                          const struct {prefix}_pbm* pbm)
  {{
  #if {PREFIX}_PBM_PRB_SPARSE
      uint32_t i, j, k;
      for (i = 0; i < HHMPC_NB_ROWSFU; i++){{
          product[i] = 0.;
          for (j = 0; j < pbm->control_veclen; j++){{
              product[i] += P[i*pbm->optvar_seqlen+j]*z[j];
          }}
      }}
      for (k = 0; k < pbm->horizon-1; k++){{
          for (i = HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU; i < HHMPC_NB_ROWSFU + (k+1)*HHMPC_NB_ROWSFU; i++){{
              product[i] = 0.;
              for (j = pbm->control_veclen + k*pbm->optvar_veclen; j < pbm->control_veclen + (k+1)*pbm->optvar_veclen; j++){{
                  product[i] += P[i*pbm->optvar_seqlen+j]*z[j];
              }}
          }}
      }}
      for (i = HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU; i < HHMPC_NB_ROWSFU + k*HHMPC_NB_ROWSFU + HHMPC_NB_ROWSFF; i++){{
          product[i] = 0.;
          for (j = pbm->control_veclen + k*pbm->optvar_veclen; j < (k+1)*pbm->optvar_veclen; j++){{
              product[i] += P[i*pbm->optvar_seqlen+j]*z[j];
          }}
      }}
  #endif
  #if {PREFIX}_PBM_PRB_COND
    {prefix}_mtx_multiply_mtx_vec(product, P, z,
                                  pbm->nb_of_ueq_constr, pbm->optvar_seqlen);
  #endif
  }}
  
  void {prefix}_multiply_H_z(real_t *product, const real_t *H, const real_t *z,
                          const struct {prefix}_pbm* pbm)
  {{
  #if {PREFIX}_PBM_PRB_SPARSE
      uint32_t i, j, k;
      for (i = 0; i < pbm->control_veclen; i++){{
          product[i] = 0.;
          for (j = 0; j < pbm->control_veclen; j++){{
              product[i] += H[i*pbm->optvar_seqlen+j]*z[j];
          }}
      }}
      for (k = 0; k < pbm->horizon-1; k++){{
          for (i = pbm->control_veclen + k*pbm->optvar_veclen; i < pbm->control_veclen + (k+1)*pbm->optvar_veclen; i++){{
              product[i] = 0.;
              for (j = pbm->control_veclen + k*pbm->optvar_veclen; j < pbm->control_veclen + (k+1)*pbm->optvar_veclen; j++){{
                  product[i] += H[i*pbm->optvar_seqlen+j]*z[j];
              }}
          }}
      }}
      for (i = pbm->control_veclen + k*pbm->optvar_veclen; i < (k+1)*pbm->optvar_veclen; i++){{
          product[i] = 0.;
          for (j = pbm->control_veclen + k*pbm->optvar_veclen; j < (k+1)*pbm->optvar_veclen; j++){{
              product[i] += H[i*pbm->optvar_seqlen+j]*z[j];
          }}
      }}
  #endif    
  #if {PREFIX}_PBM_PRB_COND
      {prefix}_mtx_multiply_mtx_vec(product, H, z,
                                    pbm->optvar_seqlen, pbm->optvar_seqlen);
  #endif
  }}
  