#include "osqp.h"   // For OSQP rho update
#include "auxil.h"
#include "proj.h"
#include "lin_alg.h"
#include "constants.h"
#include "scaling.h"
#include "util.h"

/***********************************************************
 * Auxiliary functions needed to compute ADMM iterations * *
 ***********************************************************/
#if EMBEDDED != 1
c_float compute_rho_estimate(OSQPWorkspace * work){
    c_int n, m;  // Dimensions
    c_float pri_res, dua_res; // Primal and dual residuals
    c_float pri_res_norm, dua_res_norm;  // Normalization for the residuals
    c_float temp_res_norm;   // Temporary residual norm
    c_float rho_estimate;  // Rho estimate value

    // Get problem dimensions
    n = work->data->n;
    m = work->data->m;

    // Get primal and dual residuals
    pri_res = vec_norm_inf(work->z_prev, m); 
    dua_res = vec_norm_inf(work->x_prev, n);
     
    // Normalize primal residual
    pri_res_norm = vec_norm_inf(work->z, m);     // ||z||
    temp_res_norm = vec_norm_inf(work->Ax, m);   // ||Ax||
    pri_res_norm = c_max(pri_res_norm, temp_res_norm);       // max (||z||,||Ax||)
    pri_res /= (pri_res_norm + 1e-10);  // Normalize primal residual (prevent 0 division)

    // Normalize dual residual
    dua_res_norm = vec_norm_inf(work->data->q, n);  // ||q||
    temp_res_norm = vec_norm_inf(work->Aty, n);     // ||A' y||
    dua_res_norm = c_max(dua_res_norm, temp_res_norm);          
    temp_res_norm = vec_norm_inf(work->Px, n);      //  ||P x||
    dua_res_norm = c_max(dua_res_norm, temp_res_norm);          // max(||q||,||A' y||,||P x||)
    dua_res /= (dua_res_norm + 1e-10);  // Normalize dual residual (prevent 0 division)


    // Return rho estimate
    rho_estimate = work->settings->rho * c_sqrt(pri_res / (dua_res + 1e-10));   // (prevent 0 division)
    rho_estimate = c_min(c_max(rho_estimate, RHO_MIN), RHO_MAX);  // Constrain rho values
    
    // DEBUG: Print stuff
    // c_print("current rho = %.2e\n", work->settings->rho);
    // c_print("pri_res = %.2e, dua_res = %.2e\n", pri_res, dua_res);
    // c_print("new rho = %.2e\n", rho_estimate);

    return rho_estimate;
}

c_int adapt_rho(OSQPWorkspace * work){
    c_int exitflag; // Exitflag
    c_float rho_new;  // New rho value

    exitflag = 0;   // Initialize exitflag to 0
    
    // Compute new rho
    rho_new = compute_rho_estimate(work);

    // Set rho estimate in info
    work->info->rho_estimate = rho_new;
	   
    // Check if the new rho is large or small enough and update it in case
    if (rho_new > work->settings->rho * ADAPTIVE_RHO_TOLERANCE ||
            rho_new < work->settings->rho /  ADAPTIVE_RHO_TOLERANCE){
                exitflag = osqp_update_rho(work, rho_new);
		work->info->rho_updates += 1;
    }
	
    return exitflag;
}


void set_rho_vec(OSQPWorkspace * work){
    c_int i;
    work->settings->rho = c_min(c_max(work->settings->rho, RHO_MIN), RHO_MAX);

    for(i=0; i < work->data->m; i++){
        if ( (work->data->l[i] < -OSQP_INFTY*MIN_SCALING) && (work->data->u[i] > OSQP_INFTY*MIN_SCALING) ) {
            // Loose bounds
            work->constr_type[i] = -1;
            work->rho_vec[i] = RHO_MIN;
        } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) {
            // Equality constraints
            work->constr_type[i] = 1;
            work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho;
        } else {
            // Inequality constraints
            work->constr_type[i] = 0;
            work->rho_vec[i] = work->settings->rho;
        }
        work->rho_inv_vec[i] = 1. / work->rho_vec[i];
    }
}

