text:
#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;

};

ext: c

max_stars_repo_name: shaesaert/TuLiPXML

max_issues_repo_name: shaesaert/TuLiPXML

max_forks_repo_name: shaesaert/TuLiPXML


text:
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#ifndef gauge_config
#include "config.h"
#define gauge_config 1

#define sq(x) ((x)*(x))
#define cb(x) ((x)*(x)*(x))
#endif

// SU(3) Matrix
// only the first two rows carries useful information (Lang p.80)
// note that the first two rows must be orthonormal
// use su3_normalize() for this
typedef struct{
  gsl_complex a1;
  gsl_complex a2;
  gsl_complex a3;
  gsl_complex b1;
  gsl_complex b2;
  gsl_complex b3;
} su3_matrix;

// SU(3) matrix methods --------------------------------------

// print matrix
void su3_print(const su3_matrix A){
  printf("%f + i*%f \t", GSL_REAL(A.a1), GSL_IMAG(A.a1));
  printf("%f + i*%f \t", GSL_REAL(A.a2), GSL_IMAG(A.a2));
  printf("%f + i*%f \n", GSL_REAL(A.a3), GSL_IMAG(A.a3));
  printf("%f + i*%f \t", GSL_REAL(A.b1), GSL_IMAG(A.b1));
  printf("%f + i*%f \t", GSL_REAL(A.b2), GSL_IMAG(A.b2));
  printf("%f + i*%f \n", GSL_REAL(A.b3), GSL_IMAG(A.b3));
}

// the identity matrix
su3_matrix su3_unit(){
  su3_matrix u;
  u.a1=GSL_COMPLEX_ONE;
  u.a2=GSL_COMPLEX_ZERO;
  u.a3=GSL_COMPLEX_ZERO;
  u.b1=GSL_COMPLEX_ZERO;
  u.b2=GSL_COMPLEX_ONE;
  u.b3=GSL_COMPLEX_ZERO;
  return u;
}

// normalize matrix
su3_matrix su3_norm(const su3_matrix A){
  su3_matrix result;
  // normalize first row
  double norm = sqrt(gsl_complex_abs2(A.a1)+gsl_complex_abs2(A.a2)+gsl_complex_abs2(A.a3));
  result.a1 = gsl_complex_div(A.a1,gsl_complex_rect(norm,0));
  result.a2 = gsl_complex_div(A.a2,gsl_complex_rect(norm,0));
  result.a3 = gsl_complex_div(A.a3,gsl_complex_rect(norm,0));

  // orthogonize second row
  gsl_complex dprod = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,gsl_complex_conjugate(result.a1)),
                      gsl_complex_mul(A.b2,gsl_complex_conjugate(result.a2))),
                      gsl_complex_mul(A.b3,gsl_complex_conjugate(result.a3)));
  result.b1 = gsl_complex_sub(A.b1,gsl_complex_mul(result.a1,dprod));
  result.b2 = gsl_complex_sub(A.b2,gsl_complex_mul(result.a2,dprod));
  result.b3 = gsl_complex_sub(A.b3,gsl_complex_mul(result.a3,dprod));

  // normalize second row
  norm = sqrt(gsl_complex_abs2(result.b1)+gsl_complex_abs2(result.b2)+gsl_complex_abs2(result.b3));
  result.b1 = gsl_complex_div(result.b1,gsl_complex_rect(norm,0));
  result.b2 = gsl_complex_div(result.b2,gsl_complex_rect(norm,0));
  result.b3 = gsl_complex_div(result.b3,gsl_complex_rect(norm,0));
  return result;
}

// multiply matrices
su3_matrix su3_mul(const su3_matrix A, const su3_matrix B){
  su3_matrix result;
  gsl_complex w1,w2,w3;

  // Third row of B as a cross product (c.f. Lang p.80)
  w1 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a2),gsl_complex_conjugate(B.b3)), gsl_complex_mul(gsl_complex_conjugate(B.a3),gsl_complex_conjugate(B.b2)) );
  w2 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a3),gsl_complex_conjugate(B.b1)), gsl_complex_mul(gsl_complex_conjugate(B.a1),gsl_complex_conjugate(B.b3)) );
  w3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a1),gsl_complex_conjugate(B.b2)), gsl_complex_mul(gsl_complex_conjugate(B.a2),gsl_complex_conjugate(B.b1)) );

  result.a1 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a1),gsl_complex_mul(A.a2,B.b1)), gsl_complex_mul(A.a3,w1));
  result.a2 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a2),gsl_complex_mul(A.a2,B.b2)), gsl_complex_mul(A.a3,w2));
  result.a3 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a3),gsl_complex_mul(A.a2,B.b3)), gsl_complex_mul(A.a3,w3));

  result.b1 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a1),gsl_complex_mul(A.b2,B.b1)), gsl_complex_mul(A.b3,w1));
  result.b2 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a2),gsl_complex_mul(A.b2,B.b2)), gsl_complex_mul(A.b3,w2));
  result.b3 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a3),gsl_complex_mul(A.b2,B.b3)), gsl_complex_mul(A.b3,w3));

  return result;
}

// inverse matrix
su3_matrix su3_inv(const su3_matrix A){
  su3_matrix result;
  gsl_complex w1,w2,w3;

  // Third row of B as a cross product (c.f. Lang p.80)
  w1 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b3)), gsl_complex_mul(gsl_complex_conjugate(A.a3),gsl_complex_conjugate(A.b2)) );
  w2 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a3),gsl_complex_conjugate(A.b1)), gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b3)) );
  w3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b2)), gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b1)) );

  result.a1 = gsl_complex_sub( gsl_complex_mul(A.b2,w3), gsl_complex_mul( A.b3,w2) );
  result.a2 = gsl_complex_sub( gsl_complex_mul(A.a3,w2), gsl_complex_mul( A.a2,w3) );
  result.a3 = gsl_complex_sub( gsl_complex_mul(A.a2,A.b3), gsl_complex_mul( A.a3,A.b2) );

  result.b1 = gsl_complex_sub( gsl_complex_mul(A.b3,w1), gsl_complex_mul( A.b1,w3) );
  result.b2 = gsl_complex_sub( gsl_complex_mul(A.a1,w3), gsl_complex_mul( A.a3,w1) );
  result.b3 = gsl_complex_sub( gsl_complex_mul(A.a3,A.b1), gsl_complex_mul( A.a1,A.b3) );

  return result;
}

// generate random matrix
su3_matrix su3_rand(const gsl_rng* r){
  su3_matrix result;
  result.a1 = gsl_complex_rect(1,eps*(gsl_rng_uniform(r)*2-1));
  result.a2 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  result.a3 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));

  result.b1 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  result.b2 = gsl_complex_rect(1,eps*(gsl_rng_uniform(r)*2-1));
  result.b3 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  return su3_norm(result);
}

double su3_trace(const su3_matrix A){
  gsl_complex c3;
  c3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b2)), gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b1)) );

  return GSL_REAL(gsl_complex_add(gsl_complex_add(A.a1,A.b2),c3));
}

// end of SU(3) matrix methods ---------------------------------------------

max_stars_repo_name: uchuutamashi/lqcd

max_issues_repo_name: uchuutamashi/lqcd

max_forks_repo_name: uchuutamashi/lqcd


text:
#ifndef _MARKET_INTEGRATION_
#define _MARKET_INTEGRATION_
#include <gsl/gsl_integration.h>

typedef struct{
	double (*function)(double *x, size_t n, void * p);
	void *params;
} nquad_function;

typedef void (*multi_integration_type)(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr);

void nquad(int n, nquad_function *f,  double *lb, double *ub,
           double epsabs, double epsrel, size_t limit,
           double * result, double *abserr);

void mcint(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr);

#endif

ext: h


text:
//
//  Author
//      luncliff@gmail.com
//
#pragma once
#ifndef _NDCAM_INCLUDE_H_
#define _NDCAM_INCLUDE_H_

#include <gsl/gsl>

#include <android/hardware_buffer.h>
#include <android/native_window.h>
#include <android/native_window_jni.h>

#include <media/NdkImage.h>
#include <media/NdkImageReader.h>

#include <camera/NdkCameraCaptureSession.h>
#include <camera/NdkCameraDevice.h>
#include <camera/NdkCameraError.h>
#include <camera/NdkCameraManager.h>
#include <camera/NdkCameraMetadata.h>
#include <camera/NdkCameraMetadataTags.h>
#include <camera/NdkCaptureRequest.h>

using native_window_ptr =
    std::unique_ptr<ANativeWindow, void (*)(ANativeWindow*)>;
using capture_session_output_container_ptr =
    std::unique_ptr<ACaptureSessionOutputContainer,
                    void (*)(ACaptureSessionOutputContainer*)>;
using capture_session_output_ptr =
    std::unique_ptr<ACaptureSessionOutput, void (*)(ACaptureSessionOutput*)>;
using capture_request_ptr =
    std::unique_ptr<ACaptureRequest, void (*)(ACaptureRequest*)>;

using camera_output_target_ptr =
    std::unique_ptr<ACameraOutputTarget, void (*)(ACameraOutputTarget*)>;

/**
 * Library context. Supports auto releasing and facade for features
 *
 * !!! All NDK type members must be public. There will be no encapsulation !!!
 */
struct camera_group_t final {
    // without external camera, 2 is enough(back + front).
    // But we will use more since there might be multiple(for now, 2) external
    // camera...
    static constexpr auto max_camera_count = 4;

  public:
    // Android camera manager. After the context is initialized, this must be
    // non-null if this variable is null, then the context is considered as 'not
    // initailized' and all operation *can* be ignored
    ACameraManager* manager = nullptr;
    ACameraIdList* id_list = nullptr;

