diff --git a/src/micpsolver/micpsolver.inl b/src/micpsolver/micpsolver.inl
index 041f4e1..2f33ca9 100644
--- a/src/micpsolver/micpsolver.inl
+++ b/src/micpsolver/micpsolver.inl
@@ -1,493 +1,472 @@
 /*-------------------------------------------------------
 
  - Module : micpsolver
  - File : micpsolver.inl
  - Author : Fabien Georget
 
  Copyright (c) 2014, Fabien Georget, Princeton University
 
 ---------------------------------------------------------*/
 
 #include "micpsolver.hpp" // for syntaxic coloration...
 
 #include "estimate_cond_number.hpp"
 #include "utils/log.hpp"
 
 //! \file micpsolver.inl implementation of the MiCP solver
 
 namespace specmicp {
 namespace micpsolver {
 
-//! \brief Return  \f$ e_j \in R^n \f$
-inline Eigen::VectorXd unit_vector(int n, int j) {
-    Eigen::VectorXd unit(Eigen::VectorXd::Zero(n));
-    unit(j) = 1.0;
-    return unit;
-}
-
-
 // Main algorithm
 // ##############
 
 template <class Program>
 MiCPSolverReturnCode MiCPSolver<Program>::solve(Eigen::VectorXd &x)
 {
     int cnt = 0;
     if (get_options().use_crashing) crashing(x);
     MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet;
     Eigen::VectorXd update(get_neq());
     while (retcode == MiCPSolverReturnCode::NotConvergedYet)
     {
         DEBUG << "Iteration : " << cnt;
         m_program->hook_start_iteration(x, m_phi_residuals.norm());
         setup_residuals(x);
         SPAM << "Residuals : \n ----- \n" << m_phi_residuals << "\n ----- \n";
         retcode = check_convergence(cnt, update, x);
         get_perf().return_code = retcode;
         m_max_taken = false;
         if (retcode != MiCPSolverReturnCode::NotConvergedYet) break;
         setup_jacobian(x);
         if(get_options().use_scaling)
             search_direction_calculation(update);
         else
             search_direction_calculation_no_scaling(update);
         int termcode = linesearch(update, x);
         DEBUG << "Return LineSearch : " << termcode;
         projection(x);
         ++cnt;
         get_perf().nb_iterations = cnt;
     }
     return retcode;
 }
 
 template <class Program>
 MiCPSolverReturnCode MiCPSolver<Program>::check_convergence(int nb_iterations,
                                             Eigen::VectorXd& update,
                                             Eigen::VectorXd& solution)
 {
     MiCPSolverReturnCode termcode = MiCPSolverReturnCode::NotConvergedYet;
     if (m_phi_residuals.lpNorm<Eigen::Infinity>() < get_options().fvectol)
     {
         termcode = MiCPSolverReturnCode::ResidualMinimized;
     }
     else if (nb_iterations >0 and norm_update<Eigen::Infinity>(update, solution) < get_options().steptol)
     {
         WARNING << "Error is minimized - may indicate a stationnary point";
         termcode = MiCPSolverReturnCode::ErrorMinimized;
     }
     else if (nb_iterations >  get_options().max_iter)
     {
         ERROR << "Maximum number of iteration reached (" << get_options().max_iter << ")";
         termcode = MiCPSolverReturnCode::MaxIterations;
     }
     else if (m_max_taken)
     {
         ++m_consec_max;
         if (m_consec_max == get_options().maxiter_maxstep) {
             ERROR << "Divergence detected - Maximum step length taken two many times";
             termcode = MiCPSolverReturnCode::MaxStepTakenTooManyTimes;
         }
     }
     else
     {
         m_consec_max = 0;
     }
     return termcode;
 }
 
 
 template <class Program>
 MiCPSolverReturnCode MiCPSolver<Program>::search_direction_calculation(Eigen::VectorXd& update)
 {
     Eigen::VectorXd rscaler(Eigen::VectorXd::Ones(get_neq()));
     Eigen::VectorXd cscaler(Eigen::VectorXd::Ones(get_neq()));
     scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler);
     m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal();
     Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
     m_gradient_step_taken = false;
     int m;
     for (m=0; m<4; ++m)
     {
         const double lambda = get_options().factor_gradient_search_direction;
         solver.compute(m_jacobian);
         get_perf().nb_factorization += 1;
         if (solver.info() != Eigen::Success or not solver.isInvertible())
         {
             ERROR << "System cannot be solved, we try a perturbation";
             continue;
         }
         double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
         if (cond > get_options().condition_limit)
         {
             continue;
         }
         update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi));
         update  = cscaler.cwiseProduct(update);
         double descent_cond = m_grad_phi.dot(update);
         double norm_grad = m_grad_phi.norm();
         double norm_update = update.norm();
 
         if (   (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_update,2),std::pow(norm_update,3)))
                and (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_grad,2),std::pow(norm_grad,3)))
              )
             break; // we have a solution !
         m_gradient_step_taken = true;
         m_jacobian +=  rscaler.asDiagonal() * ( lambda*Eigen::MatrixXd::Identity(get_neq(),get_neq())) * cscaler.asDiagonal();
     }
     DEBUG << "Gradient step : m = " << m;
     if (m == 4) {
         INFO << "Full gradient step taken !";
         update = -m_grad_phi;
     }
     return MiCPSolverReturnCode::NotConvergedYet;
 }
 
 template <class Program>
 MiCPSolverReturnCode MiCPSolver<Program>::search_direction_calculation_no_scaling(Eigen::VectorXd& update)
 {
     DEBUG << "Solving linear system";
     Eigen::ColPivHouseholderQR<Eigen::MatrixXd> solver;
     m_gradient_step_taken = false;
     int m;
     for (m=0; m<4; ++m)
     {
         const double lambda = get_options().factor_gradient_search_direction;
         solver.compute(m_jacobian);
         get_perf().nb_factorization += 1;
 
         if (solver.info() != Eigen::Success or not solver.isInvertible()) continue;
         double cond = estimate_condition_number(solver.matrixR().triangularView<Eigen::Upper>());
         if (cond > get_options().condition_limit)
         {
             continue;
         }
         update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi));
         double descent_cond = m_grad_phi.dot(update);
         double norm_grad = m_grad_phi.norm();
         double norm_update = update.norm();
 
         if (   (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_update,2),std::pow(norm_update,3)))
                and (descent_cond <= -get_options().factor_descent_condition*std::min(std::pow(norm_grad,2),std::pow(norm_grad,3)))
              )
             break; // we have a solution !
         m_gradient_step_taken = true;
         m_jacobian +=  lambda*Eigen::MatrixXd::Identity(get_neq(),get_neq());
     }
     DEBUG << "Gradient step : m = " << m;
     if (m ==4) {
         INFO << "Full gradient step taken !";
         update = -m_grad_phi;
     }
     return MiCPSolverReturnCode::NotConvergedYet;
 }
 
 template <class Program>
 void MiCPSolver<Program>::crashing(Eigen::VectorXd &x)
 {
     DEBUG << "Crashing ";
     const double beta = 0.5;
     const double sigma = 1e-5;
     int cnt = 0;
     while (cnt < 10)
     {
         setup_residuals(x);
         setup_jacobian(x);
         m_grad_phi = m_jacobian.transpose()*m_phi_residuals;
         Eigen::VectorXd xp(get_neq());
         int l=0;
         const int maxl = 10;
         while (l<maxl)
         {
             xp = x - std::pow(beta, l)*m_grad_phi;
             for (int i=get_neq_free(); i<get_neq(); ++i)
             {
                 if (xp(i) < 0) xp(i) = 0;
             }
             Eigen::VectorXd new_res(get_neq());
             compute_residuals(x, new_res);
             reformulate_residuals_inplace(x, new_res);
             double test = 0.5*(new_res.squaredNorm() - m_phi_residuals.squaredNorm())  + sigma*m_grad_phi.dot(x - xp);
             if (test <= 0) break;
             ++l;
         }
         if (l == maxl) break;
         x = xp;
 
         ++cnt;
     }
     get_perf().nb_crashing_iterations = cnt;
     DEBUG << "Crashing iterations : " << cnt;
     m_max_merits.reserve(4);
     SPAM << "Solution after crashing \n ------ \n " << x << "\n ----- \n";
 }
 
 // Others
 // ######
 
 template <class Program>
 void MiCPSolver<Program>::reformulate_residuals(const Eigen::VectorXd& x,
                                                 const Eigen::VectorXd& r,
                                                 Eigen::VectorXd& r_phi)
 {
     r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1);
     for (int i =  get_neq_free(); i<get_neq(); ++i)
     {
         r_phi(i) = phi_lower_bounded(x(i), r(i), 0);
     }
 }
 
 template <class Program>
 void MiCPSolver<Program>::reformulate_residuals_inplace(const Eigen::VectorXd& x,
                                                 Eigen::VectorXd& r)
 {
     for (int i =  get_neq_free(); i<get_neq(); ++i)
     {
         r(i) = phi_lower_bounded(x(i), r(i), 0);
     }
 }
 