c_int update_rho_vec(OSQPWorkspace * work){
    c_int i, exitflag, constr_type_changed;
    exitflag = 0;
    constr_type_changed = 0;

    for(i=0; i < work->data->m; i++){
        if ( (work->data->l[i] < -OSQP_INFTY*MIN_SCALING) && (work->data->u[i] > OSQP_INFTY*MIN_SCALING) ) {
            // Loose bounds
            if (work->constr_type[i] != -1){
                work->constr_type[i] = -1;
                work->rho_vec[i] = RHO_MIN;
                work->rho_inv_vec[i] = 1. / RHO_MIN;
                constr_type_changed = 1;
            }
        } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) {
            // Equality constraints
            if (work->constr_type[i] != 1){
                work->constr_type[i] = 1;
                work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho;
                work->rho_inv_vec[i] = 1. / work->rho_vec[i];
                constr_type_changed = 1;
            }
        } else {
            // Inequality constraints
            if (work->constr_type[i] != 0){
                work->constr_type[i] = 0;
                work->rho_vec[i] = work->settings->rho;
                work->rho_inv_vec[i] = 1. / work->settings->rho;
                constr_type_changed = 1;
            }
        }
    }

    // Update rho_vec in KKT matrix if constraints type has changed
    if (constr_type_changed == 1) {
        exitflag = work->linsys_solver->update_rho_vec(work->linsys_solver,
                work->rho_vec,
                work->data->m);
    }

    return exitflag;
}
#endif // EMBEDDED != 1



void swap_vectors(c_float ** a, c_float ** b){
    c_float * temp;
    temp = *b;
    *b = *a;
    *a = temp;
}


void cold_start(OSQPWorkspace *work) {

    vec_set_scalar(work->x, 0., work->data->n);
    vec_set_scalar(work->z, 0., work->data->m);
    vec_set_scalar(work->y, 0., work->data->m);

}


static void compute_rhs(OSQPWorkspace *work){
    c_int i; // Index
    for (i=0; i < work->data->n; i++){
        // Cycle over part related to x variables
        work->xz_tilde[i] = work->settings->sigma * work->x_prev[i] - work->data->q[i];
    }
    for (i = 0; i < work->data->m; i++){
        // Cycle over dual variable in the first step (nu)
        work->xz_tilde[i + work->data->n] = work->z_prev[i] - work->rho_inv_vec[i] * work->y[i];
    }

}


static void update_z_tilde(OSQPWorkspace *work){
    c_int i; // Index
    for (i = 0; i < work->data->m; i++){
        work->xz_tilde[i + work->data->n] = work->z_prev[i] + work->rho_inv_vec[i] * (work->xz_tilde[i + work->data->n] - work->y[i]);
    }
}


void update_xz_tilde(OSQPWorkspace * work){
    // Compute right-hand side
    compute_rhs(work);

    // Solve linear system
    work->linsys_solver->solve(work->linsys_solver, work->xz_tilde, work->settings);

    // Update z_tilde variable after solving linear system
    update_z_tilde(work);

}


void update_x(OSQPWorkspace * work){
    c_int i;

    // update x
    for (i = 0; i < work->data->n; i++){
        work->x[i] = work->settings->alpha * work->xz_tilde[i] +
            ((c_float) 1.0 - work->settings->alpha) * work->x_prev[i];
    }

    // update delta_x
    for (i = 0; i < work->data->n; i++){
        work->delta_x[i] = work->x[i] - work->x_prev[i];
    }

}

void update_z(OSQPWorkspace *work){
    c_int i;

    // update z
    for (i = 0; i < work->data->m; i++){
        work->z[i] = work->settings->alpha * work->xz_tilde[i + work->data->n] +
            ((c_float) 1.0 - work->settings->alpha) * work->z_prev[i] +
            work->rho_inv_vec[i] * work->y[i];
    }

    // project z
    project(work, work->z);

}