    // cached metadata
    std::array<ACameraMetadata*, max_camera_count> metadata_set{};

    //
    // even though android system limits the number of maximum open camera
    // device, we will consider multiple camera are working concurrently.
    //
    // if element is nullptr, it means the device is not open.
    std::array<ACameraDevice*, max_camera_count> device_set{};

    // if there is no session, session pointer will be null
    std::array<ACameraCaptureSession*, max_camera_count> session_set{};

    // sequence number from capture session
    std::array<int, max_camera_count> seq_id_set{};

  public:
    camera_group_t() noexcept = default;
    // copy-move is disabled
    camera_group_t(const camera_group_t&) = delete;
    camera_group_t(camera_group_t&&) = delete;
    camera_group_t& operator=(const camera_group_t&) = delete;
    camera_group_t& operator=(camera_group_t&&) = delete;
    ~camera_group_t() noexcept {
        this->release(); // ensure release precedure
    }

  public:
    void release() noexcept;

  public:
    auto open_device(uint16_t id,
                     ACameraDevice_StateCallbacks& callbacks) noexcept
        -> camera_status_t;

    // Notice that this routine doesn't free metadata
    void close_device(uint16_t id) noexcept;

    auto start_repeat(
        uint16_t id, ANativeWindow* window,
        ACameraCaptureSession_stateCallbacks& on_session_changed,
        ACameraCaptureSession_captureCallbacks& on_capture_event) noexcept
        -> camera_status_t;
    void stop_repeat(uint16_t id) noexcept;

    auto start_capture(
        uint16_t id, ANativeWindow* window,
        ACameraCaptureSession_stateCallbacks& on_session_changed,
        ACameraCaptureSession_captureCallbacks& on_capture_event) noexcept
        -> camera_status_t;
    void stop_capture(uint16_t id) noexcept;

    // ACAMERA_LENS_FACING_FRONT
    // ACAMERA_LENS_FACING_BACK
    // ACAMERA_LENS_FACING_EXTERNAL
    uint16_t get_facing(uint16_t id) noexcept;
};

// device callbacks

void context_on_device_disconnected(camera_group_t& context,
                                    ACameraDevice* device) noexcept;

void context_on_device_error(camera_group_t& context, ACameraDevice* device,
                             int error) noexcept;

// session state callbacks

void context_on_session_active(camera_group_t& context,
                               ACameraCaptureSession* session) noexcept;

void context_on_session_closed(camera_group_t& context,
                               ACameraCaptureSession* session) noexcept;

void context_on_session_ready(camera_group_t& context,
                              ACameraCaptureSession* session) noexcept;

// capture callbacks

void context_on_capture_started(camera_group_t& context,
                                ACameraCaptureSession* session,
                                const ACaptureRequest* request,
                                uint64_t time_point) noexcept;

void context_on_capture_progressed(camera_group_t& context,
                                   ACameraCaptureSession* session,
                                   ACaptureRequest* request,
                                   const ACameraMetadata* result) noexcept;

void context_on_capture_completed(camera_group_t& context,
                                  ACameraCaptureSession* session,
                                  ACaptureRequest* request,
                                  const ACameraMetadata* result) noexcept;

void context_on_capture_failed(camera_group_t& context,
                               ACameraCaptureSession* session,
                               ACaptureRequest* request,
                               ACameraCaptureFailure* failure) noexcept;

void context_on_capture_buffer_lost(camera_group_t& context,
                                    ACameraCaptureSession* session,
                                    ACaptureRequest* request,
                                    ANativeWindow* window,
                                    int64_t frame_number) noexcept;

void context_on_capture_sequence_abort(camera_group_t& context,
                                       ACameraCaptureSession* session,
                                       int sequence_id) noexcept;

void context_on_capture_sequence_complete(camera_group_t& context,
                                          ACameraCaptureSession* session,
                                          int sequence_id,
                                          int64_t frame_number) noexcept;

// status - error code to string

auto camera_error_message(camera_status_t status) noexcept -> const char*;

#endif

max_stars_repo_name: luncliff/NdkCamera

max_issues_repo_name: jerry-deng-pico/NdkCamera

max_forks_repo_name: jerry-deng-pico/NdkCamera


text:
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>

int
main (void)
{
  double x = 5.0;
  gsl_sf_result result;

  double expected = -0.17759677131433830434739701;

  int status = gsl_sf_bessel_J0_e (x, &result);

  printf ("status  = %s\n", gsl_strerror(status));
  printf ("J0(5.0) = %.18f\n"
          "      +/- % .18f\n",
          result.val, result.err);
  printf ("exact   = %.18f\n", expected);
  return status;
}

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
//  This random generator is a C++ wrapper for the GNU Scientific Library
//  Copyright (C) 2001 Torbjorn Vik

//  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.
#ifndef __multimin_fdfminimizer_h
#define __multimin_fdfminimizer_h

#include <gsl/gsl_errno.h>
#include <gsl/gsl_multimin.h>
#include <gslwrap/vector_double.h>

namespace gsl{

//! Create an instance of this class with a user defined function
/*!
  A template class with the function operator()(const vector& x)
  and derivative(const vector&, vector&), as well as a reference to an object of this class must be fournished

  User is responsible for deleting this reference !

 */
template <class fdf_function>
class multimin_fdf
{
 public:
	fdf_function* fct;

	//! These operators can be overridden
	virtual double operator()(const vector& x)
		{
			return (*fct)(x);
		}
	virtual void derivative(const vector& x, vector& g)
		{
			(*fct).derivative(x, g);
		}

	//! This operator can be overridden to gain performance in calculating the value and its derivative in a scoop
	virtual double fval_and_derivative(const vector&x, vector& g )
	{
		derivative(x, g);
		return (*this)(x);
	}

	//! This is the function gsl calls to calculate the value of f at x
	static double f(const gsl_vector* x, void *p)
	{
		vector_view x_view(*x);
		return (*(multimin_fdf *)p)(x_view);
	}

	//! This is the function gsl calls to calculate the value of g=f' at x
	static void df(const gsl_vector* x, void *p, gsl_vector* g)
	{
		vector_view x_view(*x);
		vector_view g_view(*g);
		(*(multimin_fdf *)p).derivative(x_view, g_view);
	}

	//! This is the function gsl calls to calculate the value of g=f' at x
	static void fdf(const gsl_vector* x, void *p, double* f, gsl_vector* g)
	{
		vector_view x_view(*x);
		vector_view g_view(*g);
		*f=(*(multimin_fdf *)p).fval_and_derivative(x_view, g_view);
	}

	//! Constructor (User is responsible for deleting the fdf_function object)
	multimin_fdf(fdf_function* _fct):fct(_fct){assert (fct!=NULL);}
};

//! Class for multiminimizing one dimensional functions.
/*!
  Usage:
       - Create with optional multiminimize type
	   - Set with function object and inital bounds
	   - Loop the  iterate function until convergence or maxIterations (extra facility)

	   - Recover multiminimum and bounds
 */
class multimin_fdfminimizer
{
 public:
//!
/*! Choose between :
  - gsl_multimin_fdfminimizer_conjugate_fr
  - gsl_multimin_fdfminimizer_conjugate_pr
  - gsl_multimin_fdfminimizer_vector_bfgs
  - gsl_multimin_fdfminimizer_steepest_descent

 */
	multimin_fdfminimizer(uint _dim,
						  const gsl_multimin_fdfminimizer_type* type=gsl_multimin_fdfminimizer_conjugate_fr) :
		dim(_dim), isSet(false), maxIterations(100), s(NULL)
	{
		s=gsl_multimin_fdfminimizer_alloc(type, dim);
		nIterations=0;
		if (!s)
		{
			//error
			//cout << "ERROR Couldn't allocate memory for multiminimizer" << endl;
			//throw ?
			exit(-1);
		}
	}
	~multimin_fdfminimizer(){if (s) gsl_multimin_fdfminimizer_free(s);}
	//! returns GSL_FAILURE if the interval does not contain a multiminimum
	template <class  fdf_function>
	int set(multimin_fdf<fdf_function>& function, const vector& initial_x, double step_size, double tol)
	{
		isSet=false;
		f.f   = &function.f;
		f.df  = &function.df;
		f.fdf = &function.fdf;
		f.n   = dim;
		f.params = &function;
		int status=	gsl_multimin_fdfminimizer_set(s, &f, initial_x.gslobj(), step_size, tol);
		if (!status)
		{
			isSet=true;
			nIterations=0;
		}
		return status;
	}
	int iterate()
	{
		assert_set();
		int status=gsl_multimin_fdfminimizer_iterate(s);
		nIterations++;
		if (status==GSL_FAILURE)
			isConverged=true;
		return status;
	}
	int restart(){return gsl_multimin_fdfminimizer_restart(s);}
  	double minimum(){assert_set();return gsl_multimin_fdfminimizer_minimum(s);}
	vector x_value(){assert_set();return vector_view(*gsl_multimin_fdfminimizer_x(s));}
	vector gradient(){assert_set();return vector_view(*gsl_multimin_fdfminimizer_gradient(s));}

	void SetMaxIterations(int n){maxIterations=n;}
	int GetNIterations(){return nIterations;}
	bool is_converged(){if (nIterations>=maxIterations) return true; if (isConverged) return true; return false;}
	//string name() const;

 private:
	void assert_set(){if (!isSet)exit(-1);} // Old problem of error handling: TODO

	uint dim;
	bool isSet;
	bool isConverged;
	int nIterations;
	int maxIterations;
	gsl_multimin_fdfminimizer* s;
	gsl_multimin_function_fdf f;
};
};	 // namespace gsl

#endif //__multimin_fdfminimizer_h

max_stars_repo_name: entn-at/GlottDNN

max_issues_repo_name: entn-at/GlottDNN