-double dz(double z) {
-    if (z > 0)
-    {
-        return 1.0;
-    }
-    else if (z < 0)
-    {
-        return 0;
-    }
-    else // z ==0
-        return 0.0;
-
-}
-
 // page 808 Facchinei and Pang(2003) (vol 2, chap 9)
 template <class Program>
 void MiCPSolver<Program>::reformulate_jacobian(const Eigen::VectorXd& x,
                           const Eigen::VectorXd& r,
                           Eigen::MatrixXd& jacobian
                           )
 {
     // set the z vector : contains 1 for degenerate points
     Eigen::VectorXd z(Eigen::VectorXd::Zero(get_neq()));
     for (int i=get_neq_free(); i<get_neq(); ++i)
     {
         if (x(i) == 0 and r(i) == 0)
             z(i) = 1.0;
     }
     // modify the jacobian
     const double lambda = get_options().penalization_factor;
     for (int i=get_neq_free(); i<get_neq(); ++i)
     {
-        const auto ei = unit_vector(get_neq(), i);
         Eigen::VectorXd grad_fi = jacobian.block(i, 0, 1, get_neq()).transpose();
         if (z(i) != 0)
         {
             double gpdotz = grad_fi.dot(z);
             double s = std::abs(z(i)) + std::abs(gpdotz);
             s = s * std::sqrt(z(i)*z(i)/(s*s) + gpdotz*gpdotz/(s*s));
             const double c = lambda*(z(i)/s - 1);
             const double d = lambda*(gpdotz/s -1);
-            grad_fi = c*ei + d*grad_fi;
+            grad_fi = d*grad_fi;
+            grad_fi(i) += c;
         }
         else
         {
             double s = std::abs(x(i)) + std::abs(r(i));
             s = s * std::sqrt(x(i)*x(i)/(s*s) + r(i)*r(i)/(s*s));
             double c = lambda*(x(i)/s - 1);
             double d = lambda*(r(i)/s - 1);
             if ((lambda <1) and (r(i) > 0)  and (x(i) >0))
             {
                 c -= (1-lambda)*r(i);
                 d -= (1-lambda)*x(i);
             }
 
-            grad_fi = c*ei + d*grad_fi;
+            grad_fi = d*grad_fi;
+            grad_fi(i) += c;
         }
         jacobian.block(i, 0, 1, get_neq()) = grad_fi.transpose();
     }
 }
 
 // ref : Munson et al. (2001)
 template <class Program>
 void MiCPSolver<Program>:: scaling_jacobian(
         const Eigen::MatrixXd& jacobian,
         const Eigen::VectorXd& r_phi,
         Eigen::VectorXd& rscaler,
         Eigen::VectorXd& cscaler)
 {
     for (int i=0; i<get_neq(); ++i)
     {
         const double sumhsq = jacobian.block(i, 0, 1, get_neq()).array().square().sum();
         double s = std::sqrt(r_phi(i)*r_phi(i) + sumhsq);
         rscaler(i) = 1.0/std::max(s, 1e-10);
     }
     for (int i=0; i<get_neq(); ++i)
     {
         const double sumhsq = (rscaler.asDiagonal()*jacobian).block(0, i, get_neq(), 1).array().square().sum();
         double s = std::sqrt(sumhsq);
         cscaler(i) = 1.0/std::max(s, 1e-10);
     }
 }
 
 
 template <class Program>
 int MiCPSolver<Program>::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x)
 {
      // Reference Algo A6.3.1 : Dennis and Schnabel (1983)
     DEBUG << "Linesearch";
     Eigen::VectorXd xp(get_neq());
     Eigen::VectorXd new_res(get_neq());
     double fcp;
 
     m_max_taken = false;
     int retcode = 2;
     const double alpha = 1e-6;
     double newtlen = is_step_too_long(p);
     double init_slope = m_grad_phi.dot(p);
     double rellength = std::abs(p(0));
     for (int i=1; i<get_neq(); ++i)
     {
         rellength = std::max(rellength, std::abs(p(i)));
     }
     double minlambda = get_options().steptol / rellength;
     double lambda = m_program->max_lambda(x, p);
     double lambda_prev = lambda;
 
     // non monotone linesearch
     // -----------------------
 
     double merit_value = 0.5*m_phi_residuals.squaredNorm();
     // new residual
     xp = x + lambda*p;
     compute_residuals(xp, new_res);
     reformulate_residuals_inplace(xp, new_res);
     fcp = 0.5*new_res.squaredNorm();
 
     // Skip linesearch if enough progress is done
     if (fcp < get_options().coeff_accept_newton_step *merit_value)
     {
         if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value;
         else m_max_merits.push_back(merit_value);
         x = xp;
         return 0;
     }
 
     //std::cout << "Merit value : " << merit_value << std::endl;
     double mmax = merit_value;
     if (m_max_merits.size() > 0)
     {
         mmax = m_max_merits[m_max_merits.size()-1];
     }
     if (m_max_merits.size() < 4)
     {
         m_max_merits.push_back(merit_value);
         if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4;
     }
     else if (merit_value < mmax)
     {
         m_max_merits[3] = merit_value;
         merit_value = mmax;
     }
     if (m_gradient_step_taken)
     {
         merit_value *= 100;
     }
     //std::cout << "Merit value used : " << merit_value << std::endl;
     double fc = merit_value;
     double fcp_prev;
     int cnt = 0;
     do
     {
         fcp = 0.5*new_res.squaredNorm();
         //std::cout << "fcp : " << fcp << "\n fc+alin : " << fc+alpha*lambda*init_slope << " # fc : " << fc << std::endl;
         if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003
         {
             retcode = 0;
             if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) {
                 m_max_taken = true;
             }
             break;
         }
         else if (lambda < minlambda)
         {
             retcode = 1;
             break;
         }
         else
         {
             double lambdatmp;
             if (cnt == 0) { // only a quadratic at the first
                 lambdatmp = - init_slope / (2*(fcp - fc -init_slope));
             }
             else
             {
                 const double factor = 1 /(lambda - lambda_prev);
                 const double x1 = fcp - fc - lambda*init_slope;
                 const double x2 = fcp_prev - fc - lambda_prev*init_slope;
 
                 const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev));
                 const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev));
 
                 if (a == 0)
                 { // cubic interpolation is in fact a quadratic interpolation
                     lambdatmp = - init_slope/(2*b);
                 }
                 else
                 {
                     const double disc = b*b-3*a*init_slope;
                     lambdatmp = (-b+std::sqrt(disc))/(3*a);
                 }
                 if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda;
             }
             lambda_prev = lambda;
             fcp_prev = fcp;
             if (lambdatmp < 0.1*lambda) {
                 lambda = 0.1 * lambda;
             } else {
                 lambda = lambdatmp;
             }
         }
         xp = x + lambda*p;
         compute_residuals(xp, new_res);
         reformulate_residuals_inplace(xp, new_res);
         ++cnt;
     } while(retcode == 2 and cnt < 100);
     DEBUG << "Lambda : " << lambda;
     if (cnt == 100)
     {
         ERROR << "Too much linesearch iterations ! We stop";
     }
     x = xp;
     p = lambda*p;
     return retcode;
 
 }
 
 // Projection of the variables onto the feasible set
 template <class Program>
 void MiCPSolver<Program>::projection(Eigen::VectorXd &x)
 {
     for (int i=0; i<m_program->nb_complementarity_variables(); ++i)
     {
         if (x(i+m_program->nb_free_variables()) < get_options().projection_min_variable)
         {
             x(i+m_program->nb_free_variables()) = 0;
         }
     }
 }
 
 template <class Program>
 double MiCPSolver<Program>::is_step_too_long(Eigen::VectorXd& update)
 {
     double steplength = update.norm();
     if (steplength > get_options().maxstep)
     {
         m_max_taken = true;
         update  = get_options().maxstep / steplength * update;
         steplength = get_options().maxstep;
     }
     return steplength;
 }
 
 } // end namespace micpsolver
 } // end namespace specmicp