void update_y(OSQPWorkspace *work){
    c_int i; // Index
    for (i = 0; i < work->data->m; i++){

        work->delta_y[i] = work->rho_vec[i] *
            (work->settings->alpha * work->xz_tilde[i + work->data->n] +
             ((c_float) 1.0 - work->settings->alpha) * work->z_prev[i] - work->z[i]);
        work->y[i] += work->delta_y[i];

    }
}


c_float compute_obj_val(OSQPWorkspace *work, c_float * x) {
    c_float obj_val;

    obj_val = quad_form(work->data->P, x) +
        vec_prod(work->data->q, x, work->data->n);

    if (work->settings->scaling){
        obj_val *= work->scaling->cinv;
    }

    return obj_val;
}


c_float compute_pri_res(OSQPWorkspace * work, c_float * x, c_float * z){

    // NB: Use z_prev as working vector
    // pr = Ax - z

    mat_vec(work->data->A, x, work->Ax, 0);  // Ax
    vec_add_scaled(work->z_prev, work->Ax, z, work->data->m, -1);

    // If scaling active -> rescale residual
    if (work->settings->scaling && !work->settings->scaled_termination){
        return vec_scaled_norm_inf(work->scaling->Einv, work->z_prev, work->data->m);
    }

    // Return norm of the residual
    return vec_norm_inf(work->z_prev, work->data->m);
}


c_float compute_pri_tol(OSQPWorkspace * work, c_float eps_abs, c_float eps_rel){
    c_float max_rel_eps, temp_rel_eps;

    // max_rel_eps = max(||z||, ||A x||)
    if (work->settings->scaling && !work->settings->scaled_termination){
        // ||Einv * z||
        max_rel_eps = vec_scaled_norm_inf(work->scaling->Einv, work->z, work->data->m);
        // ||Einv * A * x||
        temp_rel_eps = vec_scaled_norm_inf(work->scaling->Einv, work->Ax, work->data->m);
        // Choose maximum
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
    } else { // No unscaling required
        // ||z||
        max_rel_eps = vec_norm_inf(work->z, work->data->m);
        // ||A * x||
        temp_rel_eps = vec_norm_inf(work->Ax, work->data->m);
        // Choose maximum
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
    }

    // eps_prim
    return eps_abs + eps_rel * max_rel_eps;
}



c_float compute_dua_res(OSQPWorkspace * work, c_float * x, c_float * y){

    // NB: Use x_prev as temporary vector
    // NB: Only upper triangular part of P is stored.
    // dr = q + A'*y + P*x

    // dr = q
    prea_vec_copy(work->data->q, work->x_prev, work->data->n);

    // P * x (upper triangular part)
    mat_vec(work->data->P, x, work->Px, 0); 

    // P' * x (lower triangular part with no diagonal)
    mat_tpose_vec(work->data->P, x, work->Px, 1, 1);

    // dr += P * x (full P matrix)
    vec_add_scaled(work->x_prev, work->x_prev, work->Px, work->data->n, 1);
    
    // dr += A' * y
    if (work->data->m > 0){
        mat_tpose_vec(work->data->A, y, work->Aty, 0, 0);
        vec_add_scaled(work->x_prev, work->x_prev, work->Aty, work->data->n, 1);
    }

    // If scaling active -> rescale residual
    if (work->settings->scaling && !work->settings->scaled_termination){
        return work->scaling->cinv * vec_scaled_norm_inf(work->scaling->Dinv, work->x_prev, work->data->n); 
    }

    return vec_norm_inf(work->x_prev, work->data->n);
}