max_forks_repo_name: entn-at/GlottDNN


text:
/*
 *  gsl_headers.h
 *  Pi4U
 *
 *  Created by Panagiotis Hadjidoukas on 1/1/14.
 *  Copyright 2014 ETH Zurich. All rights reserved.
 *
 */

#ifndef _GSL_HEADERS_
#define _GSL_HEADERS_ 1

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#include <gsl/gsl_roots.h>

/* NDL */
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_sort_vector.h>

#endif

max_stars_repo_name: NMDimitriou/MetaGrowth

max_issues_repo_name: NMDimitriou/MetaGrowth

max_forks_repo_name: NMDimitriou/MetaGrowth


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

#ifndef _gemv_h
#define _gemv_h

/// @file gemv.h Computes general matrix-vector products.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Computes general real matrix-vector products.
   ///
   /// For a real matrix A and real scalars alpha, beta
   ///
   ///        y := alpha * op(A) * x + beta * y
   ///
   /// is computed, where op(A) = A or A'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param trans Specifies whether the transpose of A is used or not:
   ///
   ///        if trans = 'N' or 'n' then op(A) = A
   ///        if trans = 'T' or 't' then op(A) = A'
   ///        if trans = 'C' or 'c' then op(A) = A'
   ///
   /// @param m The number of rows of the matrix A.  m>=0
   /// @param n The number of columns of the matrix A.  n>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix m-by-n matrix A.
   /// @param ldA Column length of the matrix A.  ldA>=m.
   /// @param x Pointer to real vector x.
   /// @param incx Increment of the vector x.
   /// @param beta Real scalar.
   /// @param y Pointer to real vector y.
   /// @param incy Increment of the vector y.
   /// @ingroup BLAS

   template <typename real_t>
   int GEMV(char trans, int_t m, int_t n, real_t alpha, real_t *A, int_t ldA, real_t *x, int_t incx, real_t beta, real_t *y, int_t incy)
   {
      trans=std::toupper(trans);
      const real_t one(1.0);
      const real_t zero(0.0);
      int_t kx,ky,lenx,leny,i,ix,iy,j,jx,jy;
      real_t temp;

      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      lenx=(trans=='N')?n:m;
      leny=(trans=='N')?m:n;
      kx=(incx>0)?0:(1-lenx)*incx;
      ky=(incy>0)?0:(1-leny)*incy;

      if(beta==zero)
      {
         if(incy==1)
         {
            for(i=0;i<leny;i++)
               y[i]=zero;
         }
         else
         {
            iy=ky;
            for(i=0;i<leny;i++)
            {
               y[iy]=zero;
               iy+=incy;
            }

         }
      }
      else if(beta!=one)
      {
         if(incy==1)
         {
            for(i=0;i<leny;i++)
               y[i]*=beta;
         }
         else
         {
            iy=ky;
            for(i=0;i<leny;i++)
            {
               y[iy]*=beta;
               iy+=incy;
            }
         }
      }

      if(alpha!=zero)
      {
         if(trans=='N')
         {
            jx=kx;
            if(incy==1)
            {
               for(j=0;j<n;j++)
               {
                  temp=alpha*x[jx];
                  for(i=0;i<m;i++)
                     y[i]+=temp*A[i];
                  A+=ldA;
                  jx+=incx;
               }
            }
            else
            {
               for(j=0;j<n;j++)
               {
                  temp=alpha*x[jx];
                  iy=ky;
                  for(i=0;i<m;i++)
                  {
                     y[iy]+=temp*A[i];
                     iy+=incy;
                  }
                  A+=ldA;
                  jx+=incx;
               }
            }
         }
         else
         {
            jy=ky;
            if(incx==1)
            {
               for(j=0;j<n;j++)
               {
                  temp=zero;
                  for(i=0;i<m;i++)
                     temp+=A[i]*x[i];
                  y[jy]+=alpha*temp;
                  jy+=incy;
                  A+=ldA;
               }
            }
            else
            {
               for(j=0;j<n;j++)
               {
                  temp=zero;
                  ix=kx;
                  for(i=0;i<m;i++)
                  {
                     temp+=A[i]*x[ix];
                     ix+=incx;
                  }
                  y[jy]+=alpha*temp;
                  jy+=incy;
                  A+=ldA;
               }
            }
         }
      }
      return 0;
   }

   /// @brief Computes general complex matrix-vector products.
   ///
   /// For a complex matrix A and complex scalars alpha, beta
   ///
   ///        y := alpha * op(A) * x + beta * y
   ///
   /// is computed, where op(A) = A or A'.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param trans Specifies whether the transpose or conjugate transpose of A, or neither is used:
   ///
   ///        if trans = 'N' or 'n' then op(A) = A
   ///        if trans = 'T' or 't' then op(A) = A.'
   ///        if trans = 'C' or 'c' then op(A) = A'
   ///
   /// @param m The number of rows of the matrix A.  m>=0
   /// @param n The number of columns of the matrix A.  n>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix m-by-n matrix A.
   /// @param ldA Column length of the matrix A.  ldA>=m.
   /// @param x Pointer to complex vector x.
   /// @param incx Increment of the vector x.
   /// @param beta Complex scalar.
   /// @param y Pointer to complex vector y.
   /// @param incy Increment of the vector y.
   /// @ingroup BLAS

   template <typename real_t>
   int GEMV(char trans, int_t m, int_t n, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *x, int_t incx, complex<real_t> beta, complex<real_t> *y, int_t incy)
   {
      trans=std::toupper(trans);
      using std::conj;
      const complex<real_t> one(1.0,0.0);
      const complex<real_t> zero(0.0,0.0);
      int_t kx,ky,lenx,leny,i,ix,iy,j,jx,jy;
      complex<real_t> temp;

      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      lenx=(trans=='N')?n:m;
      leny=(trans=='N')?m:n;
      kx=(incx>0)?0:(1-lenx)*incx;
      ky=(incy>0)?0:(1-leny)*incy;

      if(beta==zero)
      {
         if(incy==1)
         {
            for(i=0;i<leny;i++)
               y[i]=zero;
         }
         else
         {
            iy=ky;
            for(i=0;i<leny;i++)
            {
               y[iy]=zero;
               iy+=incy;
            }

         }
      }
      else if(beta!=one)
      {
         if(incy==1)
         {
            for(i=0;i<leny;i++)
               y[i]*=beta;
         }
         else
         {
            iy=ky;
            for(i=0;i<leny;i++)
            {
               y[iy]*=beta;
               iy+=incy;
            }
         }
      }

      if(alpha!=zero)
      {
         switch(trans)
         {
            case 'N':
               jx=kx;
               if(incy==1)
                  for(j=0;j<n;j++)
                  {
                     temp=alpha*x[jx];
                     for(i=0;i<m;i++)
                        y[i]+=temp*A[i];
                     A+=ldA;
                     jx+=incx;
                  }
               else
                  for(j=0;j<n;j++)
                  {
                     temp=alpha*x[jx];
                     iy=ky;
                     for(i=0;i<m;i++)
                     {
                        y[iy]+=temp*A[i];
                        iy+=incy;
                     }
                     A+=ldA;
                     jx+=incx;
                  }
               break;

            case 'T':
               jy=ky;
               if(incx==1)
                  for(j=0;j<n;j++)
                  {
                     temp=zero;
                     for(i=0;i<m;i++)
                        temp+=A[i]*x[i];
                     y[jy]+=alpha*temp;
                     jy+=incy;
                     A+=ldA;
                  }
               else
                  for(j=0;j<n;j++)
                  {
                     temp=zero;
                     ix=kx;
                     for(i=0;i<m;i++)
                     {
                        temp+=A[i]*x[ix];
                        ix+=incx;
                     }
                     y[jy]+=alpha*temp;
                     jy+=incy;
                     A+=ldA;
                  }

               break;

            case 'C':
               jy=ky;
               if(incx==1)
                  for(j=0;j<n;j++)
                  {
                     temp=zero;
                     for(i=0;i<m;i++)
                        temp+=conj(A[i])*x[i];
                     y[jy]+=alpha*temp;
                     jy+=incy;
                     A+=ldA;
                  }
               else
                  for(j=0;j<n;j++)
                  {
                     temp=zero;
                     ix=kx;
                     for(i=0;i<m;i++)
                     {
                        temp+=conj(A[i])*x[ix];
                        ix+=incx;
                     }
                     y[jy]+=alpha*temp;
                     jy+=incy;
                     A+=ldA;
                  }

               break;
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int GEMV<float>(char trans, int_t m, int_t n, float alpha, float *A, int_t ldA, float *x, int_t incx, float beta, float *y, int_t incy)
   {
      trans=std::toupper(trans);
      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_sgemv(CblasColMajor,Trans,m,n,alpha,A,ldA,x,incx,beta,y,incy);

      return 0;
   }

   template <> int GEMV<double>(char trans, int_t m, int_t n, double alpha, double *A, int_t ldA, double *x, int_t incx, double beta, double *y, int_t incy)
   {
      trans=std::toupper(trans);
      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_dgemv(CblasColMajor,Trans,m,n,alpha,A,ldA,x,incx,beta,y,incy);

      return 0;
   }

   template <> int GEMV<float>(char trans, int_t m, int_t n, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *x, int_t incx, complex<float> beta, complex<float> *y, int_t incy)
   {
      trans=std::toupper(trans);
      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_cgemv(CblasColMajor,Trans,m,n,&alpha,A,ldA,x,incx,&beta,y,incy);

      return 0;
   }

   template <> int GEMV<double>(char trans, int_t m, int_t n, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *x, int_t incx, complex<double> beta, complex<double> *y, int_t incy)
   {
      trans=std::toupper(trans);
      if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -1;
      else if(m<0)
         return -2;
      else if(n<0)
         return -3;
      else if(ldA<m)
         return -6;
      else if(incx==0)
         return -8;
      else if(incy==0)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_zgemv(CblasColMajor,Trans,m,n,&alpha,A,ldA,x,incx,&beta,y,incy);

      return 0;
   }
#endif
}
#endif

max_stars_repo_name: langou/latl

max_issues_repo_name: langou/latl

max_forks_repo_name: langou/latl


text:
#include <sbmf/sbmf.h>

#define SBMF_UNUSED(x) \
	(void)x

#include <omp.h>
#include <cblas.h>
#include <lapacke.h>
#include <arpack/arpack.h>
/* #include <arpack/debug_c.h> */

/* c stdlib */
#include <stdio.h>  /* sprintf, fopen, fprintf, fread */
#include <stdlib.h> /* malloc */
#include <string.h> /* memcpy,memset */
#include <stdarg.h>
#include <assert.h>
#include <float.h>  /* isinf, isnan */

/* unix headers */
#include <signal.h>
#include <time.h>

/*
 * Getting into actual code
 */

#include "profile.c"
#include "memory/stack_allocator.c"
#include "global_state.c"
#include "indices.c"
#include "memory/bucketarray.c"
#include "memory/prioqueue.c"
#include "math/functions.c"
#include "math/matrix.c"
#include "math/find_eigenpairs.c"
#include "math/basis.c"
#include "math/hermite_integrals.c"
#include "methods/quadgk.c"
#include "methods/nlse_solver.c"
#include "methods/grosspitaevskii.c"
#include "methods/best_meanfield.c"
#include "methods/perturbation_theory.c"

/*
 * Logging definitions
 */

#define MAX_LOG_MSG_LEN 128

void sbmf_set_log_callback(sbmf_log_callback_func* func) {
	_state.log_callback = func;
}

static void sbmf_log(enum sbmf_log_level log_level, const char* fmt, va_list args) {
	if (!_state.log_callback)
		return;

	static char msg_buffer[MAX_LOG_MSG_LEN];

	vsnprintf(msg_buffer, MAX_LOG_MSG_LEN, fmt, args);
	_state.log_callback(log_level, msg_buffer);
}

void sbmf_log_info(const char* fmt, ...) {
	va_list args;
	va_start(args, fmt);
	sbmf_log(SBMF_LOG_LEVEL_INFO, fmt, args);
	va_end(args);
}

void sbmf_log_warning(const char* fmt, ...) {
	va_list args;
	va_start(args, fmt);
	sbmf_log(SBMF_LOG_LEVEL_WARNING, fmt, args);
	va_end(args);
}

void sbmf_log_error(const char* fmt, ...) {
	va_list args;
	va_start(args, fmt);
	sbmf_log(SBMF_LOG_LEVEL_ERROR, fmt, args);
	va_end(args);
}

void sbmf_log_panic(const char* fmt, ...) {
	va_list args;
	va_start(args, fmt);
	sbmf_log(SBMF_LOG_LEVEL_PANIC, fmt, args);
	va_end(args);
}

max_stars_repo_name: AntonJohansson/sbmf

max_issues_repo_name: AntonJohansson/sbmf

max_forks_repo_name: AntonJohansson/sbmf


text:
#include <stdio.h>
#include <gsl/gsl_multifit.h>

int
main (int argc, char **argv)
{
  int i, n;
  double xi, yi, ei, chisq;
  gsl_matrix *X, *cov;
  gsl_vector *y, *w, *c;

  if (argc != 2)
    {
      fprintf (stderr,"usage: fit n < data\n");
      exit (-1);
    }

  n = atoi (argv[1]);

  X = gsl_matrix_alloc (n, 3);
  y = gsl_vector_alloc (n);
  w = gsl_vector_alloc (n);

  c = gsl_vector_alloc (3);
  cov = gsl_matrix_alloc (3, 3);

  for (i = 0; i < n; i++)
    {
      int count = fscanf (stdin, "%lg %lg %lg",
                          &xi, &yi, &ei);

      if (count != 3)
        {
          fprintf (stderr, "error reading file\n");
          exit (-1);
        }

      printf ("%g %g +/- %g\n", xi, yi, ei);

      gsl_matrix_set (X, i, 0, 1.0);
      gsl_matrix_set (X, i, 1, xi);
      gsl_matrix_set (X, i, 2, xi*xi);

      gsl_vector_set (y, i, yi);
      gsl_vector_set (w, i, 1.0/(ei*ei));
    }

  {
    gsl_multifit_linear_workspace * work
      = gsl_multifit_linear_alloc (n, 3);
    gsl_multifit_wlinear (X, w, y, c, cov,
                          &chisq, work);
    gsl_multifit_linear_free (work);
  }

#define C(i) (gsl_vector_get(c,(i)))
#define COV(i,j) (gsl_matrix_get(cov,(i),(j)))

  {
    printf ("# best fit: Y = %g + %g X + %g X^2\n",
            C(0), C(1), C(2));

    printf ("# covariance matrix:\n");
    printf ("[ %+.5e, %+.5e, %+.5e  \n",
               COV(0,0), COV(0,1), COV(0,2));
    printf ("  %+.5e, %+.5e, %+.5e  \n",
               COV(1,0), COV(1,1), COV(1,2));
    printf ("  %+.5e, %+.5e, %+.5e ]\n",
               COV(2,0), COV(2,1), COV(2,2));
    printf ("# chisq = %g\n", chisq);
  }

  gsl_matrix_free (X);
  gsl_vector_free (y);
  gsl_vector_free (w);
  gsl_vector_free (c);
  gsl_matrix_free (cov);

  return 0;
}


text:
///////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2015 Microsoft Corporation. All rights reserved.
//
// This code is licensed under the MIT License (MIT).
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
///////////////////////////////////////////////////////////////////////////////

#ifndef GSL_GSL_H
#define GSL_GSL_H

#include <gsl/gsl_algorithm> // copy
#include <gsl/gsl_assert>    // Ensures/Expects
#include <gsl/gsl_byte>      // byte
#include <gsl/gsl_util>      // finally()/narrow()/narrow_cast()...
#include <gsl/multi_span>    // multi_span, strided_span...
#include <gsl/pointers>      // owner, not_null
#include <gsl/span>          // span
#include <gsl/string_span>   // zstring, string_span, zstring_builder...

#endif // GSL_GSL_H

max_stars_repo_name: Redchards/CVRP

max_issues_repo_name: Redchards/CVRP

max_forks_repo_name: Redchards/CVRP


text:
/* C functions for running vbgmm from Cython*/

/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/stat.h>
#include <float.h>

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>
#include <pthread.h>
#include <gsl/gsl_sf_exp.h>
#include <omp.h>

/*User includes*/
#include "c_vbgmm_fit.h"

void c_vbgmm_fit (double* adX, int nN, int nD, int nK, int seed, int* anAssign, int nThreads, int nIter)
{
    int debug = 0;
    int bAssign = 0;

    if (nIter < 1){
        nIter = DEF_MAX_ITER;
    }

    driverMP(adX, nN, nD, anAssign, nK, seed, nIter, DEF_EPSILON, debug, bAssign, nThreads);

    return;
}

int driverMP(double *adX, int nN, int nD, int *anAssign, int nKStart, unsigned long lSeed,
                                        int nMaxIter, double dEpsilon, int debug, int bAssign, int nThreads)
{
    t_Params           tParams;
    t_Data             tData;
    gsl_rng            *ptGSLRNG     = NULL;
    const gsl_rng_type *ptGSLRNGType = NULL;
    t_VBParams tVBParams;
    t_Cluster  *ptCluster = NULL;
    int i = 0, nK = nKStart, nNthreads = 0;
    int nT = 0;
    char *szCOutFile = NULL;

    /*initialise GSL RNG*/
    gsl_rng_env_setup();

    gsl_set_error_handler_off();

    ptGSLRNGType = gsl_rng_default;
    ptGSLRNG     = gsl_rng_alloc(ptGSLRNGType);

    /*set OMP thread number*/
    nNthreads = nThreads;
    nT = nN / 32  + 1;
    printf("%d %d %d\n",nN,nT,nNthreads);
    if (nT < nNthreads){
        nNthreads = nT;
    }

    omp_set_num_threads(nNthreads);
    fprintf(stderr,"Setting %d OMP threads\n",nNthreads);

    /*set clusters params*/
    tParams.nKStart  = nKStart;
    tParams.nMaxIter = nMaxIter;
    tParams.dEpsilon = dEpsilon;
    tParams.lSeed    = lSeed;

    fprintf(stderr,"Generate input data\n");
    fflush(stderr);
    generateInputData(adX, nN, nD, &tData);

    setVBParams(&tVBParams, &tData);

    if(debug>0){
        szCOutFile = (char *) malloc(sizeof(char)*MAX_FILE_NAME_LENGTH);

	    sprintf(szCOutFile,"%sr%d.csv",DEF_FILE_STUB,0);
	}

    if(bAssign > 0){
        nK = 0;

        for(i = 0; i < nN; i++){
            if(anAssign[i] > nK){
                nK = anAssign[i];
            }
        }
    }
    ptCluster = malloc(sizeof(t_Cluster));
    allocateCluster(ptCluster,nN,nK,nD,&tData,lSeed,nMaxIter,dEpsilon,szCOutFile);

    ptCluster->bAssign = bAssign;
    if(bAssign > 0){
        for(i = 0; i < nN; i++){
            ptCluster->anMaxZ[i] = anAssign[i];
        }
    }

    ptCluster->ptVBParams = &tVBParams;

    ptCluster->nThread = 0;

    fitEM_MP((void *) ptCluster);

    compressCluster(ptCluster);

    calcCovarMatrices(ptCluster,&tData);

    for(i = 0; i < nN; i++){
      anAssign[i] = ptCluster->anMaxZ[i];
    }

    if(debug>0){
        free(szCOutFile);
	}

    /*free up memory in data object*/
    destroyData(&tData);

    /*free up best BIC clusters*/

    destroyCluster(ptCluster);
    free(ptCluster);

    gsl_rng_free(ptGSLRNG);
    gsl_matrix_free(tVBParams.ptInvW0);

    return EXIT_SUCCESS;

    memoryError:
        fprintf(stderr, "Failed allocating memory in driver\n");
        fflush(stderr);
        exit(EXIT_FAILURE);
}

void generateInputData(double *adX, int nN, int nD, t_Data *ptData)
{
    double  **aadX = NULL;
    int  i = 0, j = 0;

    /*allocate memory for data matrix*/
    aadX = (double **) malloc(nN*sizeof(double*));
    if(!aadX)
        goto memoryError;

    for(i = 0; i < nN; i++){
        aadX[i] = (double *) malloc(nD*sizeof(double));
        if(!aadX[i])
	        goto memoryError;
    }

    for(i = 0; i < nN; i++){
        for(j = 0; j < nD; j++){
            aadX[i][j] = adX[i*nD + j];
        }
    }
    ptData->nD = nD;
    ptData->nN = nN;
    ptData->aadX = aadX;
    return;

 memoryError:
    fprintf(stderr, "Failed allocating memory in readInputData\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
}

void destroyData(t_Data *ptData)
{
    int nN = ptData->nN;
    int i = 0;

    for(i = 0; i < nN; i++){
        free(ptData->aadX[i]);
    }
    free(ptData->aadX);

    return;
}

void calcSampleVar(t_Data *ptData,double *adVar, double *adMu)
{
    double **aadX = ptData->aadX;
    int i = 0, n = 0;
    int nD = ptData->nD, nN = ptData->nN;
    /*sample means*/
    double dN = (double) nN;

    for(i = 0; i < nD; i++){
        adMu[i] = 0.0;
        adVar[i] = 0.0;
    }

    for(i = 0; i < nD; i++){
        for(n = 0; n < nN; n++){
            adMu[i] += aadX[n][i];
            adVar[i] += aadX[n][i]*aadX[n][i];
        }

        adMu[i] /= dN;

        adVar[i] = (adVar[i] - dN*adMu[i]*adMu[i])/(dN - 1.0);
    }

    return;
}

void setVBParams(t_VBParams *ptVBParams, t_Data *ptData)
{
    int i = 0, nD = ptData->nD;
    double adVar[nD], adMu[nD];

    ptVBParams->dBeta0 = DEF_BETA0;
    ptVBParams->dNu0 = (double) nD;
    ptVBParams->ptInvW0 = gsl_matrix_alloc(nD,nD);

    calcSampleVar(ptData,adVar, adMu);
    gsl_matrix_set_zero (ptVBParams->ptInvW0);

    for(i = 0; i < nD; i++){
        double dRD = adVar[i]*((double) nD);

        gsl_matrix_set(ptVBParams->ptInvW0,i,i,dRD);
    }

    ptVBParams->dLogWishartB = dLogWishartB(ptVBParams->ptInvW0, nD, ptVBParams->dNu0, TRUE);
}

void allocateCluster(t_Cluster *ptCluster, int nN, int nK, int nD, t_Data *ptData, long lSeed, int nMaxIter, double dEpsilon, char *szCOutFile)
{
    int i = 0, j = 0, k = 0;

    ptCluster->szCOutFile = szCOutFile;
    ptCluster->ptVBParams = NULL;
    ptCluster->lSeed = lSeed;
    ptCluster->nMaxIter = nMaxIter;
    ptCluster->dEpsilon = dEpsilon;
    ptCluster->ptData = ptData;

    ptCluster->nN = nN;
    ptCluster->nK = nK;
    ptCluster->nKSize = nK;
    ptCluster->nD = nD;

    ptCluster->dVBL = 0.0;

    ptCluster->anMaxZ = (int *) malloc(nN*sizeof(int)); /*destroyed*/
    if(!ptCluster->anMaxZ)
        goto memoryError;

    ptCluster->anW = (int *) malloc(nK*sizeof(int)); /*destroyed*/
    if(!ptCluster->anW)
        goto memoryError;

    for(i = 0; i < nN; i++){
        ptCluster->anMaxZ[i] = NOT_SET;
    }

    for(i = 0; i < nK; i++){
        ptCluster->anW[i] = 0;
    }

    ptCluster->aadZ = (double **) malloc(nN*sizeof(double *)); /*destroyed*/
    if(!ptCluster->aadZ)
        goto memoryError;

    for(i = 0; i < nN; i++){
        ptCluster->aadZ[i] = (double *) malloc(nK*sizeof(double)); /*destroyed*/
        if(!ptCluster->aadZ[i])
            goto memoryError;

        for(j = 0; j < nK; j++){
            ptCluster->aadZ[i][j] = 0.0;
        }
    }

    ptCluster->adLDet = (double *) malloc(nK*sizeof(double)); /*all*/
    ptCluster->adPi   = (double *) malloc(nK*sizeof(double));
    ptCluster->adBeta = (double *) malloc(nK*sizeof(double));
    ptCluster->adNu   = (double *) malloc(nK*sizeof(double)); /*destroyed*/

    if(!ptCluster->adLDet || !ptCluster->adPi)
        goto memoryError;

    if(!ptCluster->adBeta || !ptCluster->adNu)
        goto memoryError;

    for(k = 0; k < nK; k++){
        ptCluster->adLDet[k] = 0.0;
        ptCluster->adPi[k]   = 0.0;
        ptCluster->adBeta[k] = 0.0;
        ptCluster->adNu[k] = 0.0;
    }

    ptCluster->aadMu = (double **) malloc(nK*sizeof(double *));
    if(!ptCluster->aadMu)
        goto memoryError;

    ptCluster->aadM = (double **) malloc(nK*sizeof(double *));
    if(!ptCluster->aadM)
        goto memoryError;

    for(i = 0; i < nK; i++){
        ptCluster->aadM[i] = (double*) malloc (nD*sizeof(double));
        if(!ptCluster->aadM[i])
            goto memoryError;

        ptCluster->aadMu[i] = (double*) malloc (nD*sizeof(double));
        if(!ptCluster->aadMu[i])
            goto memoryError;
    }

    ptCluster->aptSigma = (gsl_matrix **) malloc(nK*sizeof(gsl_matrix *));
    if(!ptCluster->aptSigma)
        goto memoryError;

    for(i = 0; i < nK ; i++){
        ptCluster->aptSigma[i] = (gsl_matrix*) gsl_matrix_alloc (nD, nD);
    }

    ptCluster->aptCovar = (gsl_matrix **) malloc(nK*sizeof(gsl_matrix *));
    if(!ptCluster->aptCovar)
        goto memoryError;

    for(i = 0; i < nK ; i++){
        ptCluster->aptCovar[i] = (gsl_matrix*) gsl_matrix_alloc (nD, nD);
    }

    return;

    memoryError:
    fprintf(stderr, "Failed allocating memory in allocateCluster\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
}

void destroyCluster(t_Cluster* ptCluster)
{
  int i = 0, nN = ptCluster->nN, nKSize = ptCluster->nKSize;

  if(ptCluster->szCOutFile != NULL){
	  free(ptCluster->szCOutFile);
  }

  free(ptCluster->anMaxZ);

  free(ptCluster->anW);

  for(i = 0; i < nN; i++){
    free(ptCluster->aadZ[i]);
  }
  free(ptCluster->aadZ);

  free(ptCluster->adLDet);
  free(ptCluster->adPi);
  free(ptCluster->adBeta);
  free(ptCluster->adNu);

  for(i = 0; i < nKSize; i++){
    free(ptCluster->aadMu[i]);
    free(ptCluster->aadM[i]);
  }

  free(ptCluster->aadMu);
  free(ptCluster->aadM);

  for(i = 0; i < nKSize ; i++){
    gsl_matrix_free(ptCluster->aptSigma[i]);
    gsl_matrix_free(ptCluster->aptCovar[i]);
  }
  free(ptCluster->aptSigma);
  free(ptCluster->aptCovar);
  return;
}

void* fitEM_MP(void *pvCluster)
{
    t_Cluster          *ptCluster = (t_Cluster *) pvCluster;
    gsl_rng            *ptGSLRNG     = NULL;
    const gsl_rng_type *ptGSLRNGType = NULL;
    int i = 0, k = 0;
    /*initialise GSL RNG*/
    ptGSLRNGType = gsl_rng_default;
    ptGSLRNG     = gsl_rng_alloc(ptGSLRNGType);

    gsl_rng_set (ptGSLRNG, ptCluster->lSeed);

    if(ptCluster->bAssign == FALSE){
        initKMeans(ptGSLRNG, ptCluster, ptCluster->ptData);
    }
    else{
        for(i = 0; i < ptCluster->nN; i++){
            for(k = 0; k < ptCluster->nK; k++){
                ptCluster->aadZ[i][k] = 0.0;
            }
            ptCluster->aadZ[i][ptCluster->anMaxZ[i]] = 1.0;
        }
        performMStepMP(ptCluster, ptCluster->ptData);
    }

    gmmTrainVB_MP(ptCluster, ptCluster->ptData);

    gsl_rng_free(ptGSLRNG);

    return NULL;
}

void compressCluster(t_Cluster *ptCluster)
{
    int i = 0, k = 0, nNewK = 0, nN = ptCluster->nN;
    double **aadNewZ = NULL, dN = (double) nN;

    for(i = 0; i < ptCluster->nK; i++){
        if(ptCluster->adPi[i] > 0.0){
            nNewK++;
        }
    }

    aadNewZ = (double **) malloc(nN*sizeof(double *));
    if(!aadNewZ)
        goto memoryError;

    for(i = 0; i < nN; i++){
        aadNewZ[i] = (double *) malloc(nNewK*sizeof(double));
        if(!aadNewZ[i])
            goto memoryError;
    }

    for(i = 0; i < nN; i++){
        int nC = 0;
        for(k = 0; k < ptCluster->nK; k++){
            if(ptCluster->adPi[k] > 0.0){
	            aadNewZ[i][nC] = ptCluster->aadZ[i][k];
	            nC++;
            }
        }
    }

    for(i = 0; i < nN; i++){
        free(ptCluster->aadZ[i]);
    }
    free(ptCluster->aadZ);

    /*reset Z and K*/
    ptCluster->aadZ = aadNewZ;
    ptCluster->nK = nNewK;

    /*recalculate Pi*/
    for(k = 0; k < ptCluster->nK; k++){
        ptCluster->adPi[k] = 0.0;
        for(i = 0; i < nN; i++){
            ptCluster->adPi[k] += ptCluster->aadZ[i][k];
        }
        ptCluster->adPi[k] /= dN;
    }

    /*assign to best clusters*/
    for(i = 0; i < nN; i++){
        double dMaxZ = ptCluster->aadZ[i][0];
        int    nMaxK = 0;
        for(k = 1; k < ptCluster->nK; k++){
            if(ptCluster->aadZ[i][k] > dMaxZ){
	            nMaxK = k;
	            dMaxZ = ptCluster->aadZ[i][k];
            }
        }
        ptCluster->anMaxZ[i] = nMaxK;
    }

    for(k = 0; k < ptCluster->nK; k++){
        ptCluster->anW[k] = 0;
    }

    for(i = 0; i < nN; i++){
        ptCluster->anW[ptCluster->anMaxZ[i]]++;
    }

    return;

    memoryError:
    fprintf(stderr, "Failed allocating memory in main\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
}

double decomposeMatrix(gsl_matrix *ptSigmaMatrix, int nD)
{
  double dDet = 0.0;
  int status;
  int l = 0;

  status = gsl_linalg_cholesky_decomp(ptSigmaMatrix);

  if(status == GSL_EDOM){
    fprintf(stderr,"Failed Cholesky decomposition in decomposeMatrix\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
  }
  else{
    for(l = 0; l < nD; l++){
      double dT = gsl_matrix_get(ptSigmaMatrix,l,l);
      dDet += 2.0*log(dT);
    }
    gsl_linalg_cholesky_invert(ptSigmaMatrix);
    return dDet;
  }
}

double mstep(int k, double *adMu, double* adM, int nN, int nD, double** aadZ, double** aadX, double *pdBeta, double *pdNu, double *pdLDet, t_VBParams *ptVBParams, gsl_matrix *ptCovarK, gsl_matrix *ptSigmaMatrix)
{
    double dPi = 0.0, dBeta = 0.0, dLDet = 0.0, dNu = 0.0;
    int i = 0, j = 0, l = 0, m = 0;
    double      dF = 0.0;
    double **aadCovar = NULL;
    double **aadInvWK = NULL;

    aadCovar = (double **) malloc(nD*sizeof(double*));
    if(!aadCovar)
        goto memoryError;

    for(i = 0; i < nD; i++){
        aadCovar[i] = (double *) malloc(nD*sizeof(double));
        if(!aadCovar[i])
            goto memoryError;
    }

    aadInvWK = (double **) malloc(nD*sizeof(double*));
    if(!aadInvWK)
        goto memoryError;

    for(i = 0; i < nD; i++){
        aadInvWK[i] = (double *) malloc(nD*sizeof(double));
        if(!aadInvWK[i])
            goto memoryError;
    }

    /*recompute mixture weights and means*/
    for(j = 0; j < nD; j++){
        adMu[j] = 0.0;
        for(l = 0; l < nD; l++){
            aadCovar[j][l] = 0.0;
            aadInvWK[j][l] = 0.0;
        }
    }

    for(i = 0; i < nN; i++){
        if(aadZ[i][k] > MIN_Z){
            dPi += aadZ[i][k];
            for(j = 0; j < nD; j++){
                adMu[j] += aadZ[i][k]*aadX[i][j];
            }
        }
    }

    /*normalise means*/
    if(dPi > MIN_PI){
        /*Equation 10.60*/
        dBeta = ptVBParams->dBeta0 + dPi;

        for(j = 0; j < nD; j++){
            /*Equation 10.61*/
            adM[j] = adMu[j]/dBeta;
            adMu[j] /= dPi;
        }

        dNu = ptVBParams->dNu0 + dPi;

        /*calculate covariance matrices*/
        for(i = 0; i < nN; i++){
            if(aadZ[i][k] > MIN_Z){
                double adDiff[nD];

                for(j = 0; j < nD; j++){
                    adDiff[j] = aadX[i][j] - adMu[j];
                }

                for(l = 0; l < nD; l++){
                    for(m = 0; m <=l ; m++){
                        aadCovar[l][m] += aadZ[i][k]*adDiff[l]*adDiff[m];
                    }
                }
            }
        }

        for(l = 0; l < nD; l++){
            for(m = l + 1; m < nD; m++){
                aadCovar[l][m] = aadCovar[m][l];
            }
        }

        /*save sample covariances for later use*/
        for(l = 0; l < nD; l++){
            for(m = 0; m < nD; m++){
                double dC = aadCovar[l][m] / dPi;
                gsl_matrix_set(ptCovarK,l,m,dC);
            }
        }

        /*Now perform equation 10.62*/
        dF = (ptVBParams->dBeta0*dPi)/dBeta;
        for(l = 0; l < nD; l++){
            for(m = 0; m <= l; m++){
                aadInvWK[l][m] = gsl_matrix_get(ptVBParams->ptInvW0, l,m) + aadCovar[l][m] + dF*adMu[l]*adMu[m];
            }
        }

        for(l = 0; l < nD; l++){
            for(m = 0; m <= l ; m++){
                aadCovar[l][m] /= dPi;
                gsl_matrix_set(ptSigmaMatrix, l, m, aadInvWK[l][m]);
                gsl_matrix_set(ptSigmaMatrix, m, l, aadInvWK[l][m]);
            }
       }

    /*Implement Equation 10.65*/
      dLDet = ((double) nD)*log(2.0);

      for(l = 0; l < nD; l++){
        double dX = 0.5*(dNu - (double) l);
        dLDet += gsl_sf_psi (dX);
      }

      dLDet -= decomposeMatrix(ptSigmaMatrix,nD);
    }
    else{
      /*Equation 10.60*/
      dPi = 0.0;

      dBeta = ptVBParams->dBeta0;

      for(j = 0; j < nD; j++){
        /*Equation 10.61*/
        adM[j] = 0.0;
        adMu[j] = 0.0;
      }

      dNu = ptVBParams->dNu0;

      for(l = 0; l < nD; l++){
        for(m = 0; m <= l; m++){
          aadInvWK[l][m] = gsl_matrix_get(ptVBParams->ptInvW0, l,m);
        }
      }

      for(l = 0; l < nD; l++){
        for(m = 0; m <= l ; m++){
            aadInvWK[l][m] = gsl_matrix_get(ptVBParams->ptInvW0, l,m);
        }
      }

      for(l = 0; l < nD; l++){
        for(m = 0; m <= l ; m++){
            gsl_matrix_set(ptSigmaMatrix, l, m, aadInvWK[l][m]);
            gsl_matrix_set(ptSigmaMatrix, m, l, aadInvWK[l][m]);
        }
      }

      /*Implement Equation 10.65*/
      dLDet = ((double) nD)*log(2.0);

      for(l = 0; l < nD; l++){
        double dX = 0.5*(dNu - (double) l);
        dLDet += gsl_sf_psi (dX);
      }

      dLDet -= decomposeMatrix(ptSigmaMatrix,nD);
    }

    /*free up memory*/
    for(i = 0; i < nD; i++){
        free(aadCovar[i]);
        free(aadInvWK[i]);
    }

    free(aadCovar);
    free(aadInvWK);

    (*pdBeta) = dBeta;
    (*pdNu)   = dNu;
    (*pdLDet) = dLDet;
    return dPi;

    memoryError:
        fprintf(stderr, "Failed allocating memory in mstep\n");
        fflush(stderr);
        exit(EXIT_FAILURE);
}

void performMStepMP(t_Cluster *ptCluster, t_Data *ptData){
    int k = 0;
    int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
    double **aadZ = ptCluster->aadZ,**aadX = ptData->aadX;
    double **aadCovar = NULL, **aadInvWK = NULL;
    t_VBParams *ptVBParams = ptCluster->ptVBParams;

    /*perform M step*/
#pragma omp parallel for
    for(int k = 0; k < nK; k++){ /*loop components*/
        double dPi = 0.0, dBeta = 0.0, dNu = 0.0, dLDet = 0.0;

        dPi = mstep(k, ptCluster->aadMu[k],ptCluster->aadM[k], nN, nD, aadZ, aadX, &dBeta, &dNu, &dLDet, ptVBParams, ptCluster->aptCovar[k], ptCluster->aptSigma[k]);

        ptCluster->adPi[k] = dPi;
        ptCluster->adBeta[k] = dBeta;
        ptCluster->adNu[k] = dNu;
        ptCluster->adLDet[k] = dLDet;
    }

    /*Normalise pi*/

    if(1){
        double dNP = 0.0;

        for(k = 0; k < nK; k++){
            dNP += ptCluster->adPi[k];
        }

        for(k = 0; k < nK; k++){
            ptCluster->adPi[k] /= dNP;
        }
    }

    return;

    memoryError:
        fprintf(stderr, "Failed allocating memory in performMStep\n");
        fflush(stderr);
        exit(EXIT_FAILURE);
}

void initKMeans(gsl_rng *ptGSLRNG, t_Cluster *ptCluster, t_Data *ptData)
{
    /*very simple initialisation assign each data point to random cluster*/
    int i = 0, k = 0, nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
    double **aadMu = ptCluster->aadMu, **aadX = ptData->aadX;
    int *anMaxZ = ptCluster->anMaxZ, *anW = ptCluster->anW, nChange = nN;
    int nIter = 0, nMaxIter = ptCluster->nMaxIter;
    for(i = 0; i < nN; i++){
        int nIK = gsl_rng_uniform_int (ptGSLRNG, nK);
        ptCluster->anMaxZ[i] = nIK;
        anW[nIK]++;
    }

    updateMeans(ptCluster, ptData);

    while(nChange > 0 && nIter < nMaxIter){
        nChange = 0;
        /*reassign vectors*/
        for(i = 0; i < nN; i++){
        double dMinDist = DBL_MAX;
        int    nMinK = NOT_SET;

        for(k = 0; k < nK; k++){
	        double dDist = calcDist(aadX[i],aadMu[k],nD);

	        if(dDist < dMinDist){
	            nMinK = k;
	            dMinDist = dDist;
	        }
      }

      if(nMinK != anMaxZ[i]){
        int nCurr = anMaxZ[i];
	    nChange++;
	    anW[nCurr]--;
	    anW[nMinK]++;
	    anMaxZ[i] = nMinK;

	    /*check for empty clusters*/
	    if(anW[nCurr] == 0){
	        int nRandI =  gsl_rng_uniform_int (ptGSLRNG, nN);
	        int nKI = 0;
	        /*select at random from non empty clusters*/

	        while(anW[anMaxZ[nRandI]] == 1){
	            nRandI =  gsl_rng_uniform_int (ptGSLRNG, nN);
	        }

	        nKI = anMaxZ[nRandI];
	        anW[nKI]--;
	        anW[nCurr] = 1;
	        anMaxZ[nRandI] = nCurr;
	    }
    }
    }
    //printf("%d %d\n",nIter,nChange);
    nIter++;
    updateMeans(ptCluster, ptData);
  }

  for(i = 0; i < nN; i++){
    for(k = 0; k < nK; k++){
      ptCluster->aadZ[i][k] = 0.0;
    }
    ptCluster->aadZ[i][anMaxZ[i]] = 1.0;
  }

  performMStepMP(ptCluster, ptData);
  return;
}

double eqnA(int nD, gsl_matrix *ptCovarK, gsl_matrix *ptSigmaK, double *adMuK, double *adMK, double dLDetK, double dNuK, double logd2Pi, double dBetaK,double dNK)
{
    int l = 0;
    gsl_matrix *ptRes  = gsl_matrix_alloc(nD,nD);
    gsl_vector *ptDiff = gsl_vector_alloc(nD);
    double dT1 = 0.0, dT2 = 0.0, dF = 0.0;
    gsl_vector *ptR = gsl_vector_alloc(nD);
    double dD = (double) nD;
    double dRet = 0.0;

    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0,ptCovarK,ptSigmaK,0.0,ptRes);

    for(l = 0; l < nD; l++){
	    dT1 += gsl_matrix_get(ptRes,l,l);
    }

    for(l = 0; l < nD; l++){
        gsl_vector_set(ptDiff,l,adMuK[l] - adMK[l]);
    }

    gsl_blas_dsymv (CblasLower, 1.0, ptSigmaK, ptDiff, 0.0, ptR);

    gsl_blas_ddot (ptDiff, ptR, &dT2);

    dF = dLDetK - dNuK*(dT1 + dT2) - dD*(logd2Pi + (1.0/dBetaK));

    dRet = 0.5*dNK*dF;

    gsl_matrix_free(ptRes);
    gsl_vector_free(ptDiff);
    gsl_vector_free(ptR);

    return dRet;
}

double eqnB(int nD, gsl_matrix *ptInvW0, gsl_matrix *ptSigmaK, double* adMK, double dBeta0, double d2Pi, double dLDetK, double dBetaK, double dNuK, double dNu0)
{
    int l = 0;
    double dD = (double) nD;
    gsl_matrix *ptRes  = gsl_matrix_alloc(nD,nD);
    gsl_vector *ptDiff = gsl_vector_alloc(nD);
    gsl_vector *ptR = gsl_vector_alloc(nD);
    double dT1 = 0.0, dT2 = 0.0, dF = 0.0;
    double dRet = 0.0;

    gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0,ptInvW0,ptSigmaK,0.0,ptRes);

    for(l = 0; l < nD; l++){
        dT1 += gsl_matrix_get(ptRes,l,l);
    }

    for(l = 0; l < nD; l++){
	    gsl_vector_set(ptDiff,l,adMK[l]);
    }

    gsl_blas_dsymv (CblasLower, 1.0, ptSigmaK, ptDiff, 0.0, ptR);

    gsl_blas_ddot (ptDiff, ptR, &dT2);

    dF = dD*log(dBeta0/d2Pi) + dLDetK - ((dD*dBeta0)/dBetaK) - dBeta0*dNuK*dT2 - dNuK*dT1;

    dRet = 0.5*(dF + (dNu0 - dD - 1.0)*dLDetK);

    gsl_matrix_free(ptRes);
    gsl_vector_free(ptDiff);
    gsl_vector_free(ptR);

    return dRet;
}

double calcVBL_MP(t_Cluster* ptCluster)
{
    int nN = ptCluster->nN;
    int nK = ptCluster->nK, nD = ptCluster->nD;
    double dBishop1 = 0.0, dBishop2 = 0.0, dBishop3 = 0.0, dBishop4 = 0.0, dBishop5 = 0.0; /*Bishop equations 10.71...*/
    double dD = (double) nD;
    double** aadMu = ptCluster->aadMu, **aadM = ptCluster->aadM, **aadZ = ptCluster->aadZ;
    double* adBeta = ptCluster->adBeta, *adNu = ptCluster->adNu, *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi;
    double adNK[nK], adRet[nK];
    double d2Pi = 2.0*M_PI, logd2Pi = log(d2Pi), dBeta0 = ptCluster->ptVBParams->dBeta0, dNu0 = ptCluster->ptVBParams->dNu0, dRet = 0.0;
    double dK = 0.0;

    for(int k = 0; k < nK; k++){
        adNK[k] = 0.0;
    }

    /*Equation 10.72*/
    for(int i = 0; i < nN; i++){
        for(int k = 0; k < nK; k++){
            adNK[k] += aadZ[i][k];
            if(adPi[k] > 0.0){
	            dBishop2 += aadZ[i][k]*log(adPi[k]);
            }
        }
    }

    for(int k = 0; k < nK; k++){
        if(adNK[k] > 0.0){
            dK++;
        }
    }

    /*Equation 10.71*/
#pragma omp parallel for
    for(int k = 0; k < nK; k++){
        if(adNK[k] > 0.0){
            adRet[k] = eqnA(nD, ptCluster->aptCovar[k], ptCluster->aptSigma[k], aadMu[k],         aadM[k], adLDet[k], adNu[k], logd2Pi, adBeta[k],adNK[k]);
        }
        else{
            adRet[k] = 0.0;
        }
    }

    for(int k = 0; k < nK; k++){
        dBishop1 += adRet[k];
    }

    /*Equation 10.74*/
#pragma omp parallel for
    for(int k = 0; k < nK; k++){
        if(adNK[k] > 0.0){
            adRet[k] = eqnB(nD, ptCluster->ptVBParams->ptInvW0, ptCluster->aptSigma[k], aadM[k], ptCluster->ptVBParams->dBeta0, d2Pi, adLDet[k], adBeta[k], adNu[k],ptCluster->ptVBParams->dNu0);
        }
    }

    for(int k = 0; k < nK; k++){
        dBishop3 += adRet[k];
    }

    dBishop3 += dK*ptCluster->ptVBParams->dLogWishartB;

    /*Equation 10.75*/
    for(int i = 0; i < nN; i++){
        for(int k = 0; k < nK; k++){
            if(aadZ[i][k] > 0.0){
	            dBishop4 += aadZ[i][k]*log(aadZ[i][k]);
            }
        }
    }

    /*Equation 10.77*/
    for(int k = 0; k < nK; k++){
        if(adNK[k] > 0.0){
            dBishop5 += 0.5*adLDet[k] + 0.5*dD*log(adBeta[k]/d2Pi) - 0.5*dD - dWishartExpectLogDet(ptCluster->aptSigma[k], adNu[k], nD);
        }
    }

    dRet = dBishop1 + dBishop2 + dBishop3 - dBishop4 - dBishop5;

    return dRet;
}

void calcZ_MP(t_Cluster* ptCluster, t_Data *ptData){
    double **aadX = ptData->aadX, **aadZ = ptCluster->aadZ;
    int nK = ptCluster->nK, nD = ptCluster->nD, nN = ptData->nN;
    double dD = (double) nD;
    double** aadM = ptCluster->aadM, *adPi = ptCluster->adPi;

#pragma omp parallel for
    for(int i = 0; i < nN; i++){
        double dMinDist = DBL_MAX;
        double dTotalZ  = 0.0;
        double dNTotalZ = 0.0;
        double adDist[nK];
        int k = 0, l = 0;
        gsl_vector *ptDiff = gsl_vector_alloc(nD);
        gsl_vector *ptRes = gsl_vector_alloc(nD);

        for(k = 0; k < nK; k++){
            if(adPi[k] > 0.){
                /*set vector to data point*/
                for(l = 0; l < nD; l++){
                    gsl_vector_set(ptDiff,l,aadX[i][l] - aadM[k][l]);
                }

                gsl_blas_dsymv (CblasLower, 1.0, ptCluster->aptSigma[k], ptDiff, 0.0, ptRes);

                gsl_blas_ddot (ptDiff, ptRes, &adDist[k]);

                adDist[k] *= ptCluster->adNu[k];

                adDist[k] -= ptCluster->adLDet[k];

                adDist[k] += dD/ptCluster->adBeta[k];

                if(adDist[k] < dMinDist){
                    dMinDist = adDist[k];
                }
            }
        }

        for(k = 0; k < nK; k++){
            if(adPi[k] > 0.){
                aadZ[i][k] = adPi[k]*exp(-0.5*(adDist[k]-dMinDist));
                dTotalZ += aadZ[i][k];
            }
            else{
                aadZ[i][k] = 0.0;
            }
        }

        for(k = 0; k < nK; k++){
            double dF = aadZ[i][k] / dTotalZ;
            if(dF < MIN_Z){
                aadZ[i][k] = 0.0;
            }
            dNTotalZ += aadZ[i][k];
        }
        if(dNTotalZ > 0.){
            for(k = 0; k < nK; k++){
                aadZ[i][k] /= dNTotalZ;
            }
        }
        gsl_vector_free(ptRes);
        gsl_vector_free(ptDiff);
    }

    return;
}

void gmmTrainVB_MP(t_Cluster *ptCluster, t_Data *ptData)
{
    int i = 0, k = 0,nIter = 0;
    int nN = ptData->nN, nK = ptCluster->nK;
    /*change in log-likelihood*/
    double dLastVBL = 0.0, dDelta = DBL_MAX;
    double   **aadZ = ptCluster->aadZ;
    int    nMaxIter = ptCluster->nMaxIter;
    double dEpsilon = ptCluster->dEpsilon;
    FILE   *ofp = NULL;

    if(ptCluster->szCOutFile){
	    ofp = fopen(ptCluster->szCOutFile,"w");
	    if(!ofp){
		    fprintf(stderr, "Failed to open file %s in gmmTrainVB\n",ptCluster->szCOutFile);
		    fflush(stderr);
	    }
    }

    /*calculate data likelihood*/
    calcZ_MP(ptCluster,ptData);
    ptCluster->dVBL = calcVBL_MP(ptCluster);

    while(nIter < nMaxIter && dDelta > dEpsilon){

        /*update parameter estimates*/
        performMStepMP(ptCluster, ptData);

        /*calculate responsibilities*/
        calcZ_MP(ptCluster,ptData);

        dLastVBL = ptCluster->dVBL;
        ptCluster->dVBL = calcVBL_MP(ptCluster);
        dDelta = fabs(ptCluster->dVBL - dLastVBL);

        fprintf(stderr,"%d,%f,%f\n",nIter, ptCluster->dVBL, dDelta);
        fflush(stderr);
        if(ofp){
    	    fprintf(ofp,"%d,%f,%f,",nIter, ptCluster->dVBL, dDelta);
    	    for(k = 0; k < nK-1; k++){
    		    fprintf(ofp,"%f,",ptCluster->adPi[k]);
    	    }
    	    fprintf(ofp,"%f\n",ptCluster->adPi[nK - 1]);
	        fflush(ofp);
        }
        nIter++;
    }

    if(ofp){
	    fclose(ofp);
    }

    /*assign to best clusters*/
    for(i = 0; i < nN; i++){
        double dMaxZ = aadZ[i][0];
        int    nMaxK = 0;
        for(k = 1; k < nK; k++){
            if(aadZ[i][k] > dMaxZ){
	            nMaxK = k;
	            dMaxZ = aadZ[i][k];
            }
        }
        ptCluster->anMaxZ[i] = nMaxK;
    }

    return;
}

void calcCovarMatrices(t_Cluster *ptCluster, t_Data *ptData)
{
    int i = 0, j = 0, k = 0, l = 0, m = 0;
    int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
    double **aadZ = ptCluster->aadZ,**aadX = ptData->aadX;
    double *adPi = ptCluster->adPi, **aadCovar = NULL;
    double dN = (double) nN;

    aadCovar = (double **) malloc(nD*sizeof(double*));
    if(!aadCovar)
        goto memoryError;

    for(i = 0; i < nD; i++){
        aadCovar[i] = (double *) malloc(nD*sizeof(double));
        if(!aadCovar[i])
            goto memoryError;
    }

    for(k = 0; k < nK; k++){ /*loop components*/
        double*     adMu          = ptCluster->aadMu[k];
        gsl_matrix  *ptSigmaMatrix = ptCluster->aptSigma[k];
        /*recompute mixture weights and means*/
        for(j = 0; j < nD; j++){
            adMu[j] = 0.0;
            for(l = 0; l < nD; l++){
	            aadCovar[j][l] = 0.0;
            }
            /*prevents singularities*/
        aadCovar[j][j] = MIN_COVAR;
    }

    /* compute weight associated with component k*/
    adPi[k] = 0.0;
    for(i = 0; i < nN; i++){
        adPi[k] += aadZ[i][k];
        for(j = 0; j < nD; j++){
	        adMu[j] += aadZ[i][k]*aadX[i][j];
        }
    }
    /*normalise means*/
    for(j = 0; j < nD; j++){
        adMu[j] /= adPi[k];
    }

    /*calculate covariance matrices*/
    for(i = 0; i < nN; i++){
        double adDiff[nD];

        for(j = 0; j < nD; j++){
	        adDiff[j] = aadX[i][j] - adMu[j];
        }

        for(l = 0; l < nD; l++){
	        for(m = 0; m <=l ; m++){
	            aadCovar[l][m] += aadZ[i][k]*adDiff[l]*adDiff[m];
	        }
        }
    }

    for(l = 0; l < nD; l++){
        for(m = l + 1; m < nD; m++){
	        aadCovar[l][m] = aadCovar[m][l];
        }
    }

    for(l = 0; l < nD; l++){
        for(m = 0; m < nD; m++){
	        aadCovar[l][m] /= adPi[k];
	        gsl_matrix_set(ptSigmaMatrix, l, m, aadCovar[l][m]);
        }
    }

    adPi[k] /= dN; /*normalise weights*/
  }
  /*free up memory*/
  for(i = 0; i < nD; i++){
    free(aadCovar[i]);
  }

  //gsl_matrix_free(ptSigmaMatrix);
  free(aadCovar);

  return;
 memoryError:
  fprintf(stderr, "Failed allocating memory in performMStep\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

/*note assuming you are using inverse W matrix*/
double dLogWishartB(gsl_matrix *ptInvW, int nD, double dNu, int bInv)
{
  int i = 0;
  double dRet = 0.0, dT = 0.0;
  double dLogDet = 0.0, dD = (double) nD;
  gsl_matrix* ptTemp = gsl_matrix_alloc(nD,nD);

  gsl_matrix_memcpy(ptTemp, ptInvW);

  dLogDet = decomposeMatrix(ptTemp, nD);

  if(bInv == TRUE){
    dRet = 0.5*dNu*dLogDet;
  }
  else{
    dRet = -0.5*dNu*dLogDet;
  }

  dT = 0.5*dNu*dD*log(2.0);

  dT += 0.25*dD*(dD - 1.0)*log(M_PI);

  for(i = 0; i < nD; i++){
    dT += gsl_sf_lngamma(0.5*(dNu - (double) i));
  }

  gsl_matrix_free(ptTemp);

  return dRet - dT;
}

double dWishartExpectLogDet(gsl_matrix *ptW, double dNu, int nD)
{
  int i = 0;
  double dRet = 0.0, dLogDet = 0.0, dD = (double) nD;
  gsl_matrix* ptTemp = gsl_matrix_alloc(nD,nD);

  gsl_matrix_memcpy(ptTemp, ptW);

  dLogDet = decomposeMatrix(ptW, nD);

  dRet = dD*log(2.0) + dLogDet;

  for(i = 0; i < nD; i++){
    dRet += gsl_sf_psi(0.5*(dNu - (double) i));
  }

  gsl_matrix_free(ptTemp);

  return dRet;
}

void updateMeans(t_Cluster *ptCluster, t_Data *ptData)
{
  int i = 0, j = 0, k = 0;
  int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
  int *anMaxZ = ptCluster->anMaxZ;
  int *anW    = ptCluster->anW;
  double **aadX = ptData->aadX, **aadMu = ptCluster->aadMu;

  for(k = 0; k < nK; k++){

    for(j = 0; j < nD; j++){
      aadMu[k][j] = 0.0;
    }
  }

  for(i = 0; i < nN; i++){
    int nZ = anMaxZ[i];

    for(j = 0; j < nD; j++){
      aadMu[nZ][j] += aadX[i][j];
    }
  }

  for(k = 0; k < nK; k++){ /*loop components*/

    /*normalise means*/
    if(anW[k] > 0){
      for(j = 0; j < nD; j++){
	    aadMu[k][j] /= (double) anW[k];
      }
    }
    else{
      for(j = 0; j < nD; j++){
	    aadMu[k][j] = 0.0;
      }
    }
  }

  return;
}

double calcDist(double* adX, double *adMu, int nD)
{
  double dDist = 0.0;
  int i = 0;

  for(i = 0; i < nD; i++){
    double dV = adX[i] - adMu[i];
    dDist += dV*dV;
  }

  return sqrt(dDist);
}

max_stars_repo_name: merenlab/CONCOCT

max_issues_repo_name: merenlab/CONCOCT

max_forks_repo_name: merenlab/CONCOCT