#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_vector.h>
#include "cimple_mpc_computation.h"

/**
 * Set up weight matrices for the quadratic problem
 */
polytope * set_cost_function(gsl_matrix *P,
                             gsl_vector *q,
                             gsl_matrix *L,
                             gsl_vector *M,
                             current_state *now,
                             system_dynamics *s_dyn,
                             cost_function *f_cost,
                             size_t time_horizon){

    size_t N = time_horizon;
    size_t n = s_dyn->A->size1;
    size_t m = s_dyn->B->size2;

    /*Calculate P and q */
    //           |B     0     0     0   0|
    //           |AB    B     0     0   0|
    //      Ct = |A^2B  AB    B     0   0|
    //           |A^3B  A^2B  AB    B   0|
    //           |A^4B  A^3B  A^2B  AB  B|

    //            |I    0    0    0  0|
    //            |A    I    0    0  0|
    //      A_K = |A^2  A    I    0  0|
    //            |A^3  A^2  A    I  0|
    //            |A^4  A^3  A^2  A  I|

    //            |A  |
    //            |A^2|
    //      A_N = |A^3|
    //            |A^4|
    //            |A^5|

    //P = Q2 + Ct^T.R2.Ct
    //q = {([x^T.A_N^T + (A_K.K_hat)^T].[R2.Ct])+(0.5*r^T.Ct)}^T

    // symmetrize
    gsl_matrix * Q2 = gsl_matrix_alloc(N*m, N*m);
    gsl_matrix_view Q_view = gsl_matrix_submatrix(f_cost->Q,(f_cost->Q->size1-N),(f_cost->Q->size2-N),(N),(N));
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &Q_view.matrix,&Q_view.matrix, 0.0, Q2);
    gsl_matrix * R2 = gsl_matrix_alloc(N*n, N*n);
    gsl_matrix_view R_view = gsl_matrix_submatrix(f_cost->R,(f_cost->R->size1-N),(f_cost->R->size2-N),(N),(N));
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &R_view.matrix,&R_view.matrix, 0.0, R2);

    //Calculate P
    gsl_matrix * R2_dot_Ct = gsl_matrix_alloc(N*n, N*m);
    gsl_matrix_set_zero(R2_dot_Ct);
    gsl_matrix_view Ct_view = gsl_matrix_submatrix(s_dyn->aux_matrices->Ct,0,0,N,N);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, R2, &Ct_view.matrix, 0.0, R2_dot_Ct);
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &Ct_view.matrix, R2_dot_Ct, 0.0, P);
    gsl_matrix_add(P, Q2);

    //Clean up!
    gsl_matrix_free(R2);
    gsl_matrix_free(Q2);

    //Calculate q
    gsl_vector_set_zero(q);
    gsl_vector * A_N_dot_x = gsl_vector_alloc(N*n);
    gsl_vector_set_zero(A_N_dot_x);
    gsl_matrix_view A_N_view = gsl_matrix_submatrix(s_dyn->aux_matrices->A_N,0,0,N,1);
    gsl_blas_dgemv(CblasNoTrans, 1.0, &A_N_view.matrix, now->x, 0.0, A_N_dot_x);
    gsl_vector * A_K_dot_K_hat = gsl_vector_alloc(N*n);
    gsl_vector_set_zero(A_K_dot_K_hat);
    gsl_matrix_view A_K_view = gsl_matrix_submatrix(s_dyn->aux_matrices->A_K,0,0,N,N);
    gsl_vector_view K_hat_view = gsl_vector_subvector(s_dyn->aux_matrices->K_hat,0,N);
    gsl_blas_dgemv(CblasNoTrans, 1.0, &A_K_view.matrix, &K_hat_view.vector, 0.0, A_K_dot_K_hat);

    //[x^T.A_N^T + (A_K.K_hat)^T]^T
    gsl_vector_add(A_N_dot_x, A_K_dot_K_hat);

    //{([x^T.A_N^T + (A_K.K_hat)^T].[R2.Ct])}^T
    //R2_dot_Ct was calculated earlier
    gsl_blas_dgemv(CblasTrans, 1.0, R2_dot_Ct, A_N_dot_x, 0.0, q);

    //(0.5*r^T.Ct)
    gsl_vector *Right_side = gsl_vector_alloc(N*m);
    gsl_vector_view r_view = gsl_vector_subvector(f_cost->r,(f_cost->r->size-N),N);
    gsl_blas_dgemv(CblasTrans, 0.5, &Ct_view.matrix, &r_view.vector, 0.0, Right_side);
    gsl_vector_add(q, Right_side);

    //Clean up!
    gsl_matrix_free(R2_dot_Ct);
    gsl_vector_free(A_N_dot_x);
    gsl_vector_free(A_K_dot_K_hat);
    gsl_vector_free(Right_side);

    /*GUROBI Convex Optimization: solve quadratic problem*/

    //Minimizing
    polytope * constraints_polytope = polytope_alloc(L->size1, L->size2);
    gsl_matrix_memcpy(constraints_polytope->H,L);
    gsl_vector_memcpy(constraints_polytope->G,M);

    //back to gsl polytope
    polytope *opt_constraints = polytope_minimize(constraints_polytope);
    polytope_free(constraints_polytope);

//    //Minimizing
//    polytope * constraints_polytope = polytope_alloc(L->size1, L->size2);
//    gsl_matrix_memcpy(constraints_polytope->H,L);
//    gsl_vector_memcpy(constraints_polytope->G,M);
//
//    printf("\n\n\n\nHALT\n\n\n\n\n");
//    gsl_matrix_print(constraints_polytope->H,"H");
//    gsl_vector_print(constraints_polytope->G,"G");
//
//    polytope *opt_constraints = polytope_minimize(constraints_polytope);
//    polytope_free(constraints_polytope);
//
//
//    printf("\n\n\n\nNOOOOO\n\n\n\n\n");
//    gsl_matrix_print(opt_constraints->H,"H");
//    gsl_vector_print(opt_constraints->G,"G");
//
//        polytope *opt_constraints = polytope_alloc(10,N);
//        gsl_vector_set(opt_constraints->G,0,1);
//        gsl_vector_set(opt_constraints->G,1,-1);
//        gsl_vector_set(opt_constraints->G,2,1);
//        gsl_vector_set(opt_constraints->G,3,-1);
//        gsl_vector_set(opt_constraints->G,4,1);
//        gsl_vector_set(opt_constraints->G,5,-1);
//        gsl_vector_set(opt_constraints->G,6,1);
//        gsl_vector_set(opt_constraints->G,7,-1);
//        gsl_vector_set(opt_constraints->G,8,1);
//        gsl_vector_set(opt_constraints->G,9,-1);
//        gsl_matrix_set(opt_constraints->H,0,0,0.2);
//        gsl_matrix_set(opt_constraints->H,0,1,0);
//        gsl_matrix_set(opt_constraints->H,0,2,0);
//        gsl_matrix_set(opt_constraints->H,0,3,0);
//        gsl_matrix_set(opt_constraints->H,0,4,0);
//
//        gsl_matrix_set(opt_constraints->H,1,0,-20);
//        gsl_matrix_set(opt_constraints->H,1,1,0);
//        gsl_matrix_set(opt_constraints->H,1,2,0);
//        gsl_matrix_set(opt_constraints->H,1,3,0);
//        gsl_matrix_set(opt_constraints->H,1,4,0);
//        gsl_matrix_set(opt_constraints->H,2,0,0.2);
//        gsl_matrix_set(opt_constraints->H,2,1,0.2);
//        gsl_matrix_set(opt_constraints->H,2,2,0);
//        gsl_matrix_set(opt_constraints->H,2,3,0);
//        gsl_matrix_set(opt_constraints->H,2,4,0);
//
//        gsl_matrix_set(opt_constraints->H,3,0,-20);
//        gsl_matrix_set(opt_constraints->H,3,1,-20);
//        gsl_matrix_set(opt_constraints->H,3,2,0);
//        gsl_matrix_set(opt_constraints->H,3,3,0);
//        gsl_matrix_set(opt_constraints->H,3,4,0);
//        gsl_matrix_set(opt_constraints->H,4,0,0.2);
//        gsl_matrix_set(opt_constraints->H,4,1,0.2);
//        gsl_matrix_set(opt_constraints->H,4,2,0.2);
//        gsl_matrix_set(opt_constraints->H,4,3,0);
//        gsl_matrix_set(opt_constraints->H,4,4,0);
//
//        gsl_matrix_set(opt_constraints->H,5,0,-20);
//        gsl_matrix_set(opt_constraints->H,5,1,-20);
//        gsl_matrix_set(opt_constraints->H,5,2,-20);
//        gsl_matrix_set(opt_constraints->H,5,3,0);
//        gsl_matrix_set(opt_constraints->H,5,4,0);
//        gsl_matrix_set(opt_constraints->H,6,0,0.2);
//        gsl_matrix_set(opt_constraints->H,6,1,0.2);
//        gsl_matrix_set(opt_constraints->H,6,2,0.2);
//        gsl_matrix_set(opt_constraints->H,6,3,0.2);
//        gsl_matrix_set(opt_constraints->H,6,4,0);
//
//        gsl_matrix_set(opt_constraints->H,7,0,-20);
//        gsl_matrix_set(opt_constraints->H,7,1,-20);
//        gsl_matrix_set(opt_constraints->H,7,2,-20);
//        gsl_matrix_set(opt_constraints->H,7,3,-20);
//        gsl_matrix_set(opt_constraints->H,7,4,0);
//        gsl_matrix_set(opt_constraints->H,8,0,0.2);
//        gsl_matrix_set(opt_constraints->H,8,1,0.2);
//        gsl_matrix_set(opt_constraints->H,8,2,0.2);
//        gsl_matrix_set(opt_constraints->H,8,3,0.2);
//        gsl_matrix_set(opt_constraints->H,8,4,0.2);
//
//        gsl_matrix_set(opt_constraints->H,9,0,-20);
//        gsl_matrix_set(opt_constraints->H,9,1,-20);
//        gsl_matrix_set(opt_constraints->H,9,2,-20);
//        gsl_matrix_set(opt_constraints->H,9,3,-20);
//        gsl_matrix_set(opt_constraints->H,9,4,-20);
    return opt_constraints;
}

/**
 * Set up GUROBI environment and solve qp
 */