c_float compute_dua_tol(OSQPWorkspace * work, c_float eps_abs, c_float eps_rel){
    c_float max_rel_eps, temp_rel_eps;

    // max_rel_eps = max(||q||, ||A' y|, ||P x||)
    if (work->settings->scaling && !work->settings->scaled_termination){
        
        // || Dinv q||
        max_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, work->data->q, work->data->n);
        
        // || Dinv A' y ||
        temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, work->Aty, work->data->n);
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);

        // || Dinv P x||
        temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv, work->Px, work->data->n);
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);

        // Multiply by cinv
        max_rel_eps *= work->scaling->cinv;

    } else { // No scaling required
        
        // ||q||
        max_rel_eps = vec_norm_inf(work->data->q, work->data->n);

        // ||A'*y||
        temp_rel_eps = vec_norm_inf(work->Aty, work->data->n);
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);

        // ||P*x||
        temp_rel_eps = vec_norm_inf(work->Px, work->data->n);
        max_rel_eps = c_max(max_rel_eps, temp_rel_eps);

    }

    // eps_dual
    return eps_abs + eps_rel * max_rel_eps;
}


c_int is_primal_infeasible(OSQPWorkspace * work, c_float eps_prim_inf){

    // This function checks for the primal infeasibility termination criteria.
    //
    // 1) A' * delta_y < eps * ||delta_y||
    //
    // 2) u'*max(delta_y, 0) + l'*min(delta_y, 0) < -eps * ||delta_y||
    //

    c_int i; // Index for loops
    c_float norm_delta_y;
    c_float ineq_lhs;

    // Compute infinity norm of delta_y
    if (work->settings->scaling && !work->settings->scaled_termination){ // Unscale if necessary
        // Use work->Adelta_x as temporary vector
        vec_ew_prod(work->scaling->E, work->delta_y, work->Adelta_x, work->data->m);
        norm_delta_y = vec_norm_inf(work->Adelta_x, work->data->m);
    } else{
        norm_delta_y = vec_norm_inf(work->delta_y, work->data->m);
    }

    if (norm_delta_y > eps_prim_inf){ // ||delta_y|| > 0

        ineq_lhs = 0;
        for (i = 0; i < work->data->m; i++){
            ineq_lhs += work->data->u[i] * c_max(work->delta_y[i], 0) + \
                        work->data->l[i] * c_min(work->delta_y[i], 0);
        }

        // Check if the condition is satisfied: ineq_lhs < -eps
        if (ineq_lhs < -eps_prim_inf * norm_delta_y ){
            // Compute and return ||A'delta_y|| < eps_prim_inf
            mat_tpose_vec(work->data->A, work->delta_y, work->Atdelta_y, 0, 0);
            if (work->settings->scaling && !work->settings->scaled_termination){ // Unscale if necessary
                vec_ew_prod(work->scaling->Dinv, work->Atdelta_y, work->Atdelta_y, work->data->n);
            }
            return vec_norm_inf(work->Atdelta_y, work->data->n) < eps_prim_inf * norm_delta_y;
        }

    }

    // Conditions not satisfied -> not primal infeasible
    return 0;

}


c_int is_dual_infeasible(OSQPWorkspace * work, c_float eps_dual_inf){
    // This function checks for the scaled dual infeasibility termination criteria.
    //
    // 1) q * delta_x < - eps * || delta_x ||
    //
    // 2) ||P * delta_x || < eps * || delta_x ||
    //
    // 3) -> (A * delta_x)_i > -eps * || delta_x ||,    l_i != -inf
    //    -> (A * delta_x)_i <  eps * || delta_x ||,    u_i != inf
    //


    c_int i; // Index for loops
    c_float norm_delta_x;
    c_float cost_scaling;

    // Compute norm of delta_x
    if (work->settings->scaling && !work->settings->scaled_termination){ // Unscale if necessary
        norm_delta_x = vec_scaled_norm_inf(work->scaling->D, work->delta_x, work->data->n);
        cost_scaling = work->scaling->c;
    } else{
        norm_delta_x = vec_norm_inf(work->delta_x, work->data->n);
        cost_scaling = 1.0;
    }

    // Prevent 0 division || delta_x || > 0
    if (norm_delta_x > eps_dual_inf){

        // Normalize delta_x by its norm
        /* vec_mult_scalar(work->delta_x, 1./norm_delta_x, work->data->n); */

        // Check first if q'*delta_x < 0
        if (vec_prod(work->data->q, work->delta_x, work->data->n) <
                - cost_scaling * eps_dual_inf * norm_delta_x){

            // Compute product P * delta_x (NB: P is store in upper triangular form)
            mat_vec(work->data->P, work->delta_x, work->Pdelta_x, 0);
	    mat_tpose_vec(work->data->P, work->delta_x, work->Pdelta_x, 1, 1);

            // Scale if necessary
            if (work->settings->scaling && !work->settings->scaled_termination){
                vec_ew_prod(work->scaling->Dinv, work->Pdelta_x, work->Pdelta_x, work->data->n);
            }
	    
            // Check if || P * delta_x || = 0
            if (vec_norm_inf(work->Pdelta_x, work->data->n) <
                    cost_scaling * eps_dual_inf * norm_delta_x){

                // Compute A * delta_x
                mat_vec(work->data->A, work->delta_x, work->Adelta_x, 0);

                // Scale if necessary
                if (work->settings->scaling && !work->settings->scaled_termination){
                    vec_ew_prod(work->scaling->Einv, work->Adelta_x, work->Adelta_x, work->data->m);
                }
		
                // De Morgan Law Applied to dual infeasibility conditions for A * x
                // NB: Note that 1e-06 is used to adjust the infinity value
                //      in case the problem is scaled.
                for (i = 0; i < work->data->m; i++){
                    if (((work->data->u[i] < OSQP_INFTY*MIN_SCALING) && (work->Adelta_x[i] >  eps_dual_inf * norm_delta_x)) ||
                            ((work->data->l[i] > -OSQP_INFTY*MIN_SCALING) && (work->Adelta_x[i] < -eps_dual_inf * norm_delta_x))){
                        // At least one condition not satisfied -> not dual infeasible
                        return 0;
                    }
                }

                // All conditions passed -> dual infeasible
                return 1;
            }
        }
    }

    // Conditions not satisfied -> not dual infeasible
    return 0;
}



void store_solution(OSQPWorkspace *work) {
#ifndef EMBEDDED
    c_float norm_vec;
#endif
    if ((work->info->status_val != OSQP_PRIMAL_INFEASIBLE) &&
            (work->info->status_val != OSQP_PRIMAL_INFEASIBLE_INACCURATE) &&
            (work->info->status_val != OSQP_DUAL_INFEASIBLE) &&
            (work->info->status_val != OSQP_DUAL_INFEASIBLE_INACCURATE)){
        prea_vec_copy(work->x, work->solution->x, work->data->n);   // primal
        prea_vec_copy(work->y, work->solution->y, work->data->m);   // dual

        if(work->settings->scaling) // Unscale solution if scaling has been performed
            unscale_solution(work);
    } else { // Problem primal or dual infeasible. Solution is NaN
        vec_set_scalar(work->solution->x, OSQP_NAN, work->data->n);
        vec_set_scalar(work->solution->y, OSQP_NAN, work->data->m);

#ifndef EMBEDDED
        // Normalize infeasibility certificates if embedded is off 
        // NB: It requires a division
        if ((work->info->status_val != OSQP_PRIMAL_INFEASIBLE) ||
                ((work->info->status_val != OSQP_PRIMAL_INFEASIBLE_INACCURATE))){
            norm_vec = vec_norm_inf(work->delta_y, work->data->m);
            vec_mult_scalar(work->delta_y, 1./norm_vec, work->data->m);
        }
        if ((work->info->status_val != OSQP_DUAL_INFEASIBLE) ||
                ((work->info->status_val != OSQP_DUAL_INFEASIBLE_INACCURATE))){
            norm_vec = vec_norm_inf(work->delta_x, work->data->n);
            vec_mult_scalar(work->delta_x, 1./norm_vec, work->data->n);
        }
        
#endif
        
        // Cold start iterates to 0 for next runs
        cold_start(work);
    }
}