void compute_optimal_control_qp(gsl_matrix *low_u,
                                double *low_cost,
                                gsl_matrix *P,
                                gsl_vector* q,
                                polytope *opt_constraints,
                                size_t time_horizon,
                                size_t n){

    //Initialization for qp in gurobi
    GRBenv   *env   = NULL;
    GRBmodel *model = NULL;
    int       error = 0;
    double    sol[time_horizon];
    int       optimstatus;
    double    cost;

    /* Create environment */

    error = GRBloadenv(&env, "qp.log");
    if (error) goto QUIT;
    error = GRBsetintparam(env, GRB_INT_PAR_OUTPUTFLAG, 0);
    if (error) goto QUIT;

    /* Create an empty model */

    error = GRBnewmodel(env, &model, "qp", 0, NULL, NULL, NULL, NULL, NULL);
    if (error) goto QUIT;
    /* Add variables */

    error = GRBaddvars(model, (int)time_horizon, 0, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
                       NULL);
    if (error) goto QUIT;

    /* Quadratic objective terms */

    error = gsl_matrix_to_qpterm_gurobi(P, model, time_horizon);
    if (error) goto QUIT;

    /* Linear objective term */

    error = gsl_vector_to_linterm_gurobi(q, model, time_horizon);
    if (error) goto QUIT;

    /* Add constraints*/

    error = polytope_to_constraints_gurobi(opt_constraints,model,time_horizon);
    if (error) goto QUIT;
    /* Optimize model */

    error = GRBoptimize(model);
    if (error) goto QUIT;

    /* Write model to 'qp.lp' */

    error = GRBwrite(model, "qp.lp");
    if (error) goto QUIT;

    /* Capture solution information */

    error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
    if (error) goto QUIT;

    error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &cost);
    if (error) goto QUIT;

    error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, (int)time_horizon, sol);
    if (error) goto QUIT;

    printf("\nOptimization complete\n");
    if (optimstatus == GRB_OPTIMAL) {
        printf("\nOptimal objective: %.4e\n", cost);

        for(size_t i=0;i<time_horizon;i++){
            printf("  u%d=%.4f", (int)i,sol[i]);

        }
        printf("\n");

    } else if (optimstatus == GRB_INF_OR_UNBD) {
        printf("\nModel is infeasible or unbounded\n");
    } else {
        printf("\nOptimization was stopped early\n");
    }

    if(cost < *low_cost){
        for(size_t i = 0; i<n; i++){
            for(size_t j = 0; j<time_horizon; j++){
                gsl_matrix_set(low_u,i, j,sol[j]);
            }
        }
        *low_cost = cost;
    }

    QUIT:

    /* Error reporting */

    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }

    /* Free model */
    error = GRBfreemodel(model);
    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }

    /* Free environment */

    GRBfreeenv(env);
}

/**
 * Calculate (optimal) input that will be applied to take plant from current state (now) to target_abs_state.
 */
void get_input (gsl_matrix * low_u,
                current_state * now,
                discrete_dynamics * d_dyn,
                system_dynamics * s_dyn,
                int target_abs_state,
                cost_function * f_cost,
                size_t current_time_horizon,
                polytope **polytope_list_backup) {

    //Set input back to zero (safety precaution)
    gsl_matrix_set_zero(low_u);

    //Help variables:
    size_t n = now->x->size;
    size_t N = current_time_horizon;

    double low_cost = INFINITY;
    double err_weight = f_cost->distance_error_weight;

    //Set start region (depends on conservative path or not)
    polytope *P1;

    int start = now->current_abs_state;

    if (d_dyn->conservative == 1){
        // Take convex hull or polytope as starting polytope P1

        // if convex_hull != NULL => hull was computed => several polytopes in that region
        if (d_dyn->abstract_states_set[start]->convex_hull->H != NULL){
            P1 = d_dyn->abstract_states_set[start]->convex_hull;
        } else{
            P1 = d_dyn->abstract_states_set[start]->cells[0]->polytope_description;
        }
    } else{
        // Take original proposition preserving abstract state as constraint
        // must be single polytope (ensuring convex)

        if (d_dyn->original_regions[start]->cells_count == 1){
            P1 = d_dyn->original_regions[start]->cells[0]->polytope_description;
        } else {
            fprintf(stderr, "\nIn Region of polytopes(%d): `conservative = False` arg requires that original abstract_states_set be convex\n", now->current_abs_state);
            exit(EXIT_FAILURE);
        }
    }

    //Find optimal path into target region
    //by finding polytope that is easiest to reach in target region

    // for each polytope in target region
    for (int i = 0; i < d_dyn->abstract_states_set[target_abs_state]->cells_count; i++){
        polytope *P3 = d_dyn->abstract_states_set[target_abs_state]->cells[i]->polytope_description;

        //Finding a path to target region
        if (err_weight > 0){
            //Set r (=xc.R)(from default to polytope specific):
            double *xc = P3->chebyshev_center;

            //r[n*(N-1), n*N] += err_weight * xc;
            for (size_t j = n * (N - 1); j < n*N; j++){
                double * element_value = gsl_vector_ptr(f_cost->r, j);
                * element_value += err_weight * xc[i];
                gsl_vector_set(f_cost->r, j, *element_value);
            }

            search_better_path(low_u, now,s_dyn, P1, P3,d_dyn->ord, N, f_cost, &low_cost, polytope_list_backup, d_dyn->time_horizon);
            //Reset r vector to default values
            for (size_t j = n * (N - 1); j < n*N; j++){
                double * element_value = gsl_vector_ptr(f_cost->r, j);
                * element_value -= err_weight * xc[i];
                gsl_vector_set(f_cost->r, j, * element_value);
            }

        } else{
            search_better_path(low_u, now,s_dyn, P1, P3,d_dyn->ord, N, f_cost, &low_cost, polytope_list_backup, d_dyn->time_horizon);
        }
    }

    if (low_cost == INFINITY){
        //raise Exception
        fprintf(stderr, "\nget_input: Did not find any trajectory\n");
        exit(EXIT_FAILURE);
    }

};

/**
 * Calculates (optimal) input to reach desired state (P3) from current state (now) through convex optimization
 */
void search_better_path(gsl_matrix *low_u,
                        current_state *now,
                        system_dynamics *s_dyn,
                        polytope *P1,
                        polytope *P3,
                        int ord ,
                        size_t time_horizon,
                        cost_function * f_cost,
                        double *low_cost,
                        polytope **polytope_list_backup,
                        size_t total_time){

    //Auxiliary variables
    size_t N = time_horizon;
    size_t n = s_dyn->A->size2;
    size_t m = s_dyn->B->size2;

    //Build list of polytopes that the state is going to be in in the next N time steps
    polytope **polytope_list = malloc(sizeof(polytope)*(N+1));
    polytope_list[0] = polytope_alloc(P1->H->size1,P1->H->size2);
    gsl_matrix_memcpy(polytope_list[0]->H, P1->H);
    gsl_vector_memcpy(polytope_list[0]->G, P1->G);

    for (int i = 1; i < N; i++) {
        polytope_list[i] = polytope_alloc(P1->H->size1,P1->H->size2);
        gsl_matrix_memcpy(polytope_list[i]->H, P1->H);
        gsl_vector_memcpy(polytope_list[i]->G, P1->G);
    }
    polytope_list[N] = polytope_alloc(P3->H->size1, P3->H->size2);
    gsl_matrix_memcpy(polytope_list[N]->H, P3->H);
    gsl_vector_memcpy(polytope_list[N]->G, P3->G);

    size_t sum_polytope_sizes = 0;
    for(int i = 0; i<N+1; i++){
        sum_polytope_sizes += polytope_list[i]->H->size1;
    }

    polytope *constraints = set_path_constraints(now, s_dyn, polytope_list, N);

    //Updating backup list of polytopes
    //If polytope list doesn't have to be initialized completely, old ones have first to be destroyed:
    if(N< total_time) {
        for (size_t i = total_time; i > total_time - N - 1; i--) {
            polytope_free(polytope_list_backup[i]);
        }
    }
    //List is updated (or created, if total_time == N)
    for(size_t i = total_time-N; i< total_time+1; i++){
        size_t k =i-(total_time-N);
        polytope_list_backup[i] = polytope_alloc(polytope_list[k]->H->size1,polytope_list[k]->H->size2);
        gsl_matrix_memcpy(polytope_list_backup[i]->H,polytope_list[k]->H);
        gsl_vector_memcpy(polytope_list_backup[i]->G,polytope_list[k]->G);
    }

    for(int i = 0; i< N+1; i++){
        polytope_free(polytope_list[i]);
    }
    free(polytope_list);

    if (ord == 2){
        gsl_matrix * P = gsl_matrix_alloc(N*m, N*m);
        gsl_vector * q = gsl_vector_alloc(N*m);

        polytope *opt_constraints = set_cost_function(P, q, constraints->H, constraints->G, now, s_dyn, f_cost, N);
        compute_optimal_control_qp(low_u, low_cost, P, q, opt_constraints, N, n);
        polytope_free(opt_constraints);
        gsl_vector_free(q);
        gsl_matrix_free(P);

    }
    polytope_free(constraints);
};

/**
 * Compute a polytope that constraints the system over the next N time steps to fullfill the GR(1) specifications
 */