void update_info(OSQPWorkspace *work, c_int iter, c_int compute_objective, c_int polish){
    c_float * x, * z, * y;  // Allocate pointers to variables
    c_float * obj_val, * pri_res, *dua_res;  // objective value, residuals

#ifdef PROFILING
    c_float *run_time;  // Execution time
#endif

#ifndef EMBEDDED
    if (polish){
        x = work->pol->x;
        y = work->pol->y;
        z = work->pol->z;
        obj_val = &work->pol->obj_val;
        pri_res = &work->pol->pri_res;
        dua_res = &work->pol->dua_res;
#ifdef PROFILING
        run_time = &work->info->polish_time;
#endif
    } else {
#endif // EMBEDDED
        x = work->x;
        y = work->y;
        z = work->z;
        obj_val = &work->info->obj_val;
        pri_res = &work->info->pri_res;
        dua_res = &work->info->dua_res;
        work->info->iter = iter; // Update iteration number
#ifdef PROFILING
        run_time = &work->info->solve_time;
#endif
#ifndef EMBEDDED
    }
#endif


    // Compute the objective if needed
    if (compute_objective){
        *obj_val = compute_obj_val(work, x);
    }

    // Compute primal residual
    if (work->data->m == 0) {
        // No constraints -> Always primal feasible
        *pri_res = 0.;
    } else {
        *pri_res = compute_pri_res(work, x, z);
    }

    // Compute dual residual
    *dua_res = compute_dua_res(work, x, y);

    // Update timing
#ifdef PROFILING
    *run_time = toc(work->timer);
#endif

#ifdef PRINTING
    work->summary_printed = 0;  // The just updated info have not been printed
#endif
}


void reset_info(OSQPInfo *info){ 
#ifdef PROFILING
    // Initialize info values.
    info->solve_time = 0.0;  // Solve time to zero
#ifndef EMBEDDED
    info->polish_time = 0.0;  // Polish time to zero
#endif
    // NB: We do not reset the setup_time because it is performed only once
#endif
    
    update_status(info, OSQP_UNSOLVED);  // Problem is unsolved

#if EMBEDDED != 1
    info->rho_updates = 0;   // Rho updates are now 0
#endif     

}


void update_status(OSQPInfo *info, c_int status_val) {
    // Update status value
    info->status_val = status_val;

    // Update status string depending on status val
    if(status_val == OSQP_SOLVED)
        c_strcpy(info->status, "solved");
    if(status_val == OSQP_SOLVED_INACCURATE)
        c_strcpy(info->status, "solved inaccurate");
    else if (status_val == OSQP_PRIMAL_INFEASIBLE)
        c_strcpy(info->status, "primal infeasible");
    else if (status_val == OSQP_PRIMAL_INFEASIBLE_INACCURATE)
        c_strcpy(info->status, "primal infeasible inaccurate");
    else if (status_val == OSQP_UNSOLVED)
        c_strcpy(info->status, "unsolved");
    else if (status_val == OSQP_DUAL_INFEASIBLE)
        c_strcpy(info->status, "dual infeasible");
    else if (status_val == OSQP_DUAL_INFEASIBLE_INACCURATE)
        c_strcpy(info->status, "dual infeasible inaccurate");
    else if (status_val == OSQP_MAX_ITER_REACHED)
        c_strcpy(info->status, "maximum iterations reached");
    else if (status_val == OSQP_SIGINT)
        c_strcpy(info->status, "interrupted");
}



c_int check_termination(OSQPWorkspace *work, c_int approximate){
    c_float eps_prim, eps_dual, eps_prim_inf, eps_dual_inf;
    c_int exitflag;
    c_int prim_res_check, dual_res_check, prim_inf_check, dual_inf_check;
    c_float eps_abs, eps_rel;

    // Initialize variables to 0
    exitflag = 0;
    prim_res_check = 0; dual_res_check = 0;
    prim_inf_check = 0; dual_inf_check = 0;

    // Initialize tolerances
    eps_abs = work->settings->eps_abs;
    eps_rel = work->settings->eps_rel;
    eps_prim_inf = work->settings->eps_prim_inf;
    eps_dual_inf = work->settings->eps_dual_inf;

    // If approximate solution required, increase tolerances by 10
    if (approximate){
        eps_abs *= 10;
        eps_rel *= 10;
        eps_prim_inf *= 10;
        eps_dual_inf *= 10;
    }

    // Check residuals
    if (work->data->m == 0){
        prim_res_check = 1;  // No contraints -> Primal feasibility always satisfied
    }
    else {
        // Compute primal tolerance
        eps_prim = compute_pri_tol(work, eps_abs, eps_rel);

        // Primal feasibility check
        if (work->info->pri_res < eps_prim) {
            prim_res_check = 1;
        } else {
            // Primal infeasibility check
            prim_inf_check = is_primal_infeasible(work, eps_prim_inf);
        }
    }  // End check if m == 0

    // Compute dual tolerance
    eps_dual = compute_dua_tol(work, eps_abs, eps_rel);

    // Dual feasibility check
    if (work->info->dua_res < eps_dual) {
        dual_res_check = 1;
    } else {
        // Check dual infeasibility
        dual_inf_check = is_dual_infeasible(work, eps_dual_inf);
    }

    // Compare checks to determine solver status
    if (prim_res_check && dual_res_check){
        // Update final information
        if (approximate){
            update_status(work->info, OSQP_SOLVED_INACCURATE);
        } else {
            update_status(work->info, OSQP_SOLVED);
        }
        exitflag = 1;
    }
    else if (prim_inf_check){
        // Update final information
        if (approximate){
            update_status(work->info, OSQP_PRIMAL_INFEASIBLE_INACCURATE);
        } else {
            update_status(work->info, OSQP_PRIMAL_INFEASIBLE);
        }
        if (work->settings->scaling && !work->settings->scaled_termination){
            // Update infeasibility certificate
            vec_ew_prod(work->scaling->E, work->delta_y, work->delta_y, work->data->m);
        }
        work->info->obj_val = OSQP_INFTY;
        exitflag = 1;
    }
    else if (dual_inf_check){
        // Update final information
        if (approximate){
            update_status(work->info, OSQP_DUAL_INFEASIBLE_INACCURATE);
        } else {
            update_status(work->info, OSQP_DUAL_INFEASIBLE);
        }
        if (work->settings->scaling && !work->settings->scaled_termination){
            // Update infeasibility certificate
            vec_ew_prod(work->scaling->D, work->delta_x, work->delta_x, work->data->n);
        }
        work->info->obj_val = -OSQP_INFTY;
        exitflag = 1;
    }

    return exitflag;

}


#ifndef EMBEDDED


c_int validate_data(const OSQPData * data){
    c_int j;

    if(!data){
#ifdef PRINTING
        c_print("Missing data!\n");
#endif
        return 1;
    }

    // General dimensions Tests
    if (data->n <= 0 || data->m < 0){
#ifdef PRINTING
        c_print("n must be positive and m nonnegative; n = %i, m = %i\n",
                (int)data->n, (int)data->m);
#endif
        return 1;
    }

    // Matrix P
    if (data->P->m != data->n ){
#ifdef PRINTING
        c_print("P does not have dimension n x n with n = %i\n", (int)data->n);
#endif
        return 1;
    }
    if (data->P->m != data->P->n ){
#ifdef PRINTING
        c_print("P is not square\n");
#endif
        return 1;
    }

    // Matrix A
    if (data->A->m != data->m || data->A->n != data->n){
#ifdef PRINTING
        c_print("A does not have dimension m x n with m = %i and n = %i\n",
                (int)data->m, (int)data->n);
#endif
        return 1;
    }

    // Lower and upper bounds
    for (j = 0; j < data->m; j++) {
        if (data->l[j] > data->u[j]) {
#ifdef PRINTING
            c_print("Lower bound at index %d is greater than upper bound: %.4e > %.4e\n",
                    (int)j, data->l[j], data->u[j]);
#endif
            return 1;
        }
    }

    // TODO: Complete with other checks

    return 0;
}