polytope * set_path_constraints(current_state * now,
                                system_dynamics * s_dyn,
                                polytope **list_polytopes,
                                size_t N){
    //Disturbance assumed at every step and full dimension of s_dyn.Wset

    // Help variables
    size_t n = s_dyn->A->size2;  // State space dimension
    size_t m = s_dyn->B->size2;
    size_t sum_polytope_dim = 0; // Sum of dimension n of all polytopes in the list
    polytope *scaled_W_set = polytope_linear_transform(s_dyn->W_set, s_dyn->E); // multiplication: EW

    polytope **robust_polytope_list = malloc(sizeof(polytope)*(N+1));
    for(size_t i = 0; i < N+1; i++){
        //Subtract EW of polytope to make them robust against disturbances
        robust_polytope_list[i] = polytope_pontryagin(list_polytopes[i], scaled_W_set);
        sum_polytope_dim += robust_polytope_list[i]->H->size1;

    }

    /* INITIALIZE MATRICES: Lk, Mk, constraints*/
    polytope *constraints = polytope_alloc(sum_polytope_dim, n*(N+1));
    gsl_matrix_set_zero(constraints->H);
    gsl_vector_set_zero(constraints->G);
    /*
     *     |G_0|
     *     |G_1|
     *Mk = |G_2| dim[sum_polytope_dim x 1]
     *     |G_3|
     *     |G_4|
     *
     *   with M = |Mk|
     *            |Mu|
     * */

    /*
     *                 |H_0 0  0  0  0 |
     *                 |0 H_1  0  0  0 |
     * constaints->H=  |0  0  H_2 0  0 | dim[sum_polytope_dim x n*N]
     *                 |0  0   0 H_3 0 |
     *                 |0  0   0  0 H_4|
     * */

    //Loop over N and polytopes in parallel
    size_t polytope_count = 0;
    for(int i = 0; i<N+1; i++){

        //Set diagonal block N of constaints->H
        for(size_t j = 0; j<n; j++){

            for(size_t k = 0; k<robust_polytope_list[i]->H->size1; k++){
                gsl_matrix_set(constraints->H,polytope_count+k, j+(i*n), gsl_matrix_get(robust_polytope_list[i]->H,k,j));
            }
        }

        // Build constraints->g
        for(size_t j = 0; j<robust_polytope_list[i]->G->size; j++){
            // Set entries Mk[j+polytope_count] = G_i[j]
            gsl_vector_set(constraints->G,j+polytope_count,gsl_vector_get(robust_polytope_list[i]->G,j));
        }

        polytope_count += robust_polytope_list[i]->H->size1;

    }
    /*Update L and M*/

    // Lk = H_constaints->Hdiag.L_default
    gsl_matrix_view L_default_view = gsl_matrix_submatrix(s_dyn->aux_matrices->L_default,0,0,n*(N+1),(m*N)+n);
    gsl_matrix *L_full = gsl_matrix_alloc(sum_polytope_dim, (m*N)+n);
    gsl_vector *M_full = gsl_vector_alloc(sum_polytope_dim);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, constraints->H, &L_default_view.matrix ,0.0,L_full);

    //Update M such that it includes the uncertainty of noise
    gsl_vector_memcpy(M_full, constraints->G);

    // Remove first constraints on x(0) they are obviously already satisfied
    // L_x = L[{(polytope[0]+1),(dim_n(L))},{1,n}]
    gsl_matrix_view L_x = gsl_matrix_submatrix(L_full, robust_polytope_list[0]->H->size1, 0, (L_full->size1)-(robust_polytope_list[0]->H->size1), n);
    // L_u = L[{(polytope[0]+1),(dim_n(L))},{(n+1),(dim_m(L))}]
    gsl_matrix_view L_u = gsl_matrix_submatrix(L_full, robust_polytope_list[0]->H->size1, n, (L_full->size1)-(robust_polytope_list[0]->H->size1), L_full->size2 - n);
    // M_view = M[{(polytope[0]+1),(dim_n(M))}]
    gsl_vector_view M_view = gsl_vector_subvector(M_full, robust_polytope_list[0]->H->size1, M_full->size-robust_polytope_list[0]->H->size1);

    //M = M-(L_x.x)
    gsl_vector * L_x_dot_X0 = gsl_vector_alloc((L_full->size1)-(robust_polytope_list[0]->H->size1));
    gsl_blas_dgemv(CblasNoTrans, 1.0, &L_x.matrix, now->x, 0.0, L_x_dot_X0);
    gsl_vector_sub(&M_view.vector, L_x_dot_X0);

    polytope * return_constraints = polytope_alloc(constraints->H->size1-robust_polytope_list[0]->H->size1, ((constraints->H->size2)-(robust_polytope_list[0]->H->size2)));

    gsl_matrix_memcpy(return_constraints->H, &L_u.matrix);
    gsl_vector_memcpy(return_constraints->G, &M_view.vector);
    //Clean up!
    polytope_free(scaled_W_set);
    polytope_free(constraints);
    for(size_t i = 0; i < N+1; i++){
        polytope_free(robust_polytope_list[i]);
    }
    free(robust_polytope_list);
    gsl_vector_free(L_x_dot_X0);

    return return_constraints;

};


//This does CELL (~soma) stage of peephole LSTM (long short-term memory) model.
//This requires each neuron to have 4 input time series, Xc, Xi, Xf, Xo,
//where Xc is the usual (or "cellular") input and Xi, Xf, Xo the inputs for the input, forget, output gates.
//Xc, Xi, Xf, Xo are the output of separate linear IN stages (weights and baises).
//In this version, these are stacked into one matrix
//For dim=0: X = [Xc; Xi; Xf; Xo]; for dim=1: X = [Xc Xi Xf Xo].

//For dim=0, I[:,t] = sig{Xi[:,t] + Ui*C[:,t-1]}
//           F[:,t] = sig{Xf[:,t] + Uf*C[:,t-1]}
//           O[:,t] = sig{Xo[:,t] + Uo*C[:,t-1]}
//           C[:,t] = F[:,t].*C[:,t-1] + I[:,t].*sig{Xc[t,:]}
//           Y[:,t] = tanh{O[:,t].*C[:,t]}
//with sizes Xc, Xi, Xf, Xo: N x T
//               Ui, Uf, Uo: N x N
//                        Y: N x T
//
//For dim=1, I[t,:] = sig{Xi[t,:] + C[t-1,:]*Ui}
//           F[t,:] = sig{Xf[t,:] + C[t-1,:]*Uf}
//           O[t,:] = sig{Xo[t,:] + C[t-1,:]*Uo}
//           C[t,:] = F[t,:].*C[t-1,:] + I[t,:].*sig{Xc[t,:]}
//           Y[t,:] = tanh{O[t,:].*C[t,:]}
//with sizes Xc, Xi, Xf, Xo: T x N
//               Ui, Uf, Uo: N x N
//                        Y: T x N
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//I is the input gate, F is the forget gate, O is the output gate,
//C is the "cell input activation vector",
//H is an intermediate (hidden) vector (sometimes called the "cell state vector"),
//Uc, Ui, Uf, Uo are NxN matrices, and Y is the final output (sometimes called the "hidden state vector").

//The tanh is omitted here!! (To allow trying other nonlinearities.)

//Note that, the neurons of a layer are independent only if Uc, Ui, Uf, Uo are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifdef I
    #undef I