c_int validate_linsys_solver(c_int linsys_solver){
    if (linsys_solver != SUITESPARSE_LDL_SOLVER &&
            linsys_solver != MKL_PARDISO_SOLVER){
        return 1;
    }
    // TODO: Add more solvers in case

    // Valid solver
    return 0;
}


c_int validate_settings(const OSQPSettings * settings){
    if (!settings){
#ifdef PRINTING
        c_print("Missing settings!\n");
#endif
        return 1;
    }
    if (settings->scaling < 0) {
#ifdef PRINTING
        c_print("scaling must be nonnegative\n");
#endif
        return 1;
    }
    if (settings->adaptive_rho != 0 && settings->adaptive_rho != 1) {
#ifdef PRINTING
        c_print("adaptive_rho must be either 0 or 1\n");
#endif
        return 1;
    }
    if (settings->adaptive_rho_interval < 0) {
#ifdef PRINTING
        c_print("adaptive_rho_interval must be nonnegative\n");
#endif
        return 1;
    }
    if (settings->adaptive_rho_fraction <= 0) {
#ifdef PRINTING
        c_print("adaptive_rho_fraction must be positive\n");
#endif
        return 1;
    }
    if (settings->adaptive_rho_tolerance < 1) {
#ifdef PRINTING
        c_print("adaptive_rho_tolerance must be >= 1\n");
#endif
        return 1;
    }
    if (settings->polish_refine_iter < 0) {
#ifdef PRINTING
        c_print("polish_refine_iter must be nonnegative\n");
#endif
        return 1;
    }
    if (settings->rho <= 0) {
#ifdef PRINTING
        c_print("rho must be positive\n");
#endif
        return 1;
    }
    if (settings->delta <= 0) {
#ifdef PRINTING
        c_print("delta must be positive\n");
#endif
        return 1;
    }
    if (settings->max_iter <= 0) {
#ifdef PRINTING
        c_print("max_iter must be positive\n");
#endif
        return 1;
    }
    if (settings->eps_abs <= 0) {
#ifdef PRINTING
        c_print("eps_abs must be positive\n");
#endif
        return 1;
    }
    if (settings->eps_rel <= 0) {
#ifdef PRINTING
        c_print("eps_rel must be positive\n");
#endif
        return 1;
    }
    if (settings->eps_prim_inf <= 0) {
#ifdef PRINTING
        c_print("eps_prim_inf must be positive\n");
#endif
        return 1;
    }
    if (settings->eps_dual_inf <= 0) {
#ifdef PRINTING
        c_print("eps_dual_inf must be positive\n");
#endif
        return 1;
    }
    if (settings->alpha <= 0 || settings->alpha >= 2) {
#ifdef PRINTING
        c_print("alpha must be between 0 and 2\n");
#endif
        return 1;
    }
    if (validate_linsys_solver(settings->linsys_solver)){
#ifdef PRINTING
        c_print("linsys_solver not recognized\n");
#endif
        return 1;
    }
    if (settings->verbose != 0 && settings->verbose != 1) {
#ifdef PRINTING
        c_print("verbose must be either 0 or 1\n");
#endif
        return 1;
    }
    if (settings->scaled_termination != 0 && settings->scaled_termination != 1) {
#ifdef PRINTING
        c_print("scaled_termination must be either 0 or 1\n");
#endif
        return 1;
    }
    if (settings->check_termination < 0) {
#ifdef PRINTING
        c_print("check_termination must be nonnegative\n");
#endif
        return 1;
    }
    if (settings->warm_start != 0 && settings->warm_start != 1) {
#ifdef PRINTING
        c_print("warm_start must be either 0 or 1\n");
#endif
        return 1;
    }

    return 0;

}

#endif  // #ifndef EMBEDDED