#endif

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int lstm_peephole_s (float *Y, const float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm_peephole_d (double *Y, const double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm_peephole_inplace_s (float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int lstm_peephole_inplace_d (double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int lstm_peephole_s (float *Y, const float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    float *C, *I, *F, *O;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                Y[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                Y[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                Y[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                Y[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_d (double *Y, const double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    double *C, *I, *F, *O;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                Y[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                Y[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                Y[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    Y[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                Y[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    Y[tN+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_inplace_s (float *X, const float *Ui, const float *Uf, const float *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    float *C, *I, *F, *O;
    if (!(C=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in lstm_peephole_inplace_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                X[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                X[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0f / (1.0f+expf(-X[NT+nT]));
                C[n] = I[n] / (1.0f+expf(-X[nT]));
                X[nT] = C[n] / (1.0f+expf(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&X[NT+t],(int)T,I,1); cblas_scopy((int)N,&X[NT2+t],(int)T,F,1); cblas_scopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0f / (1.0f+expf(-X[N+n]));
                C[n] = I[n] / (1.0f+expf(-X[n]));
                X[n] = C[n] / (1.0f+expf(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_scopy((int)N,&X[tN4+N],1,I,1); cblas_scopy((int)N,&X[tN4+N2],1,F,1); cblas_scopy((int)N,&X[tN4+N3],1,O,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0f / (1.0f+expf(-I[n]));
                    C[n] = C[n]/(1.0f+expf(-F[n])) + I[n]/(1.0f+expf(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0f+expf(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_inplace_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int lstm_peephole_inplace_d (double *X, const double *Ui, const double *Uf, const double *Uo, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    const size_t N2 = 2*N, N3 = 3*N, NT = N*T, NT2 = 2*N*T, NT3 = 3*N*T;
    size_t nT, tN, tN4;

    double *C, *I, *F, *O;
    if (!(C=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(I=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(O=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in lstm_peephole_inplace_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                X[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                X[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                I[n] = 1.0 / (1.0+exp(-X[NT+nT]));
                C[n] = I[n] / (1.0+exp(-X[nT]));
                X[nT] = C[n] / (1.0+exp(-X[NT3+nT]));
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&X[NT+t],(int)T,I,1); cblas_dcopy((int)N,&X[NT2+t],(int)T,F,1); cblas_dcopy((int)N,&X[NT3+t],(int)T,O,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[t+n*T]));
                    X[t+n*T] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                I[n] = 1.0 / (1.0+exp(-X[N+n]));
                C[n] = I[n] / (1.0+exp(-X[n]));
                X[n] = C[n] / (1.0+exp(-X[N3+n]));
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N; tN4 = 4*tN;
                cblas_dcopy((int)N,&X[tN4+N],1,I,1); cblas_dcopy((int)N,&X[tN4+N2],1,F,1); cblas_dcopy((int)N,&X[tN4+N3],1,O,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Ui,(int)N,C,1,o,I,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,C,1,o,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uo,(int)N,C,1,o,O,1);
                for (size_t n=0u; n<N; ++n)
                {
                    I[n] = 1.0 / (1.0+exp(-I[n]));
                    C[n] = C[n]/(1.0+exp(-F[n])) + I[n]/(1.0+exp(-X[tN4+n]));
                    X[tN4+n] = C[n] / (1.0+exp(-O[n]));
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in lstm_peephole_inplace_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* linalg/hh.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#define REAL double

/* [Engeln-Mullges + Uhlig, Alg. 4.42]
 */

int
gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x)
{
  if (A->size1 > A->size2)
    {
      /* System is underdetermined. */

      GSL_ERROR ("System is underdetermined", GSL_EINVAL);
    }
  else if (A->size2 != x->size)
    {
      GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
    }
  else
    {
      int status ;

      gsl_vector_memcpy (x, b);

      status = gsl_linalg_HH_svx (A, x);

      return status ;
    }
}

int
gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x)
{
  if (A->size1 > A->size2)
    {
      /* System is underdetermined. */

      GSL_ERROR ("System is underdetermined", GSL_EINVAL);
    }
  else if (A->size2 != x->size)
    {
      GSL_ERROR ("matrix and vector sizes must be equal", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      const size_t M = A->size2;
      size_t i, j, k;
      REAL *d = (REAL *) malloc (N * sizeof (REAL));

      if (d == 0)
        {
          GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);
        }

      /* Perform Householder transformation. */

      for (i = 0; i < N; i++)
        {
          const REAL aii = gsl_matrix_get (A, i, i);
          REAL alpha;
          REAL f;
          REAL ak;
          REAL max_norm = 0.0;
          REAL r = 0.0;

          for (k = i; k < M; k++)
            {
              REAL aki = gsl_matrix_get (A, k, i);
              r += aki * aki;
            }

          if (r == 0.0)
            {
              /* Rank of matrix is less than size1. */
              free (d);
              GSL_ERROR ("matrix is rank deficient", GSL_ESING);
            }

          alpha = sqrt (r) * GSL_SIGN (aii);

          ak = 1.0 / (r + alpha * aii);
          gsl_matrix_set (A, i, i, aii + alpha);

          d[i] = -alpha;

          for (k = i + 1; k < N; k++)
            {
              REAL norm = 0.0;
              f = 0.0;
              for (j = i; j < M; j++)
                {
                  REAL ajk = gsl_matrix_get (A, j, k);
                  REAL aji = gsl_matrix_get (A, j, i);
                  norm += ajk * ajk;
                  f += ajk * aji;
                }
              max_norm = GSL_MAX (max_norm, norm);

              f *= ak;

              for (j = i; j < M; j++)
                {
                  REAL ajk = gsl_matrix_get (A, j, k);
                  REAL aji = gsl_matrix_get (A, j, i);
                  gsl_matrix_set (A, j, k, ajk - f * aji);
                }
            }

          if (fabs (alpha) < 2.0 * GSL_DBL_EPSILON * sqrt (max_norm))
            {
              /* Apparent singularity. */
              free (d);
              GSL_ERROR("apparent singularity detected", GSL_ESING);
            }

          /* Perform update of RHS. */

          f = 0.0;
          for (j = i; j < M; j++)
            {
              f += gsl_vector_get (x, j) * gsl_matrix_get (A, j, i);
            }
          f *= ak;
          for (j = i; j < M; j++)
            {
              REAL xj = gsl_vector_get (x, j);
              REAL aji = gsl_matrix_get (A, j, i);
              gsl_vector_set (x, j, xj - f * aji);
            }
        }

      /* Perform back-substitution. */

      for (i = N; i > 0 && i--;)
        {
          REAL xi = gsl_vector_get (x, i);
          REAL sum = 0.0;
          for (k = i + 1; k < N; k++)
            {
              sum += gsl_matrix_get (A, i, k) * gsl_vector_get (x, k);
            }

          gsl_vector_set (x, i, (xi - sum) / d[i]);
        }

      free (d);
      return GSL_SUCCESS;
    }
}


#include <math.h>
#include <stdlib.h>

#include "fd3sep.h"
/* #include "mxfuns.h" */

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>

#define SVCUT 1.0e-9

/*************************************************************************/

double fd3sep ( long K, long M, long N, double **dftobs, double *sig,
   double **rvm, double **lfm, double **dftmod, double **dftres ) {

   long i, j, k, n;
   double s2;
   gsl_matrix *A, *U, *X, *V;
   gsl_vector *S, *w, *b, *x;

   /* prepare */

   A = gsl_matrix_alloc ( 2*M, 2*K );
   U = gsl_matrix_alloc ( 2*M, 2*K );
   X = gsl_matrix_alloc ( 2*K, 2*K );
   V = gsl_matrix_alloc ( 2*K, 2*K );
   S = gsl_vector_alloc ( 2*K );
   w = gsl_vector_alloc ( 2*K );
   b = gsl_vector_alloc ( 2*M );
   x = gsl_vector_alloc ( 2*K );

   /* for each DFT component */

   for ( s2 = n = 0 ; n <= N/2 ; n++ ) {

      /* assemble data vector and model matrix */

      for ( j = 0 ; j < M ; j++ ) {
         double s = *(sig+j);
         gsl_vector_set ( b, 2*j,   *(*(dftobs+j)+2*n)  /s );
         gsl_vector_set ( b, 2*j+1, *(*(dftobs+j)+2*n+1)/s );
         for ( k = 0 ; k < K ; k++ ) {
            double q, v, fv, rez, imz;
            q = 2.0 * M_PI * ((double)n)/((double)N);
            v = *(*(rvm+k)+j);
            fv = floor(v);
            rez = *(*(lfm+k)+j) *
                ( ((fv+1)-v)*cos(fv*q) + (v-fv)*cos((fv+1)*q) );
            imz = - *(*(lfm+k)+j) *
                ( ((fv+1)-v)*sin(fv*q) + (v-fv)*sin((fv+1)*q) );
            gsl_matrix_set ( A, 2*j,   2*k,     rez/s );
            gsl_matrix_set ( A, 2*j,   2*k+1, - imz/s );
            gsl_matrix_set ( A, 2*j+1, 2*k,     imz/s );
            gsl_matrix_set ( A, 2*j+1, 2*k+1,   rez/s );
         }
         gsl_matrix_memcpy ( U, A );
      }

      /* solve for model parameters */

      /* gsl_linalg_SV_decomp ( U, V, S, w ); */
      /* gsl_linalg_SV_decomp_mod ( U, X, V, S, w ); */
      /* gsl_linalg_SV_decomp_jacobi ( U, V, S ); */

      gsl_linalg_SV_decomp_mod ( U, X, V, S, w );
      for ( k = 0 ; k < 2*K-1 ; k++ )
         if ( gsl_vector_get(S,2*K-1-k)/gsl_vector_get(S,0) < SVCUT )
            gsl_vector_set ( S, 2*K-1-k, 0 );
      gsl_linalg_SV_solve ( U, V, S, b, x );

      /* copy to output */

      for ( k = 0 ; k < K ; k++ ) {
         *(*(dftmod+k)+2*n  ) = gsl_vector_get ( x, 2*k   );
         *(*(dftmod+k)+2*n+1) = gsl_vector_get ( x, 2*k+1 );
      }

      /* compute s2 */

      for ( j = 0 ; j < M ; j++ ) for ( i = 0 ; i < 2 ; i++ ) {

        double bc, db, s = *(sig+j);

        for ( bc = k = 0 ; k < K ; k++ ) {
          bc += gsl_matrix_get( A, 2*j+i, 2*k   ) * gsl_vector_get( x, 2*k   );
          bc += gsl_matrix_get( A, 2*j+i, 2*k+1 ) * gsl_vector_get( x, 2*k+1 );
        }
        db = gsl_vector_get ( b, 2*j+i ) - bc;
        s2 += db * db * ( n % ((N+1)/2) ? 2 : 1 );
        *(*(dftres+j)+2*n+i) = db * s;

      }

   }

   /* close */

   gsl_matrix_free ( A );
   gsl_matrix_free ( U );
   gsl_matrix_free ( X );
   gsl_matrix_free ( V );
   gsl_vector_free ( S );
   gsl_vector_free ( w );
   gsl_vector_free ( b );
   gsl_vector_free ( x );

   return s2;

}

/*************************************************************************/

void dft_fwd ( long m, long n, double **mxin, double **mxout ) {

   long i, j;
   double a = 1.0 / sqrt(n);
   gsl_fft_real_wavetable * dft_rewt;
   gsl_fft_real_workspace * dft_rews;

   dft_rewt = gsl_fft_real_wavetable_alloc (n);
   dft_rews = gsl_fft_real_workspace_alloc (n);

   for ( j = 0 ; j < m ; j++ ) {
      for ( i = 0; i < n; i++ ) *(*(mxout+j)+i+1) = a * *(*(mxin+j)+i);
      gsl_fft_real_transform ( *(mxout+j)+1, 1, n, dft_rewt, dft_rews );
      *(*(mxout+j)+0) = *(*(mxout+j)+1);
      *(*(mxout+j)+1) = 0;
      if ( ! n % 2 ) *(*(mxout+j)+n+1) = 0; /* if n even */
   }

   gsl_fft_real_wavetable_free ( dft_rewt );
   gsl_fft_real_workspace_free ( dft_rews );

}

/*************************************************************************/

void dft_bck ( long m, long n, double **mxin, double **mxout ) {

   long i, j;
   double a = 1.0 / sqrt(n);
   gsl_fft_halfcomplex_wavetable * dft_hcwt;
   gsl_fft_real_workspace * dft_rews;

   dft_hcwt = gsl_fft_halfcomplex_wavetable_alloc (n);
   dft_rews = gsl_fft_real_workspace_alloc (n);

   for ( j = 0 ; j < m ; j++ ) {
      *(*(mxout+j)+0) = *(*(mxin+j)+0) / a;
      for ( i = 1; i < n; i++ ) *(*(mxout+j)+i) = *(*(mxin+j)+i+1) / a;
      gsl_fft_halfcomplex_inverse ( *(mxout+j), 1, n, dft_hcwt, dft_rews );
   }

   gsl_fft_halfcomplex_wavetable_free ( dft_hcwt );
   gsl_fft_real_workspace_free ( dft_rews );

}

/*************************************************************************/


/* specfunc/bessel_Jn.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include "bessel.h"
#include "bessel_amp_phase.h"
#include "bessel_olver.h"
#include <gsl/gsl_sf_bessel.h>

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_sf_bessel_Jn_e(int n, double x, gsl_sf_result * result)
{
  int sign = 1;

  if(n < 0) {
    /* reduce to case n >= 0 */
    n = -n;
    if(GSL_IS_ODD(n)) sign = -sign;
  }

  if(x < 0.0) {
    /* reduce to case x >= 0. */
    x = -x;
    if(GSL_IS_ODD(n)) sign = -sign;
  }

  /* CHECK_POINTER(result) */

  if(n == 0) {
    gsl_sf_result b0;
    int stat_J0 = gsl_sf_bessel_J0_e(x, &b0);
    result->val = sign * b0.val;
    result->err = b0.err;
    return stat_J0;
  }
  else if(n == 1) {
    gsl_sf_result b1;
    int stat_J1 = gsl_sf_bessel_J1_e(x, &b1);
    result->val = sign * b1.val;
    result->err = b1.err;
    return stat_J1;
  }
  else {
    if(x == 0.0) {
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else if(x*x < 10.0*(n+1.0)*GSL_ROOT5_DBL_EPSILON) {
      gsl_sf_result b;
      int status = gsl_sf_bessel_IJ_taylor_e((double)n, x, -1, 50, GSL_DBL_EPSILON, &b);
      result->val  = sign * b.val;
      result->err  = b.err;
      result->err += GSL_DBL_EPSILON * fabs(result->val);
      return status;
    }
    else if(GSL_ROOT4_DBL_EPSILON * x > (n*n+1.0)) {
      int status = gsl_sf_bessel_Jnu_asympx_e((double)n, x, result);
      result->val *= sign;
      return status;
    }
    else if(n > 50) {
      int status = gsl_sf_bessel_Jnu_asymp_Olver_e((double)n, x, result);
      result->val *= sign;
      return status;
    }
    else if(x > 1000.0)
    {
      /* We need this to avoid feeding large x to CF1; note that
       * due to the above check, we know that n <= 50.
       */
      int status = gsl_sf_bessel_Jnu_asympx_e((double)n, x, result);
      result->val *= sign;
      return status;
    }
    else {
      double ans;
      double err;
      double ratio;
      double sgn;
      int stat_b;
      int stat_CF1 = gsl_sf_bessel_J_CF1((double)n, x, &ratio, &sgn);

      /* backward recurrence */
      double Jkp1 = GSL_SQRT_DBL_MIN * ratio;
      double Jk   = GSL_SQRT_DBL_MIN;
      double Jkm1;
      int k;

      for(k=n; k>0; k--) {
        Jkm1 = 2.0*k/x * Jk - Jkp1;
        Jkp1 = Jk;
        Jk   = Jkm1;
      }

      if(fabs(Jkp1) > fabs(Jk)) {
        gsl_sf_result b1;
        stat_b = gsl_sf_bessel_J1_e(x, &b1);
        ans = b1.val/Jkp1 * GSL_SQRT_DBL_MIN;
        err = b1.err/Jkp1 * GSL_SQRT_DBL_MIN;
      }
      else {
        gsl_sf_result b0;
        stat_b = gsl_sf_bessel_J0_e(x, &b0);
        ans = b0.val/Jk * GSL_SQRT_DBL_MIN;
        err = b0.err/Jk * GSL_SQRT_DBL_MIN;
      }

      result->val = sign * ans;
      result->err = fabs(err);
      return GSL_ERROR_SELECT_2(stat_CF1, stat_b);
    }
  }
}

int
gsl_sf_bessel_Jn_array(int nmin, int nmax, double x, double * result_array)
{
  /* CHECK_POINTER(result_array) */

  if(nmin < 0 || nmax < nmin) {
    int n;
    for(n=nmax; n>=nmin; n--) {
      result_array[n-nmin] = 0.0;
    }
    GSL_ERROR ("domain error", GSL_EDOM);
  }
  else if(x == 0.0) {
    int n;
    for(n=nmax; n>=nmin; n--) {
      result_array[n-nmin] = 0.0;
    }
    if(nmin == 0) result_array[0] = 1.0;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result r_Jnp1;
    gsl_sf_result r_Jn;
    int stat_np1 = gsl_sf_bessel_Jn_e(nmax+1, x, &r_Jnp1);
    int stat_n   = gsl_sf_bessel_Jn_e(nmax,   x, &r_Jn);
    int stat = GSL_ERROR_SELECT_2(stat_np1, stat_n);

    double Jnp1 = r_Jnp1.val;
    double Jn   = r_Jn.val;
    double Jnm1;
    int n;

    if(stat == GSL_SUCCESS) {
      for(n=nmax; n>=nmin; n--) {
        result_array[n-nmin] = Jn;
        Jnm1 = -Jnp1 + 2.0*n/x * Jn;
        Jnp1 = Jn;
        Jn   = Jnm1;
      }
    }
    else {
      for(n=nmax; n>=nmin; n--) {
        result_array[n-nmin] = 0.0;
      }
    }

    return stat;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Jn(const int n, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Jn_e(n, x, &result));
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

int i;
int max_lines=30;
double xi, yi, x[30], y[30];

const char* getfield(char* line, int num)
{
    const char* tok;
    for (tok = strtok(line, ",");
            tok && *tok;
            tok = strtok(NULL, ";\n"))
    {
        if (!--num)
            return tok;
    }
    return NULL;
}

void read_csv(char* filename, int max_lines){
  FILE * f = fopen(filename, "r");
  char line[1024];
  int l = 0;
  fgets(line, 1024, f);
  while(fgets(line, 1024, f)){
    char* tmp = strdup(line);
    double y_tmp = atof(getfield(tmp, 2));
    double x_tmp = atof(getfield(tmp, 1));
    if(l < max_lines){
      x[l] = x_tmp;
      y[l] = y_tmp;
    }
    l++;

    free(tmp);
  }
}

int
main (void)
{
  read_csv("secret_fun.csv", max_lines);

  // printf ("#m=0,S=17\n");
  //
  // for (i = 0; i < max_lines; i++)
  //   {
  //     // x[i] = i + 0.5 * sin (i);
  //     // y[i] = i + cos (i * i);
  //     printf ("%f %f \n", x[i], y[i]);
  //   }

  // printf ("#m=1,S=0\n");
  //
  {
    gsl_interp_accel *acc
      = gsl_interp_accel_alloc ();
    gsl_spline *spline
      = gsl_spline_alloc (gsl_interp_cspline, 30);

    gsl_spline_init (spline, x, y, 30);

    printf("x,y\n" );
    for (xi = x[0]; xi < x[29]; xi += 0.01)
      {
        printf("%g, ", xi);
        yi = gsl_spline_eval (spline, xi, acc);
        printf ("%g\n", yi);
      }
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);
  }
  return 0;
}


/* specfunc/bessel_Ynu.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_bessel.h"

#include "error.h"

#include "bessel.h"
#include "bessel_olver.h"
#include "bessel_temme.h"

/* Perform forward recurrence for Y_nu(x) and Y'_nu(x)
 *
 *        Y_{nu+1} =  nu/x Y_nu - Y'_nu
 *       Y'_{nu+1} = -(nu+1)/x Y_{nu+1} + Y_nu
 */
#if 0
static
int
bessel_Y_recur(const double nu_min, const double x, const int kmax,
               const double Y_start, const double Yp_start,
	       double * Y_end, double * Yp_end)
{
  double x_inv = 1.0/x;
  double nu = nu_min;
  double Y_nu  = Y_start;
  double Yp_nu = Yp_start;
  int k;

  for(k=1; k<=kmax; k++) {
    double nuox = nu*x_inv;
    double Y_nu_save = Y_nu;
    Y_nu  = -Yp_nu + nuox * Y_nu;
    Yp_nu = Y_nu_save - (nuox+x_inv) * Y_nu;
    nu += 1.0;
  }
  *Y_end  = Y_nu;
  *Yp_end = Yp_nu;
  return GSL_SUCCESS;
}
#endif

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_bessel_Ynu_e(double nu, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0 || nu < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(nu > 50.0) {
    return gsl_sf_bessel_Ynu_asymp_Olver_e(nu, x, result);
  }
  else {
    /* -1/2 <= mu <= 1/2 */
    int N = (int)(nu + 0.5);
    double mu = nu - N;

    gsl_sf_result Y_mu, Y_mup1;
    int stat_mu;
    double Ynm1;
    double Yn;
    double Ynp1;
    int n;

    if(x < 2.0) {
      /* Determine Ymu, Ymup1 directly. This is really
       * an optimization since this case could as well
       * be handled by a call to gsl_sf_bessel_JY_mu_restricted(),
       * as below.
       */
      stat_mu = gsl_sf_bessel_Y_temme(mu, x, &Y_mu, &Y_mup1);
    }
    else {
      /* Determine Ymu, Ymup1 and Jmu, Jmup1.
       */
      gsl_sf_result J_mu, J_mup1;
      stat_mu = gsl_sf_bessel_JY_mu_restricted(mu, x, &J_mu, &J_mup1, &Y_mu, &Y_mup1);
    }

    /* Forward recursion to get Ynu, Ynup1.
     */
    Ynm1 = Y_mu.val;
    Yn   = Y_mup1.val;
    for(n=1; n<=N; n++) {
      Ynp1 = 2.0*(mu+n)/x * Yn - Ynm1;
      Ynm1 = Yn;
      Yn   = Ynp1;
    }

    result->val  = Ynm1; /* Y_nu */
    result->err  = (N + 1.0) * fabs(Ynm1) * (fabs(Y_mu.err/Y_mu.val) + fabs(Y_mup1.err/Y_mup1.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(Ynm1);

    return stat_mu;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Ynu(const double nu, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Ynu_e(nu, x, &result));
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define COMPEARTH_PRIVATE_DET3X3 1
#include "compearth.h"
/*
#ifdef COMPEARTH_USE_MKL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
*/

//static double det3x3ColumnMajor(const double *__restrict__ A);

/*!
 * @brief Ensure that U is a rotation matrix with det(U) = 1.
 *
 * @param[in] n      Number of rotation matrices.
 * @param[in] Uin    Rotation matrices to check.  This is an array of dimension
 *                   [3 x 3 x n] where each [3 x 3] matrix is in column major
 *                   format.
 * @param[in] Uout   Rotation matrices with determinants all positive.  This
 *                   is an array of dimension [3 x 3 x n] where each [3 x 3]
 *                   matrix is in column major format.
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @copyright MIT
 *
 */
int compearth_Udetcheck(const int n,
                        const double *__restrict__ Uin,
                        double *__restrict__ Uout)
{
    double det;
    int i, imt;
    // Copy Uin to Uout
    memcpy(Uout, Uin, 9*(size_t) n*sizeof(double));
    //cblas_dcopy(9*n, Uin, 1, Uout, 1);
    // Fix
    for (imt=0; imt<n; imt++)
    {
        det = det3x3ColumnMajor(&Uout[9*imt]);
        // Negate the second column
        if (det < 0.0)
        {
            for (i=0; i<3; i++)
            {
                Uout[9*imt+3+i] =-Uout[9*imt+3+i];
            }
        }
    }
    // Verify
    for (imt=0; imt<n; imt++)
    {
        det = det3x3ColumnMajor(&Uout[9*imt]);
        if (det < 0.0)
        {
            fprintf(stderr, "%s: Error det(u) < 0\n", __func__);
            return -1;
        }
    }
    return 0;
}

/*
static double det3x3ColumnMajor(const double *__restrict__ A)
{
    double det;
    det = A[0]*( A[4]*A[8] - A[5]*A[7])
        - A[3]*( A[1]*A[8] - A[2]*A[7])
        + A[6]*( A[1]*A[5] - A[2]*A[4]);
    return det;
}
*/


#include <vector>
#include <fstream>
#include <cmath>
#include <cblas.h>
#include <fftw3.h>
#include <sys/time.h>
#include <time.h>

using namespace std;

class CochlearModel_1D{

private:
    int N;
    double Lb;
    double L;
    double W;
    double H;
    double rho;
    double dx;
    vector<double> x;

    vector<double> ch_damp;

    vector<double> k1;
    double m1;
    vector<double> c1;
    vector<double> k2;
    double m2;
    vector<double> c2;
    vector<double> k3;
    vector<double> c3;
    vector<double> k4;
    vector<double> c4;

    vector<double> k1k3;
    vector<double> c1c3;
    vector<double> k2k3;
    vector<double> c2c3;

    double g;
    double b;
    vector<double> gamma;

    double dt;
    double beta;

    vector<double> Gohc(vector<double> uc);
    vector<double> dGohc(vector<double> uc, vector<double> vc);
    void get_g(vector<double> vb, vector<double> ub, vector<double> vt, vector<double> ut, vector<double>& gb, vector<double>& gt);

public:
    CochlearModel_1D(int num_segment, vector<double> gamma_given)
    {
        N = num_segment;
        Lb = 3.5;
        L = 0.1;
        W = 0.1;
        H = 0.1;
        rho = 1.0;
        dx = Lb/N;

        m1 = 3e-3;
        m2 = 0.5e-3;
        for (int ii = 0; ii < N; ii++){
            x.push_back(dx * ii);
//            ch_damp.push_back(2.8 * exp(-0.1*x[ii]));
            ch_damp.push_back(1.0);

            k1.push_back(2.2e8 * exp(-3 * x[ii]));
            c1.push_back(6 + 670 * exp(-1.5 * x[ii]) * ch_damp[ii]);
            k2.push_back(1.4e6 * exp(-3.3 * x[ii]));
            c2.push_back(4.4 * exp(-1.65 * x[ii]) * ch_damp[ii]);
            k3.push_back(2.0e6 * exp(-3 * x[ii]));
            c3.push_back(0.8 * exp(-0.6 * x[ii]) * ch_damp[ii]);
            k4.push_back(1.15e8 * exp(-3 * x[ii]));
            c4.push_back(440.0 * exp(-1.5 * x[ii]) * ch_damp[ii]);

            k1k3.push_back(k1[ii] + k3[ii]);
            c1c3.push_back(c1[ii] + c3[ii]);
            k2k3.push_back(k2[ii] + k3[ii]);
            c2c3.push_back(c2[ii] + c3[ii]);
            gamma.push_back(gamma_given[ii]);
        }
        g = 1;
        b = 0.4;
        dt = 10e-6;
        beta = 50e-7;
    }

   void solve_time_domain(vector<double> f, vector<vector<double> >& mat_vb, vector<vector<double> >& mat_ub, vector<vector<double> >& mat_p);
};


/*
	Simbicon 1.5 Controller Editor Framework,
	Copyright 2009 Stelian Coros, Philippe Beaudoin and Michiel van de Panne.
	All rights reserved. Web: www.cs.ubc.ca/~van/simbicon_cef

	This file is part of the Simbicon 1.5 Controller Editor Framework.

	Simbicon 1.5 Controller Editor Framework is free software: you can
	redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	Simbicon 1.5 Controller Editor Framework is distributed in the hope
	that it will be useful, but WITHOUT ANY WARRANTY; without even the
	implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	See the GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with Simbicon 1.5 Controller Editor Framework.
	If not, see <http://www.gnu.org/licenses/>.
*/

#pragma once

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <MathLib/MathLib.h>
#include <MathLib/ThreeTuple.h>

#include <gsl/matrix/gsl_matrix.h>
#include <gsl/vector/gsl_vector.h>

#include <MathLib/Matrix.h>

#define VECTOR_AT(v, i) (*((v->data + (i * v->tda ))))

/*====================================================================================================================================================================*
 | This class will be used to represent matrices of arbitrary sizes (m rows by n columns) that have elements of type double. The underlying data strucutre used by    |
 | this class is gsl's (Gnu Scientific Library) matrix class. This class also makes use of the ATLAS implementation of BLAS for some operations such as matrix-matrix |
 | multiplication. This class is meant to improve performance, not necessarily ease of use.                                                                           |
 *====================================================================================================================================================================*/

class Vector : public Matrix{
public:
	/**
		constructor - creates an n row vector that is not initialized to any particular values
	*/
	Vector(int n);

	/**
		default constructor
	*/
	Vector();

	/**
		copy constructor - performs a deep copy of the matrix passed in as a parameter.
	*/
	Vector(const Vector& other);

	/**
		destructor.
	*/
    virtual ~Vector();

	/**
		copy operator - performs a deep copy of the Vector passed in as a parameter.
	*/
	Vector& operator=(const Vector &other);

	/**
		copy operator - performs a deep copy of the Vector passed in as a parameter.
	*/
	Vector& operator=(const Matrix &other);

	/**
		this method performs a shallow copy of the Vector that is passed in as a parameter.
	*/
	void shallowCopy(const Matrix& other);

	/**
		this method performs a deep copy of the vector that is passed in as a paramerer.
	*/
	void deepCopy(const Matrix& other);

	/**
		This method sets the current vector to be equal to one of the products: A * b or A'*b.
		The value of transA indicates if A is transposed or not
	*/
	void setToProductOf(const Matrix& A, const Matrix& B, bool transA = false, bool transB = false);

	/**
		This method sets the current vector to be equal to one of the rows of A - shallow column only!
	*/
	void setToRow(const Matrix& A, int row, int start = 0, int howMany = -1);

	/**
		This method sets the current vector to be equal to one of the cols of A - shallow column only!
	*/
	void setToCol(const Matrix& A, int col, int start = 0, int howMany = -1);

	/**
		This method prints the contents of the matrix - testing purpose only.
	*/
	void printVector() const;

	/**
		This method returns a copy of the value of the matrix at (i,j)
	*/
	double get(int i) const;

	/**
		This method sets the value of the matrix at (i,j) to newVal.
	*/
	void set(int i, double newVal);

	/**
		Computes the 2-norm squared for the current vector.
	*/
	inline double normSquared(){
		int r = getRowCount();
		double result = 0;
		for (int i=0;i<r;i++){
			double n = MATRIX_AT(matrix, i, 0) ;
			result += n*n;
		}
		return result;
	}

};

void testVectorClass();


//
//  ger.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 12/23/11.
//  Copyright (c) 2011 University of Colorado Denver. All rights reserved.
//

#ifndef _ger_h
#define _ger_h

/// @file ger.h Performs vector outer product.

#include "latl.h"

namespace LATL
{
   /// @brief Performs a vector outer product of two real vectors.
   ///
   /// For a real matrix A, real vectors x and y, and real scalar alpha,
   ///
   ///        A := alpha * x * y' + A
   ///
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param m Specifies the number of rows of the matrix A.  m>=0
   /// @param n Specifies the number of coumns of the matrix A.  n>=0
   /// @param alpha Real scalar.
   /// @param x Pointer to real vector x.
   /// @param incx Increment of the vector x.  x!=0
   /// @param y Pointer to real vector y.
   /// @param incy Increment of the vector y.  y!=0
   /// @param A Pointer to real m-by-n matrix A.
   /// @param ldA Column length of matrix A.  ldA>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int GER(int_t m, int_t n, real_t alpha, real_t *x, int_t incx, real_t *y, int_t incy, real_t *A, int_t ldA)
   {
      const real_t zero(0.0);
      int_t i,j,kx,jy,ix;
      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;
      else if((m==0)||(n==0)||(alpha==zero))
         return 0;

      jy=(incy>0)?0:(1-n)*incy;
      kx=(incx>0)?0:(1-m)*incx;

      if(incx==1)
      {
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               A[i]+=x[i]*alpha*y[jy];
            jy+=incy;
            A+=ldA;
         }
      }
      else
      {
         for(j=0;j<n;j++)
         {
            ix=kx;
            for(i=0;i<m;i++)
            {
               A[i]+=x[ix]*alpha*y[jy];
               ix+=incx;
            }
            A+=ldA;
            jy+=incy;
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int GER<float>(int_t m, int_t n, float alpha, float *x, int_t incx, float *y, int_t incy, float *A, int_t ldA)
   {
      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;

      cblas_sger(CblasColMajor,m,n,alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

   template <> int GER<double>(int_t m, int_t n, double alpha, double *x, int_t incx, double *y, int_t incy, double *A, int_t ldA)
   {
      if(m<0)
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<m)
         return -9;

      cblas_dger(CblasColMajor,m,n,alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

#endif
}
#endif


/* linalg/test_luc.c
 *
 * Copyright (C) 2019 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

static int
test_LUc_decomp_eps(const gsl_matrix_complex * m, const double eps, const char * desc)
{
  int s = 0;
  const size_t M = m->size1;
  const size_t N = m->size2;

  gsl_matrix_complex * A = gsl_matrix_complex_alloc(M, N);
  gsl_matrix_complex * PLU = gsl_matrix_complex_calloc(M, N);
  gsl_permutation * p = gsl_permutation_alloc(M);
  size_t i, j;
  int signum;

  gsl_matrix_complex_memcpy(A, m);
  gsl_linalg_complex_LU_decomp(A, p, &signum);

  if (M >= N)
    {
      gsl_matrix_complex_view U = gsl_matrix_complex_submatrix(A, 0, 0, N, N);

      /* copy L factor into PLU */
      for (j = 0; j < GSL_MIN(M, N); ++j)
        {
          gsl_vector_complex_view v1 = gsl_matrix_complex_subcolumn(A, j, j, M - j);
          gsl_vector_complex_view v2 = gsl_matrix_complex_subcolumn(PLU, j, j, M - j);

          gsl_vector_complex_memcpy(&v2.vector, &v1.vector);
          gsl_matrix_complex_set(PLU, j, j, GSL_COMPLEX_ONE);
        }

      /* PLU := L * U */
      gsl_blas_ztrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, GSL_COMPLEX_ONE, &U.matrix, PLU);
    }
  else
    {
      gsl_matrix_complex_view L = gsl_matrix_complex_submatrix(A, 0, 0, M, M);

      /* copy U factor into PLU */
      for (i = 0; i < GSL_MIN(M, N); ++i)
        {
          gsl_vector_complex_view v1 = gsl_matrix_complex_subrow(A, i, i, N - i);
          gsl_vector_complex_view v2 = gsl_matrix_complex_subrow(PLU, i, i, N - i);
          gsl_vector_complex_memcpy(&v2.vector, &v1.vector);
        }

      /* PLU := L * U */
      gsl_blas_ztrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, GSL_COMPLEX_ONE, &L.matrix, PLU);
    }

  /* PLU := P * L * U */
  for (j = 0; j < N; ++j)
    {
      gsl_vector_complex_view v = gsl_matrix_complex_column(PLU, j);
      gsl_permute_vector_complex_inverse(p, &v.vector);
    }

  /* now test m = PLU */
  for (i = 0; i < M; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          gsl_complex aij = gsl_matrix_complex_get(PLU, i, j);
          gsl_complex mij = gsl_matrix_complex_get(m, i, j);

          gsl_test_rel(GSL_REAL(aij), GSL_REAL(mij), eps,
                       "%s real: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, j, GSL_REAL(aij), GSL_REAL(mij));

          gsl_test_rel(GSL_IMAG(aij), GSL_IMAG(mij), eps,
                       "%s imag: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, M, N, i, j, GSL_IMAG(aij), GSL_IMAG(mij));
        }
    }

  gsl_matrix_complex_free(A);
  gsl_matrix_complex_free(PLU);
  gsl_permutation_free(p);

  return s;
}

static int
test_LUc_decomp(gsl_rng * r)
{
  int s = 0;
  size_t n;

  /* test square matrices */
  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix_complex * m = gsl_matrix_complex_alloc(n, n);

      create_random_complex_matrix(m, r);
      test_LUc_decomp_eps(m, 1.0e4 * n * GSL_DBL_EPSILON, "complex_LU_decomp random");

      gsl_matrix_complex_free(m);
    }

  {
    gsl_matrix_complex * m = gsl_matrix_complex_alloc(100, 50);
    create_random_complex_matrix(m, r);
    test_LUc_decomp_eps(m, 256.0 * n * GSL_DBL_EPSILON, "complex_LU_decomp rect1");
    gsl_matrix_complex_free(m);
  }

  {
    gsl_matrix_complex * m = gsl_matrix_complex_alloc(50, 100);
    create_random_complex_matrix(m, r);
    test_LUc_decomp_eps(m, 1.0e3 * n * GSL_DBL_EPSILON, "complex_LU_decomp rect2");
    gsl_matrix_complex_free(m);
  }

  {
    gsl_matrix_complex * m = gsl_matrix_complex_alloc(80, 100);
    create_random_complex_matrix(m, r);
    test_LUc_decomp_eps(m, 1.0e3 * n * GSL_DBL_EPSILON, "complex_LU_decomp rect3");
    gsl_matrix_complex_free(m);
  }

  return s;
}

static int
test_LUc_solve_eps(const gsl_matrix_complex * m, const gsl_vector_complex * rhs, const gsl_vector_complex * sol, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = m->size1;
  int signum;
  size_t i;

  gsl_permutation * perm = gsl_permutation_alloc(N);
  gsl_matrix_complex * lu  = gsl_matrix_complex_alloc(N, N);
  gsl_vector_complex * x = gsl_vector_complex_alloc(N);
  gsl_vector_complex * residual = gsl_vector_complex_alloc(N);

  gsl_matrix_complex_memcpy(lu, m);

  s += gsl_linalg_complex_LU_decomp(lu, perm, &signum);
  s += gsl_linalg_complex_LU_solve(lu, perm, rhs, x);

  for (i = 0; i < N; i++)
    {
      gsl_complex xi = gsl_vector_complex_get(x, i);
      gsl_complex yi = gsl_vector_complex_get(sol, i);

      gsl_test_rel(GSL_REAL(xi), GSL_REAL(yi), eps, "%s: real %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, GSL_REAL(xi), GSL_REAL(yi));

      gsl_test_rel(GSL_IMAG(xi), GSL_IMAG(yi), eps, "%s: imag %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, GSL_IMAG(xi), GSL_IMAG(yi));
    }

  s += gsl_linalg_complex_LU_refine(m, lu, perm, rhs, x, residual);

  for (i = 0; i < N; i++)
    {
      gsl_complex xi = gsl_vector_complex_get(x, i);
      gsl_complex yi = gsl_vector_complex_get(sol, i);

      gsl_test_rel(GSL_REAL(xi), GSL_REAL(yi), eps, "%s: improved real %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, GSL_REAL(xi), GSL_REAL(yi));

      gsl_test_rel(GSL_IMAG(xi), GSL_IMAG(yi), eps, "%s: improved imag %3lu[%lu]: %22.18g    %22.18g\n",
                   desc, N, i, GSL_IMAG(xi), GSL_IMAG(yi));
    }

  gsl_vector_complex_free(residual);
  gsl_vector_complex_free(x);
  gsl_matrix_complex_free(lu);
  gsl_permutation_free(perm);

  return s;
}

static int
test_LUc_solve(gsl_rng * r)
{
  int s = 0;
  size_t n;

  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix_complex * m = gsl_matrix_complex_alloc(n, n);
      gsl_vector_complex * rhs = gsl_vector_complex_alloc(n);
      gsl_vector_complex * sol = gsl_vector_complex_alloc(n);

      create_random_complex_matrix(m, r);
      create_random_complex_vector(sol, r);
      gsl_blas_zgemv(CblasNoTrans, GSL_COMPLEX_ONE, m, sol, GSL_COMPLEX_ZERO, rhs);
      test_LUc_solve_eps(m, rhs, sol, 1.0e5 * n * GSL_DBL_EPSILON, "complex_LU_solve random");

      gsl_matrix_complex_free(m);
      gsl_vector_complex_free(rhs);
      gsl_vector_complex_free(sol);
    }

  return s;
}

int
test_LUc_invert_eps(const gsl_matrix_complex * m, const double eps, const char *desc)
{
  int s = 0;
  size_t i, j, N = m->size1;

  gsl_matrix_complex * lu  = gsl_matrix_complex_alloc(N, N);
  gsl_matrix_complex * c  = gsl_matrix_complex_alloc(N, N);
  gsl_permutation * perm = gsl_permutation_alloc(N);
  int signum;

  gsl_matrix_complex_memcpy(lu, m);

  s += gsl_linalg_complex_LU_decomp(lu, perm, &signum);
  s += gsl_linalg_complex_LU_invx(lu, perm);

  /* c = m m^{-1} */
  gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, GSL_COMPLEX_ONE, m, lu, GSL_COMPLEX_ZERO, c);

  /* c should be the identity matrix */

  for (i = 0; i < N; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          gsl_complex cij = gsl_matrix_complex_get(c, i, j);
          double expected = (i == j) ? 1.0 : 0.0;

          gsl_test_rel(GSL_REAL(cij), expected, eps, "%s real (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, GSL_REAL(cij), expected);

          gsl_test_rel(GSL_IMAG(cij), 0.0, eps, "%s imag (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, GSL_IMAG(cij), 0.0);
        }
    }

  gsl_matrix_complex_free(lu);
  gsl_matrix_complex_free(c);
  gsl_permutation_free(perm);

  return s;
}

static int
test_LUc_invert(gsl_rng * r)
{
  int s = 0;
  size_t n;

  for (n = 1; n <= 50; ++n)
    {
      gsl_matrix_complex * m = gsl_matrix_complex_alloc(n, n);

      create_random_complex_matrix(m, r);
      test_LUc_invert_eps(m, 1.0e5 * n * GSL_DBL_EPSILON, "complex_LU_invert random");

      gsl_matrix_complex_free(m);
    }

  return s;
}


#include <assert.h>
#include <gsl/gsl_nan.h>

#include "csm_all.h"

int minmax(int from, int to, int x) {
	return GSL_MAX(GSL_MIN(x,to),from);
}

void possible_interval(
	const double *p_i_w, LDP ld,
	double max_angular_correction_deg, double max_linear_correction, int*from, int*to, int*start_cell)
{
	double angle_res = (ld->max_theta-ld->min_theta)/ld->nrays;

	/* Delta for the angle */
	double delta = fabs(deg2rad(max_angular_correction_deg)) +
	        fabs(atan(max_linear_correction/norm_d(p_i_w)));

	/* Dimension of the cell range */
	int range = (int) ceil(delta/angle_res);

	/* To be turned into an interval of cells */
	double start_theta = atan2(p_i_w[1], p_i_w[0]);

	/* Make sure that start_theta is in the interval [min_theta,max_theta].
	   For example, -1 is not in [0, 2pi] */
	if(start_theta<ld->min_theta) start_theta += 2*M_PI;
	if(start_theta>ld->max_theta) start_theta -= 2*M_PI;

	*start_cell  = (int)
		((start_theta - ld->min_theta) / (ld->max_theta-ld->min_theta) * ld->nrays);

	*from = minmax(0,ld->nrays-1, *start_cell-range);
	*to =   minmax(0,ld->nrays-1, *start_cell+range);

	if(0)
	printf("from: %d to: %d delta: %f start_theta: %f min/max theta: [%f,%f] range: %d start_cell: %d\n",
		*from, *to,
		delta,start_theta,ld->min_theta,ld->max_theta, range, *start_cell);
}

int distance_counter = 0;

double distance_squared_d(const double a[2], const double b[2]) {
	distance_counter++;
	double x = a[0]-b[0];
	double y = a[1]-b[1];
	return x*x + y*y;
}

double distance_d(const double a[2], const double b[2]) {
	return sqrt(distance_squared_d(a,b));
}

int is_nan(double v) {
	return v == v ? 0 : 1;
}

int any_nan(const double *d, int n) {
	int i; for(i=0;i<n;i++)
		if(is_nan(d[i]))
			return 1;
	return 0;
}

double norm_d(const double p[2]) {
	return sqrt(p[0]*p[0]+p[1]*p[1]);
}

double deg2rad(double deg) {
	return deg * (M_PI / 180);
}

double rad2deg(double rad) {
	return rad * (180 / M_PI);
}

void copy_d(const double*from, int n, double*to) {
	int i; for(i=0;i<n;i++) to[i] = from[i];
}

void ominus_d(const double x[3], double res[3]) {
	double c = cos(x[2]);
	double s = sin(x[2]);
	res[0] = -c*x[0]-s*x[1];
	res[1] =  s*x[0]-c*x[1];
	res[2] = -x[2];
}

/** safe if res == x1 */
void oplus_d(const double x1[3], const double x2[3], double res[3]) {
	double c = cos(x1[2]);
	double s = sin(x1[2]);
	double x = x1[0]+c*x2[0]-s*x2[1];
	double y = x1[1]+s*x2[0]+c*x2[1];
 	double theta = x1[2]+x2[2];
	res[0]=x;
	res[1]=y;
	res[2]=theta;
}

void transform_d(const double point2d[2], const double pose[3], double result2d[2]) {
	double theta = pose[2];
	double c = cos(theta); double s = sin(theta);
	result2d[0] = pose[0] + c * point2d[0] - s * point2d[1];
	result2d[1] = pose[1] + s * point2d[0] + c * point2d[1];
}

void pose_diff_d(const double pose2[3], const double pose1[3], double res[3]) {
	double temp[3];
	ominus_d(pose1, temp);
	oplus_d(temp, pose2, res);

	while(res[2] > +M_PI) res[2] -= 2*M_PI;
	while(res[2] < -M_PI) res[2] += 2*M_PI;
}

double square(double x) {
	return x*x;
}

double angleDiff(double a, double b) {
	double t = a - b;
	while(t<-M_PI) t+= 2*M_PI;
	while(t>M_PI)  t-= 2*M_PI;
	return t;
}

void projection_on_line_d(const double a[2],
	const double b[2],
	const double p[2],
	double res[2], double *distance)
{
	double t0 = a[0]-b[0];
	double t1 = a[1]-b[1];
	double one_on_r = 1 / sqrt(t0*t0+t1*t1);
	/* normal */
	double nx = t1  * one_on_r ;
	double ny = -t0 * one_on_r ;
	double c= nx, s = ny;
	double rho = c*a[0]+s*a[1];

	res[0] =   c*rho + s*s*p[0] - c*s*p[1] ;
	res[1] =   s*rho - c*s*p[0] + c*c*p[1] ;

	if(distance)
		*distance = fabs(rho-(c*p[0]+s*p[1]));
}

void projection_on_segment_d(
	const double a[2],
	const double b[2],
	const double x[2],
	double proj[2])
{
	projection_on_line_d(a,b,x,proj,0);
	if ((proj[0]-a[0])*(proj[0]-b[0]) +
	    (proj[1]-a[1])*(proj[1]-b[1]) < 0 ) {
		/* the projection is inside the segment */
	} else
		if(distance_squared_d(a,x) < distance_squared_d(b,x))
			copy_d(a,2,proj);
		else
			copy_d(b,2,proj);
}

double dist_to_segment_squared_d(const double a[2], const double b[2], const double x[2]) {
	double projection[2];
	projection_on_segment_d(a, b, x, projection);
	double distance_sq_d = distance_squared_d(projection, x);
	return distance_sq_d;
}

double dist_to_segment_d(const double a[2], const double b[2], const double x[2]) {
	double proj[2]; double distance;
	projection_on_line_d(a,b,x,proj, &distance);
	if ((proj[0]-a[0])*(proj[0]-b[0]) +
	    (proj[1]-a[1])*(proj[1]-b[1]) < 0 ) {
		/* the projection is inside the segment */
		return distance;
	} else
		return sqrt(GSL_MIN( distance_squared_d(a,x), distance_squared_d(b,x)));
}

int count_equal(const int*v, int n, int value) {
	int num = 0, i;
	for(i=0;i<n;i++) if(value == v[i]) num++;
	return num;
}

double normalize_0_2PI(double t) {
	if(is_nan(t)) {
		sm_error("Passed NAN to normalize_0_2PI().\n");
		return GSL_NAN;
	}
	while(t<0) t+=2*M_PI;
	while(t>=2*M_PI) t-=2*M_PI;
	return t;
}

double dot_d(const double p[2], const double q[2]);

double dot_d(const double p[2], const double q[2]) {
	return p[0]*q[0] + p[1]*q[1];
}

/* Executes ray tracing for a segment. p0 and p1 are the segments extrema, eye is the position
of the eye, and direction is the direction of the ray coming out of the eye. Returns true
if the ray intersects the segment, and in that case *range contains the length of the ray. */
int segment_ray_tracing(const double p0[2], const double p1[2], const double eye[2], double direction, double*range) {

	*range = NAN;

	// p0 - p1
	double arrow[2] = {p1[0]-p0[0],p1[1]-p0[1]};
	// Normal to segment line
	double S[2] = { -arrow[1], arrow[0]};
	// Viewing direction
	double N[2] = { cos(direction), sin(direction)};
	// If S*N = 0 then they cannot cross
	double S_dot_N = dot_d(S,N);
	if( S_dot_N == 0) return 0;
	// Rho of the line in polar coordinates (multiplied by |S|)
	double line_rho = dot_d(p0,S);
	// Rho of the eye  (multiplied by |S|)
	double eye_rho = dot_d(eye,S);
	// Black magic
	double dist = (line_rho - eye_rho) / S_dot_N;
	if(dist<=0) return 0;

	// Now we check whether the crossing point
	// with the line lies within the segment

	// Crossing point
	double crossing[2] = {eye[0] + N[0]*dist, eye[1]+N[1]*dist};
	// Half of the segment
	double midpoint[2] = { 0.5*(p1[0]+p0[0]),0.5*(p1[1]+p0[1])};

	double seg_size = distance_d(p0, p1);
	double dist_to_midpoint = distance_d(crossing, midpoint);

	if(dist_to_midpoint > seg_size/2 )
		return 0;

	*range = dist;
	return 1;
}

double segment_alpha(const double p0[2], const double p1[2]) {
	double arrow[2] = {p1[0]-p0[0],p1[1]-p0[1]};
	// Normal to segment line
	double S[2] = { -arrow[1], arrow[0]};
	return atan2(S[1], S[0]);
}

static char tmp_buf[1024];
const char* friendly_pose(const double*pose) {
	sprintf(tmp_buf, "(%4.2f mm, %4.2f mm, %4.4f deg)",
		1000*pose[0],1000*pose[1],rad2deg(pose[2]));
	return tmp_buf;
}

double max_in_array(const double*v, int n) {
	assert(n>0);
	double m = v[0];
	int i;
	for(i=0;i<n;i++)
		if(v[i]>m) m = v[i];
	return m;
}


/**
  * Copyright 2016 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
  * Matrix Market Suite is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
  * along with Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

#include <lapacke.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>

#include "LUDecomposition.h"

void usageLUDecomposition(){

	fprintf(stderr, "\n");
	fprintf(stderr, "Usage: MM-Suite LUDecomposition [options] <input-matrix>\n");
	fprintf(stderr, "\nInput/output options:\n\n");
	fprintf(stderr, "       -o STR        Output file name. Default: stdout\n");
	fprintf(stderr, "       -r            Input format is row per line. Default: False\n");
	fprintf(stderr, "\n");

}

int LUDecomposition(int argc, char *argv[]) {

	int 			ret_code = 1;
	int 			option;

	unsigned long 		*II;

	unsigned long 		*J;
	double 			*values;

	unsigned long 		M;
	unsigned long 		N;
	unsigned long long 	nz;

	int			_M;
	int			_N;

	char			*outputFileName = NULL;

	char			*inputMatrixFile = NULL;

	int			inputFormatRow = 0;

	while ((option = getopt(argc, argv,"ro:")) >= 0) {
		switch (option) {
			case 'o' :
				//free(outputFileName);

				outputFileName = (char *) malloc(sizeof(char)*strlen(optarg)+1);
				strcpy(outputFileName,optarg);

				break;

			case 'r':
				inputFormatRow = 1;
				break;

			default: break;
		}

	}

	if ((optind + 1 > argc) || (optind + 2 <= argc)) {
		usageLUDecomposition();
		return 0;
	}

	if(outputFileName == NULL) {
		outputFileName = (char *) malloc(sizeof(char)*7);
		sprintf(outputFileName,"stdout");
	}

	inputMatrixFile = (char *)malloc(sizeof(char)*strlen(argv[optind])+1);

	strcpy(inputMatrixFile,argv[optind]);

	//Read matrix
	if(inputFormatRow){

		if(!readDenseCoordinateMatrixRowLine(inputMatrixFile,&II,&J,&values,&M,&N,&nz)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}

	}
	else {
		if(!readDenseCoordinateMatrix(inputMatrixFile,&II,&J,&values,&M,&N,&nz)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}
	}

	_M = (int) M;
	_N = (int) N;

	int ipiv[_M];

	/*
	lapack_int LAPACKE_dgetrf( int matrix_layout, lapack_int m, lapack_int n,
                           double* a, lapack_int lda, lapack_int* ipiv );
	*/

        ret_code = LAPACKE_dgetrf(LAPACK_ROW_MAJOR,_M,_N, values,_N, ipiv);

	if(inputFormatRow){
		writeLUCoordinateMatrixRowLine(outputFileName, values,M,N,nz, ipiv);
	}
	else{
		writeLUCoordinateMatrix(outputFileName, values,M,N,nz, ipiv);
	}

	//writeDenseCoordinateMatrix("stdout",values,M,N,nz);
	if(!ret_code)
		return 1;
	else
		return 0;

}


/*
 * Tema 2 ASC
 * 2020 Spring
 */
#include <cblas.h>
#include "utils.h"

#define IDX2L(i, j, N) (((i) * (N)) + (j))

/*
 * Add your BLAS implementation here
 */

double* my_solver(int N, double *A, double *B) {
	int i, j;
	double alpha = 1.0;
	/* Calculate the final result in C matrix */
	double *C = calloc(N * N, sizeof(double));
	if (C == NULL) {
		return NULL;
	}
	/* Calculate the B*At part in D */
	double *D = calloc(N * N, sizeof(double));
	if (D == NULL) {
		return NULL;
	}
	/* Calculate the A * A in A_square */
	double *A_square = calloc(N * N, sizeof(double));
	if (A_square == NULL) {
		return NULL;
	}

	/* Copy B matrix in D matrix */
	for (i = 0; i != N; i++) {
		for (j = 0; j != N; j++) {
			D[IDX2L(i, j, N)] = B[IDX2L(i, j, N)];
		}
	}
	/* Copy A matrix in A_square matrix */
	for (i = 0; i != N; i++) {
		for (j = 0; j != N; j++) {
			if ( i <= j) {
				A_square[IDX2L(i, j, N)] = A[IDX2L(i, j, N)];
			}
		}
	}
	/*
	 * Multiply matrixes B * At, where At is A transposed
	 * The result is saved in D matrix
	 */
	cblas_dtrmm(CblasRowMajor, CblasRight, CblasUpper, CblasTrans,
				CblasNonUnit, N, N, alpha, A, N, D, N);
	/*
	 * Multiply matrixes A * A, saved result in A_square
	 * Matrix A_square is a triangular matrix
	 */
	cblas_dtrmm(CblasRowMajor, CblasRight, CblasUpper, CblasNoTrans,
			    CblasNonUnit, N, N, alpha, A, N, A_square, N);

	/*
	 * Multiply matrixes A_square * B, where A_square = A * A
	 * The result is saved in B matrix
	 */
	cblas_dtrmm(CblasRowMajor, CblasLeft, CblasUpper, CblasNoTrans,
				CblasNonUnit, N, N, alpha, A_square, N, B, N);

	/* Add matrixes D and B, save the result in C matrix */
	for (i = 0; i != N; i++) {
		for (j = 0; j != N; j++) {
			C[IDX2L(i, j, N)] = D[IDX2L(i, j, N)] + B[IDX2L(i, j, N)];
		}
	}
	/* Free memory from D and A_square */
	free(D);
	free(A_square);
	printf("BLAS SOLVER\n");
	return C;
}