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

#include <gsl/gsl_integration.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

#include "ccl.h"

/*----- ROUTINE: dc_NakamuraSuto -----
INPUT: cosmology, scale factor
TASK: Computes the peak threshold: delta_c(z) assuming LCDM.
Cosmology dependence of the critical linear density according to the spherical-collapse model.
Fitting function from Nakamura & Suto (1997; arXiv:astro-ph/9710107).
*/
double dc_NakamuraSuto(ccl_cosmology *cosmo, double a, int *status){

  double Om_mz = ccl_omega_x(cosmo, a, ccl_species_m_label, status);
  double dc0 = (3./20.)*pow(12.*M_PI,2./3.);
  double dc = dc0*(1.+0.012299*log10(Om_mz));

  return dc;

}

/*----- ROUTINE: Dv_BryanNorman -----
INPUT: cosmology, scale factor
TASK: Computes the virial collapse density contrast with respect to the matter density assuming LCDM.
Cosmology dependence of the virial collapse density according to the spherical-collapse model
Fitting function from Bryan & Norman (1998; arXiv:astro-ph/9710107)
*/
double Dv_BryanNorman(ccl_cosmology *cosmo, double a, int *status){

  double Om_mz = ccl_omega_x(cosmo, a, ccl_species_m_label, status);
  double x = Om_mz-1.;
  double Dv0 = 18.*pow(M_PI,2);
  double Dv = (Dv0+82.*x-39.*pow(x,2))/Om_mz;

  return Dv;
}

static double sigmaM_m2r(ccl_cosmology *cosmo, double halomass, int *status)
{
  double rho_m, smooth_radius;

  // Comoving matter density
  rho_m = ccl_rho_x(cosmo, 1., ccl_species_m_label, 1, status);

  smooth_radius = pow((3.0*halomass) / (4*M_PI*rho_m), (1.0/3.0));

  return smooth_radius;
}

void ccl_cosmology_compute_sigma(ccl_cosmology *cosmo, ccl_f2d_t *psp, int *status)
{
  if(cosmo->computed_sigma)
    return;

  int na = cosmo->spline_params.A_SPLINE_NA_SM + cosmo->spline_params.A_SPLINE_NLOG_SM - 1;
  int nm = cosmo->spline_params.LOGM_SPLINE_NM;
  double *m = NULL;
  double *y = NULL;
  double *aa = NULL;

  // create linearly-spaced values of log-mass.
  m = ccl_linear_spacing(cosmo->spline_params.LOGM_SPLINE_MIN,
                         cosmo->spline_params.LOGM_SPLINE_MAX, nm);
  if (m == NULL ||
      (fabs(m[0]-cosmo->spline_params.LOGM_SPLINE_MIN)>1e-5) ||
      (fabs(m[nm-1]-cosmo->spline_params.LOGM_SPLINE_MAX)>1e-5) ||
      (m[nm-1]>10E17)) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
                                     "Error creating linear spacing in m\n");
  }

  // create scale factor array
  if (*status == 0) {
    aa = ccl_linlog_spacing(cosmo->spline_params.A_SPLINE_MINLOG_SM,
                            cosmo->spline_params.A_SPLINE_MIN_SM,
                            cosmo->spline_params.A_SPLINE_MAX,
                            cosmo->spline_params.A_SPLINE_NLOG_SM,
                            cosmo->spline_params.A_SPLINE_NA_SM);
    if (aa == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo,
                                       "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
                                       "Error creating scale factor array\n");
    }
  }

  // create space for y, to be filled with sigma
  if (*status == 0) {
    y = malloc(sizeof(double)*nm*na);
    if (y == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo,
                                       "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
                                       "memory allocation\n");
    }
  }

  // fill in sigma, if no errors have been triggered at this time.
  if (*status == 0) {
    #pragma omp parallel shared(na, aa, nm, m, y, status, cosmo, psp) \
                         default(none)
    {
      int i, j;
      double a_sf, smooth_radius;
      int local_status = *status;

      #pragma omp for
      for (j=0; j<na; j++) {
        a_sf = aa[j];
        for (i=0; i<nm; i++) {
          smooth_radius = sigmaM_m2r(cosmo, pow(10,m[i]), &local_status);
          y[j*nm + i] = log(ccl_sigmaR(cosmo, smooth_radius, a_sf,
                                       psp, &local_status));
        }
      } //end omp for
      if (local_status) {
        #pragma omp atomic write
        *status = local_status;
      }
    } //end omp parallel
  }

  gsl_spline2d *lsM = NULL;
  if (*status == 0) {
    lsM = gsl_spline2d_alloc(gsl_interp2d_bicubic, nm, na);
    if (lsM == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo,
                                       "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
                                       "error allocating 2D spline\n");
    }
  }

  if(*status == 0) {
    int s2dstatus=gsl_spline2d_init(lsM, m, aa, y, nm, na);
    if (s2dstatus) {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(cosmo,
                                       "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
                                       "error initializing spline\n");
    }
  }

  if (*status == 0) {
    cosmo->computed_sigma = true;
    cosmo->data.logsigma = lsM;
  }
  else
    gsl_spline2d_free(lsM);

  free(aa);
  free(m);
  free(y);
}

/*----- ROUTINE: ccl_sigma_M -----
INPUT: ccl_cosmology * cosmo, double halo mass in units of Msun, double scale factor
TASK: returns sigma from the sigmaM interpolation. Also computes the sigma interpolation if
necessary.
*/
double ccl_sigmaM(ccl_cosmology *cosmo, double log_halomass, double a, int *status)
{
  // Check if sigma has already been calculated
  if (!cosmo->computed_sigma) {
    *status = CCL_ERROR_SIGMA_INIT;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_massfunc.c: ccl_sigmaM(): "
                                     "sigma(M) spline has not been computed!");
    return NAN;
  }

  double lgsigmaM;
  int gslstatus = gsl_spline2d_eval_e(cosmo->data.logsigma, log_halomass,
                                      a, NULL, NULL, &lgsigmaM);

  if(gslstatus != GSL_SUCCESS) {
    ccl_raise_gsl_warning(gslstatus, "ccl_massfunc.c: ccl_sigmaM():");
    *status |= gslstatus;
  }

  return exp(lgsigmaM);
}

/*----- ROUTINE: ccl_dlnsigM_dlogM -----
INPUT: ccl_cosmology *cosmo, double halo mass in units of Msun
TASK: returns the value of the derivative of ln(sigma^-1) with respect to log10 in halo mass.
*/
double ccl_dlnsigM_dlogM(ccl_cosmology *cosmo, double log_halomass, double a, int *status)
{
  // Check if sigma has already been calculated
  if (!cosmo->computed_sigma) {
    *status = CCL_ERROR_SIGMA_INIT;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_massfunc.c: ccl_dlnsigM_dlogM(): "
                                     "sigma(M) spline has not been computed!");
    return NAN;
  }

  double dlsdlgm;
  int gslstatus = gsl_spline2d_eval_deriv_x_e(cosmo->data.logsigma,
                                              log_halomass, a,
                                              NULL, NULL, &dlsdlgm);
  if(gslstatus) {
    ccl_raise_gsl_warning(gslstatus, "ccl_massfunc.c: ccl_dlnsigM_dlogM():");
    *status |= gslstatus;
  }
  return -dlsdlgm;
}


/**
 * \brief Function Definition Library to implement Sensor Fusion Algorithm
 * \author jaspalsingh@cmail.carleton.ca
 * \version 1.0
 * \date 2019-12-01
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include "sensor_fusion.h"

/**
 * \brief Structure pointer for computed support degree matrix
 * \details Structure pointer for calculating support degree matrix
 * \param[in] sensor_readings array of sensor readings
 * \return Success: pointer to the structure support_degree_matrix
 * \return Failure: NULL
 */
struct support_degree_matrix *compute_support_degree_matrix(double *sensor_readings) {
    struct support_degree_matrix *spd;
    spd = (struct support_degree_matrix *)malloc(sizeof(struct support_degree_matrix));
    // count the number of sensor readings
    int length = sizeof(sensor_readings) / sizeof(sensor_readings[0]);
    spd->sensor_count = length;
    double **arrptr = (double **)malloc(length * sizeof(double *));
    int count = 0;
    for (int i = 0; i < length; i++) {
        arrptr[i] = (double *)malloc(length * sizeof(double));
    }
    // allocate memory space for support degree matrix structure
    spd->sd_matrix = (double *)malloc(sizeof(double) * length * length);
    if (arrptr == NULL || spd == NULL || spd->sd_matrix == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    // compute support degree matrix
    for (int i = 0; i < length; i++) {
        for (int j = 0; j < length; j++) {
            arrptr[i][j] = exp(-1 * fabs(sensor_readings[i] - sensor_readings[j]));
            spd->sd_matrix[count] = arrptr[i][j];
            count++;
        }
    }
    printf("INFO : Computed support degree matrix\n");
    for (int i = 0; i < length * length; i++) {
        printf("DEBUG: Value => %f\n", spd->sd_matrix[i]);
    }
    free(arrptr);
    return spd;
}

/**
 * \brief Structure pointer calculate eigen value and eigen vector
 * \details Structure pointer for calculating eigen value and eigen vector
 * \param[in] support_degree_matrix pointer to structure support degree matrix
 * \return Success: pointer to structure eigen_value_vector
 * \return Failure: NULL
 */
struct eigen_value_vector *compute_eigen(struct support_degree_matrix *spd) {
    if ((spd->sd_matrix == NULL) || (spd->sensor_count == 0)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return NULL;
    }
    int length = spd->sensor_count;
    struct eigen_value_vector *eigen;
    // allocation memory space for eigen value vector structure
    eigen = (struct eigen_value_vector *)malloc(sizeof(struct eigen_value_vector));
    eigen->eigen_value = (double *)malloc(sizeof(double) * length);
    eigen->eigen_vector = (double **)malloc(length * sizeof(double *));
    for (int i = 0; i < length; i++) {
        eigen->eigen_vector[i] = (double *)malloc(length * sizeof(double));
    }
    if (eigen == NULL || eigen->eigen_vector == NULL || eigen->eigen_value == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    //Refer: https://www.gnu.org/software/gsl/doc/html/eigen.html
    // compute eigen values
    gsl_matrix_view m = gsl_matrix_view_array(spd->sd_matrix, length, length);
    gsl_vector *eval = gsl_vector_alloc(length);
    gsl_matrix *evec = gsl_matrix_alloc(length, length);
    gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(length);
    gsl_eigen_symmv(&m.matrix, eval, evec, w);
    gsl_eigen_symmv_free(w);
    gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_DESC);
    {
        int i;
        for (i = 0; i < length; i++) {
            eigen->eigen_value[i] = gsl_vector_get(eval, i);
            gsl_vector_view evec_i = gsl_matrix_column(evec, i);
            for (int j = 0; j < length; j++) {
                eigen->eigen_vector[i][j] = *(*(&evec_i.vector.data) + j * length);
            }
        }
    }
    for (int i = 0; i < length; i++) {
        printf("DEBUG: Eigen Value => %g\n", eigen->eigen_value[i]);
        printf("DEBUG: Eigen Vector => \n");
        for (int j = 0; j < length; j++) {
            printf("%g\n", eigen->eigen_vector[i][j]);
        }
    }
    gsl_vector_free(eval);
    gsl_matrix_free(evec);
    return eigen;
}

/**
 * \brief Function to calculate the contribution rate of principal component
 * \details Compute the contribution rate of kth and mth principal components
 * \param[in] eigen_value_vector pointer to structure of eigen_value_vector
 * \param[in] sensor_count total count of sensors
 * \return Success: pointer to contribution_rate of type double
 * \return Failure: NULL
 */
double *compute_contribution_rate(struct eigen_value_vector *eigen,
                                  int sensor_count) {
    if ((sensor_count == 0) || (eigen->eigen_value == NULL) || (eigen->eigen_vector == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return NULL;
    }
    double *contribution_rate = (double *)malloc(sensor_count * sizeof(double));
    if (contribution_rate == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    double sum = 0;
    for (int i = 0; i < sensor_count; i++) {
        sum += eigen->eigen_value[i];
    }
    printf("INFO : Compute contribution rate\n");
    for (int j = 0; j < sensor_count; j++) {
        contribution_rate[j] = eigen->eigen_value[j] / sum;
        printf("DEBUG: Rate => %g\n", contribution_rate[j]);
    }
    return contribution_rate;
}

/**
 * \brief Function to compute contribution rate
 * \details Compute the count of contribution rates to be used for sensors
 * \param[in] contribution_rate
 * \param[in] threshold
 * \param[in] sensor_count number of sensors to be used
 * \return Success: contribution rate count,
 * \return Failure: -1
 */
int compute_contrib_rate_count(double *contribution_rate,
                               float threshold,
                               int sensor_count) {
    if ((contribution_rate == NULL) || (threshold == 0) || (sensor_count == 0)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return -1;
    }
    double sum = 0;
    int m;
    for (int k = 0; k < sensor_count; k++) {
        for (int j = 0; j <= k; j++) {
            sum += contribution_rate[j];
        }
        if (sum <= threshold) {
            m = k - 1;
            printf("DEBUG: Contribution rate to use %d\n", m);
            return m;
        }
    }
    printf("DEBUG: Count of Contribution rates => %d\n", sensor_count);
    return sensor_count;
}

/**
 * \brief Function to compute principal component
 * \details Computed principal component for each of eigen vector
 * \param[in] support_degree_matrix pointer to structure support degree matrix
 * \param[in] eigen_vector pointer to pointer of eigen vector of type double
 * \param[in] contrib_rate_count number of contribution rate to be used
 * \return Success: principal of support degree matrix,
 * \return Failure: NULL
 */
double **compute_principal_component(struct support_degree_matrix *spd,
                                     double **eigen_vector,
                                     int contrib_rate_count) {
    if ((spd->sensor_count == 0) || (contrib_rate_count == 0) || (spd->sd_matrix == NULL) || (eigen_vector == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return NULL;
    }
    int n = spd->sensor_count;
    int m = contrib_rate_count;
    int count = 0;
    double **arrptr = (double **)malloc(n * sizeof(double *));
    for (int i = 0; i < n; i++) {
        arrptr[i] = (double *)malloc(n * sizeof(double));
    }
    if (arrptr == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    printf("INFO : Compute support degree matrix\n");
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            arrptr[i][j] = spd->sd_matrix[count];
            printf("%f\n", arrptr[i][j]);
            count++;
        }
    }
    double **principal_components_matrix = (double **)malloc(m * sizeof(double *));
    for (int i = 0; i < n; i++) {
        principal_components_matrix[i] = (double *)malloc(n * sizeof(double));
    }
    if (principal_components_matrix == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            principal_components_matrix[i][j] = 0;
            for (int k = 0; k < n; k++) {
                principal_components_matrix[i][j] += eigen_vector[i][k] * arrptr[k][j];
            }
        }
    }
    free(arrptr);
    return principal_components_matrix;
}

/**
 * \brief Function to compute integrated support degree
 * \details Computed support degree matrix for each of the sensors
 * \param[in] principle_components pointer to pointer of principle component of type double
 * \param[in] contribution_rate pointer to contribution rate of type double
 * \param[in] contrib_rate_count number of contribution rate to be used
 * \param[in] sensor_count number of sensors
 * \return Success: array of support degree matrix,
 * \return Failure: NULL
 */
double *compute_integrated_support_degree(double **principle_components,
                                          double *contribution_rate,
                                          int contrib_rate_count,
                                          int sensor_count) {
    if ((sensor_count == 0) || (contrib_rate_count == 0) || (principle_components == NULL) || (contribution_rate == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return NULL;
    }
    double *arr = (double *)malloc(sensor_count * sizeof(double));
    if (arr == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    // Calculate integrated support degree matrix
    for (int i = 0; i < sensor_count; i++) {
        for (int j = 0; j < contrib_rate_count; j++) {
            printf("INFO: Principal Component => %d-%f, Contribution Rate => %d-%f\n", i, principle_components[i][j], j, contribution_rate[i]);
            arr[i] += principle_components[i][j] * contribution_rate[i];
        }
    }
    return arr;
}

/**
 * \brief Function to eliminate invalid sensor readings
 * \details Discard the faulty sensor readings from the support degree matrix
 * \param[in,out] integrated_support_degree_matrix pointer to integrated support degree matrix
 * \param[in] fault_tolerance_threshold threshold value to cut off invalid readings
 * \param[in] sensor_count total count of sensors
 * \return Success: 0,
 * \return Failure: -1
 */
int eliminate_incorrect_data(double *integrated_support_degree_matrix,
                             double fault_tolerance_threshold,
                             int sensor_count) {
    int i;
    double mean = 0;
    double sum = 0;
    double *arr = integrated_support_degree_matrix;
    if ((sensor_count == 0) || (arr == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return -1;
    }
    for (i = 0; i < sensor_count; i++) {
        sum += arr[i];
    }
    mean = sum / (i + 1);
    for (i = 0; i < sensor_count; i++) {
        if (fabs(arr[i]) < fabs(fault_tolerance_threshold * mean)) {
            arr[i] = 0;
        }
    }
    return 0;
}

/**
 * \brief Function to compute weight coefficient for each sensor
 * \details Compute weighted coefficients for each sensors from the support degree matrix
 * \param[in] integrated_support_degree_matrix pointer to integrated support degree matrix
 * \param[in] sensor_count total count of sensors
 * \return Success: array of weighted coefficients,
 * \return Failure: NULL
 */
double *compute_weight_coefficient(double *integrated_support_degree_matrix,
                                   int sensor_count) {
    double sum_weight_coefficient = 0;
    if ((sensor_count == 0) || (integrated_support_degree_matrix == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return NULL;
    }
    double *arr = (double *)malloc(sensor_count * sizeof(double));
    if (arr == NULL) {
        printf("ERROR: Failed to allocate memory at %s\n", __func__);
        return NULL;
    }
    memset(arr, 0, sensor_count * sizeof(double));
    for (int i = 0; i < sensor_count; i++) {
        sum_weight_coefficient += integrated_support_degree_matrix[i];
    }
    for (int i = 0; i < sensor_count; i++) {
        arr[i] = integrated_support_degree_matrix[i] / sum_weight_coefficient;
    }
    return arr;
}

/**
 * \brief Function to compute fused output
 * \details Compute the aggregated value of sensors from the weighted coefficients
 * \param[in] weight_coefficient pointer to weighted coefficient of sensors
 * \param[in] sensor_data pointer to data of sensors
 * \param[in] sensor_count total count of sensors
 * \return Success: fused value of sensor data
 * \return Failure: -1
 */
double compute_fused_output(double *weight_coefficient,
                            double *sensor_data,
                            int sensor_count) {
    double fused_output = 0;
    if ((sensor_count == 0) || (weight_coefficient == NULL) || (sensor_data == NULL)) {
        printf("ERROR: Invalid param passed at %s\n", __func__);
        return -1;
    }
    for (int i = 0; i < sensor_count; i++) {
        fused_output += weight_coefficient[i] * sensor_data[i];
    }
    return fused_output;
}


/* BalSelSims.c

Simulation of balancing selection simulation with sampling (finite population effects)
Selection; reproduction; mutation based on deterministic recursions, then sampling
Repeats for 'Length' generations

Simulation uses routines found with the GNU Scientific Library (GSL)
(http://www.gnu.org/software/gsl/)
Since GSL is distributed under the GNU General Public License
(http://www.gnu.org/copyleft/gpl.html), you must download it
separately from this file.

This program can be compiled in e.g. GCC using a command like:
gcc BalSelSims -lm -lgsl -lgslcblas -I/usr/local/include -L/usr/local/lib BalSelSims.c

Then run by executing:
./BalSelSims N s rec sex self gc reps
Where:
- N is the population size
- s is the fitness disadvantage of homozygotes
- rec is recombination rate
- sex is rate of sex (a value between 0 = obligate asex, and 1 = obligate sex)
- self is selfing rate
- gc is gene conversion
- reps is how many times to introduce linked neutral allele

Note that haplotypes are defined as:
x1 = ab
x2 = Ab
x3 = aB
x4 = AB

Genotypes defined as:
g11 = g1 = ab/ab
g12 = g2 = Ab/ab
g13 = g3 = aB/ab
g14 = g4 = AB/ab
g22 = g5 = Ab/Ab
g23 = g6 = Ab/aB
g24 = g7 = Ab/AB
g33 = g8 = aB/aB
g34 = g9 = aB/AB
g44 = g10 = AB/AB

*/

/* Preprocessor statements */
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stddef.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Function prototypes */
void geninit(double *geninit);
void selection(double *geninit);
void reproduction(double *geninit);
void gconv(double *geninit);
void neutinit(double *geninit,const gsl_rng *r);
double ncheck(double *geninit);
double pcheck(double *geninit);

/* Global variable declaration */
unsigned int N = 0;		/* Pop size */
double s = 0;			/* Fitness disadvantage of homozyogtes */
double rec = 0;			/* Recombination rate */
double sex = 0;			/* Rate of sexual reproduction */
double self = 0;		/* Rate of self-fertilisation */
double gc = 0;			/* Rate of gene conversion */

/* Main program */
int main(int argc, char *argv[]){
	unsigned int g, i; 				/* Counters. Reps counter, geno counter */
	unsigned int reps;				/* Length of simulation (no. of introductions of neutral site) */
	double Bcheck = 0;				/* Frequency of B after each reproduction */
	double Acheck = 0;				/* Frequency of polymorphism */
	double Hsum = 0;				/* Summed heterozygosity over transit time of neutral allele */

	/* GSL random number definitions */
	const gsl_rng_type * T;
	gsl_rng * r;

	/* This reads in data from command line. */
	if(argc != 8){
		fprintf(stderr,"Invalid number of input values.\n");
		exit(1);
	}
	N = strtod(argv[1],NULL);
	s = strtod(argv[2],NULL);
	rec = strtod(argv[3],NULL);
	sex = strtod(argv[4],NULL);
	self = strtod(argv[5],NULL);
	gc = strtod(argv[6],NULL);
	reps = strtod(argv[7],NULL);

	/* Arrays definition and memory assignment */
	double *genotype = calloc(10,sizeof(double));				/* Genotype frequencies */
	unsigned int *gensamp = calloc(10,sizeof(unsigned int));	/* New population samples */

	/* create a generator chosen by the
    environment variable GSL_RNG_TYPE */

	gsl_rng_env_setup();
	if (!getenv("GSL_RNG_SEED")) gsl_rng_default_seed = time(0);
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);

	/* Initialising genotypes */
	geninit(genotype);

    /* Run simulation for 2000 generations to create a burn in */
    for(g = 0; g < 2000; g++){

    	/* Selection routine */
    	selection(genotype);

    	/* Reproduction routine */
    	reproduction(genotype);

       	/* Gene conversion routine */
       	gconv(genotype);

       	/* Sampling based on new frequencies */
       	gsl_ran_multinomial(r,10,N,genotype,gensamp);
       	for(i = 0; i < 10; i++){
			*(genotype + i) = (*(gensamp + i))/(1.0*N);
       	}

       	/* Printing out results (for testing) */
		/*
	   	for(i = 0; i < 10; i++){
			printf("%.10lf ", *(genotype + i));
		}
		printf("\n");
		*/
    }

	/* Reintroducing neutral genotype, resetting hap sum */
	neutinit(genotype,r);
    Bcheck = ncheck(genotype);
    Hsum = Bcheck*(1-Bcheck);
    /* printf("%.10lf %.10lf\n",Bcheck,Hsum); */

    /* Introduce and track neutral mutations 'reps' times */
    g = 0;
    while(g < reps){

    	/* Selection routine */
    	selection(genotype);

    	/* Reproduction routine */
    	reproduction(genotype);

    	/* Gene conversion routine */
       	gconv(genotype);

    	/* Sampling based on new frequencies */
       	gsl_ran_multinomial(r,10,N,genotype,gensamp);
       	for(i = 0; i < 10; i++){
       		*(genotype + i) = (*(gensamp + i))/(1.0*N);
       	}

       	/* Checking state of haplotypes: if B fixed reset so can start fresh next time */
		Bcheck = ncheck(genotype);
		Hsum += Bcheck*(1-Bcheck);
		/* printf("%.10lf %.10lf\n",Bcheck,Hsum); */

		/* If polymorphism fixed then abandon simulation */
       	Acheck = pcheck(genotype);
       	if(Acheck == 0){
       		g = reps;
       	}

       	if(Bcheck == 0 || Bcheck == 1){
       		printf("%.10lf\n",Hsum);
       		g++;
       		/* printf("Rep Number %d\n",g); */

       		if(Bcheck == 1){
       			/* Reset genotypes so B becomes ancestral allele */
       			*(genotype + 0) = *(genotype + 7);
       			*(genotype + 1) = *(genotype + 8);
       			*(genotype + 4) = *(genotype + 9);
       			*(genotype + 7) = 0;
       			*(genotype + 8) = 0;
       			*(genotype + 9) = 0;
       		}

     		/* Reintroducing neutral genotype, resetting hap sum */
	    	neutinit(genotype,r);
			Bcheck = ncheck(genotype);
       		Hsum = Bcheck*(1-Bcheck);
       	}

	}	/* End of simulation */

	/* Freeing memory and wrapping up */
 	gsl_rng_free(r);
 	free(gensamp);
	free(genotype);
	/* printf("The End!\n"); */
	return 0;
}

/* Initialising genotypes */
void geninit(double *geninit){

	/* Basic idea: before neutral mutation introduced, g0 = 1/4; g1 = 1/2; g4 = 1/4. Deterministic frequencies. */

	*(geninit + 0) = 0.25;
	*(geninit + 1) = 0.50;
	*(geninit + 2) = 0;
	*(geninit + 3) = 0;
	*(geninit + 4) = 0.25;
	*(geninit + 5) = 0;
	*(geninit + 6) = 0;
	*(geninit + 7) = 0;
	*(geninit + 8) = 0;
	*(geninit + 9) = 0;

}	/* End of gen initiation routine */

/* Initialising NEUTRAL allele */
void neutinit(double *geninit, const gsl_rng *r){

	double *probin = calloc(4,sizeof(double));					/* Probability inputs, determine location of neutral allele */
	unsigned int *probout = calloc(4,sizeof(unsigned int));		/* Output from multinomial sampling */

	/* Basic idea: g11 at freq p^2; g12 at freq 2pq; g22 at freq q^2.
	So weighted sampling to determine which genotype the neutral allele arises on
	*/

	/* Prob definitions */
	*(probin + 0) = *(geninit + 0);
	*(probin + 1) = (*(geninit + 1))/2.0;
	*(probin + 2) = (*(geninit + 1))/2.0;
	*(probin + 3) = *(geninit + 4);

	gsl_ran_multinomial(r,4,1,probin,probout);

	/* Redefining genotypes depending on outcome */
	if(*(probout + 0) == 1){
		*(geninit + 0) = (*(geninit + 0)) - 1/(1.0*N);
		*(geninit + 2) = (*(geninit + 2)) + 1/(1.0*N);
	}
	else if(*(probout + 1) == 1){
		*(geninit + 1) = (*(geninit + 1)) - 1/(1.0*N);
		*(geninit + 3) = (*(geninit + 3)) + 1/(1.0*N);
	}
	else if(*(probout + 2) == 1){
		*(geninit + 1) = (*(geninit + 1)) - 1/(1.0*N);
		*(geninit + 5) = (*(geninit + 5)) + 1/(1.0*N);
	}
	else if(*(probout + 3) == 1){
		*(geninit + 4) = (*(geninit + 4)) - 1/(1.0*N);
		*(geninit + 6) = (*(geninit + 6)) + 1/(1.0*N);
	}

 	free(probout);
	free(probin);

}	/* End of gen initiation routine */

/* Selection routine */
void selection(double *geninit){
	double Waa, WAa, WAA;		/* Fitness of locus A (bal sel locus) */
	double Wmean;				/* Mean fitness */

	Waa = 1-s;
	WAa = 1;
	WAA = 1-s;

	/* Mean fitness calculation */
	Wmean = ((*(geninit + 0))*Waa) + ((*(geninit + 1))*WAa) + ((*(geninit + 2))*Waa) + ((*(geninit + 3))*WAa) + ((*(geninit + 4))*WAA) + ((*(geninit + 5))*WAa) + ((*(geninit + 6))*WAA) + ((*(geninit + 7))*Waa) + ((*(geninit + 8))*WAa) + ((*(geninit + 9))*WAA);

	/* Changing frequencies by selection */
	*(geninit + 0) = ((*(geninit + 0))*Waa)/Wmean;
	*(geninit + 1) = ((*(geninit + 1))*WAa)/Wmean;
	*(geninit + 2) = ((*(geninit + 2))*Waa)/Wmean;
	*(geninit + 3) = ((*(geninit + 3))*WAa)/Wmean;
	*(geninit + 4) = ((*(geninit + 4))*WAA)/Wmean;
	*(geninit + 5) = ((*(geninit + 5))*WAa)/Wmean;
	*(geninit + 6) = ((*(geninit + 6))*WAA)/Wmean;
	*(geninit + 7) = ((*(geninit + 7))*Waa)/Wmean;
	*(geninit + 8) = ((*(geninit + 8))*WAa)/Wmean;
	*(geninit + 9) = ((*(geninit + 9))*WAA)/Wmean;

}	/* End of selection routine */

/* Reproduction routine */
void reproduction(double *geninit){
	/* Fed-in genotype frequencies (for ease of programming) */
	double g11s, g12s, g13s, g14s, g22s, g23s, g24s, g33s, g34s, g44s;
	/* Genotype frequencies after sex (outcross and selfing) */
	double g11SX, g12SX, g13SX, g14SX, g22SX, g23SX, g24SX, g33SX, g34SX, g44SX;
	/* Genotype frequencies after ASEX */
	double g11AS, g12AS, g13AS, g14AS, g22AS, g23AS, g24AS, g33AS, g34AS, g44AS;
	/* Haplotypes */
	double x1, x2, x3, x4;

	/* Initial definition of genotypes */
	g11s = *(geninit + 0);
	g12s = *(geninit + 1);
	g13s = *(geninit + 2);
	g14s = *(geninit + 3);
	g22s = *(geninit + 4);
	g23s = *(geninit + 5);
	g24s = *(geninit + 6);
	g33s = *(geninit + 7);
	g34s = *(geninit + 8);
	g44s = *(geninit + 9);

	/* Baseline change in haplotype frequencies */
	x1 = g11s + (g12s + g13s + g14s)/2.0 - ((g14s - g23s)*rec)/2.0;
	x2 = g22s + (g12s + g23s + g24s)/2.0 + ((g14s - g23s)*rec)/2.0;
	x3 = g33s + (g13s + g23s + g34s)/2.0 + ((g14s - g23s)*rec)/2.0;
	x4 = g44s + (g14s + g24s + g34s)/2.0 - ((g14s - g23s)*rec)/2.0;

	/* Change in SEXUAL frequencies (both outcrossing and selfing) */
	g11SX = (g11s + (g12s + g13s + g14s*pow((1 - rec),2) + g23s*pow(rec,2))/4.0)*self*sex + (1 - self)*pow(x1,2)*sex;
	g22SX = (g22s + (g12s + g24s + g23s*pow((1 - rec),2) + g14s*pow(rec,2))/4.0)*self*sex + (1 - self)*pow(x2,2)*sex;
	g33SX = (g33s + (g13s + g34s + g23s*pow((1 - rec),2) + g14s*pow(rec,2))/4.0)*self*sex + (1 - self)*pow(x3,2)*sex;
	g44SX = (g44s + (g24s + g34s + g14s*pow((1 - rec),2) + g23s*pow(rec,2))/4.0)*self*sex + (1 - self)*pow(x4,2)*sex;
	g12SX = ((g12s + (g14s + g23s)*(1 - rec)*rec)*self*sex)/2.0 + 2.0*(1 - self)*x1*x2*sex;
	g13SX = ((g13s + (g14s + g23s)*(1 - rec)*rec)*self*sex)/2.0 + 2.0*(1 - self)*x1*x3*sex;
	g14SX = ((g14s*pow((1 - rec),2) + g23s*pow(rec,2))*self*sex)/2.0 + 2.0*(1 - self)*x1*x4*sex;
	g23SX = ((g23s*pow((1 - rec),2) + g14s*pow(rec,2))*self*sex)/2.0 + 2.0*(1 - self)*x2*x3*sex;
	g24SX = ((g24s + (g14s + g23s)*(1 - rec)*rec)*self*sex)/2.0 + 2.0*(1 - self)*x2*x4*sex;
	g34SX = ((g34s + (g14s + g23s)*(1 - rec)*rec)*self*sex)/2.0 + 2.0*(1 - self)*x3*x4*sex;

	/* Change in ASEXUAL frequencies */
	g11AS = g11s*(1 - sex);
	g12AS = g12s*(1 - sex);
	g13AS = g13s*(1 - sex);
	g14AS = g14s*(1 - sex);
	g22AS = g22s*(1 - sex);
	g23AS = g23s*(1 - sex);
	g24AS = g24s*(1 - sex);
	g33AS = g33s*(1 - sex);
	g34AS = g34s*(1 - sex);
	g44AS = g44s*(1 - sex);

	/* Combining to give overall frequency change following reproduction */
	*(geninit + 0) = g11AS + g11SX;
	*(geninit + 1) = g12AS + g12SX;
	*(geninit + 2) = g13AS + g13SX;
	*(geninit + 3) = g14AS + g14SX;
	*(geninit + 4) = g22AS + g22SX;
	*(geninit + 5) = g23AS + g23SX;
	*(geninit + 6) = g24AS + g24SX;
	*(geninit + 7) = g33AS + g33SX;
	*(geninit + 8) = g34AS + g34SX;
	*(geninit + 9) = g44AS + g44SX;

}	/* End of reproduction routine */

/* Gene conversion routine */
void gconv(double *geninit){

	/* Fed-in genotype frequencies (for ease of programming) */
	double g11r, g12r, g13r, g14r, g22r, g23r, g24r, g33r, g34r, g44r;
	/* Frequencies after gene conversion */
	double g11gc, g12gc, g13gc, g14gc, g22gc, g23gc, g24gc, g33gc, g34gc, g44gc;

	/* Initial definition of genotypes */
	g11r = *(geninit + 0);
	g12r = *(geninit + 1);
	g13r = *(geninit + 2);
	g14r = *(geninit + 3);
	g22r = *(geninit + 4);
	g23r = *(geninit + 5);
	g24r = *(geninit + 6);
	g33r = *(geninit + 7);
	g34r = *(geninit + 8);
	g44r = *(geninit + 9);

	/* Gene conversion equations */
	g11gc = g11r + (gc*g12r)/4.0 + (gc*g13r)/4.0;
	g12gc = g12r*(1 - gc/2.0) + ((g14r + g23r)*gc)/4.0;
	g13gc = g13r*(1 - gc/2.0) + ((g14r + g23r)*gc)/4.0;
	g14gc = (1-gc)*g14r;
	g22gc = g22r + (g12r*gc)/4.0 + (g24r*gc)/4.0;
	g23gc = (1-gc)*g23r;
	g24gc = g24r*(1 - gc/2.0) + ((g14r + g23r)*gc)/4.0;
	g33gc = g33r + (g13r*gc)/4.0 + (g34r*gc)/4.0;
	g34gc = g34r*(1 - gc/2.0) + ((g14r + g23r)*gc)/4.0;
	g44gc = g44r + (g24r*gc)/4.0 + (g34r*gc)/4.0;

	/* Output */
	*(geninit + 0) = g11gc;
	*(geninit + 1) = g12gc;
	*(geninit + 2) = g13gc;
	*(geninit + 3) = g14gc;
	*(geninit + 4) = g22gc;
	*(geninit + 5) = g23gc;
	*(geninit + 6) = g24gc;
	*(geninit + 7) = g33gc;
	*(geninit + 8) = g34gc;
	*(geninit + 9) = g44gc;
}		/* End of gene conversion routine */

/* Has neutral allele fixed or not?  Measuring freq of B */
double ncheck(double *geninit){
	/* Fed-in genotype frequencies (for ease of programming) */
	double g11s, g12s, g13s, g14s, g22s, g23s, g24s, g33s, g34s, g44s;
	/* Haplotypes ONLY CONTAINING B */
	double x3, x4;
	double Btot = 0;        /* Total frequency of B */

	/* Initial definition of genotypes */
	g11s = *(geninit + 0);
	g12s = *(geninit + 1);
	g13s = *(geninit + 2);
	g14s = *(geninit + 3);
	g22s = *(geninit + 4);
	g23s = *(geninit + 5);
	g24s = *(geninit + 6);
	g33s = *(geninit + 7);
	g34s = *(geninit + 8);
	g44s = *(geninit + 9);

	/* Calculation of haplotypes containing B */

	x3 = g33s + (g13s + g23s + g34s)/2.0;
	x4 = g44s + (g14s + g24s + g34s)/2.0;

	/* Checking */
	Btot = x3 + x4;
	return Btot;

}	/* End of B check routine */

/* Checking if balancing polymorphism lost or not */
double pcheck(double *geninit){
	/* Fed-in genotype frequencies (for ease of programming) */
	double g11s, g12s, g13s, g14s, g22s, g23s, g24s, g33s, g34s, g44s;
	double x2, x4;
	double Atot = 0;        /* Total frequency of A */

	/* Initial definition of genotypes */
	g11s = *(geninit + 0);
	g12s = *(geninit + 1);
	g13s = *(geninit + 2);
	g14s = *(geninit + 3);
	g22s = *(geninit + 4);
	g23s = *(geninit + 5);
	g24s = *(geninit + 6);
	g33s = *(geninit + 7);
	g34s = *(geninit + 8);
	g44s = *(geninit + 9);

	/* Calculation of haplotypes containing B */

	x2 = g22s + (g12s + g23s + g24s)/2.0;
	x4 = g44s + (g14s + g24s + g34s)/2.0;

	/* Checking */
	Atot = x2 + x4;
	return Atot;

}	/* End of A check routine */

/* End of program */


#include <cstdlib>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_sf_gamma.h>
#include <complex>
#include <ctime>
#include <vector>

#define MAXIT 100000000
#define EPS 1e-12
#define FPMIN 1.0e-12
using namespace std;

typedef complex<double> cdouble;

#define REAL(z,i) ((z)[i][0])
#define IMAG(z,i) ((z)[i][1])

cdouble lngamma( cdouble );
cdouble gamma( cdouble );
cdouble betai( cdouble, cdouble, cdouble );
cdouble upperbetai( cdouble, cdouble, cdouble );
cdouble betacf( cdouble, cdouble, cdouble );
void nrerror(const char[]);


/**
 * @file      MeshHelpers.h
 *
 * @brief     This header contains mesh related helper functions
 *
 * @author    Stefan Reinhold
 * @copyright Copyright (C) 2018 Stefan Reinhold  -- All Rights Reserved.
 *            You may use, distribute and modify this code under the terms of
 *            the AFL 3.0 license; see LICENSE for full license details.
 */

#pragma once

#include "Mesh.h"

#include <Eigen/Core>
#include <gsl/gsl>
#include <igl/cotmatrix.h>
#include <igl/per_vertex_normals.h>

namespace CortidQCT {

namespace Internal {

template <class T = float>
using VertexMatrix = Eigen::Matrix<T, Eigen::Dynamic, 3>;
template <class T = float> using NormalMatrix = VertexMatrix<T>;
using FacetMatrix =
    Eigen::Matrix<typename Mesh<float>::Index, Eigen::Dynamic, 3>;
using LabelVector =
    Eigen::Matrix<typename Mesh<float>::Label, Eigen::Dynamic, 1>;
template <class T = float> using LaplacianMatrix = Eigen::SparseMatrix<T>;

/// Returns the Nx3 vertex matrix of the mesh
template <class T> inline VertexMatrix<T> vertexMatrix(Mesh<T> const &mesh) {
  return mesh
      .withUnsafeVertexPointer([&mesh](auto const *ptr) {
        return Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> const>{
            ptr, 3, gsl::narrow<Eigen::Index>(mesh.vertexCount())};
      })
      .transpose();
}

/// Returns the Mx3 index matrix of the mesh
template <class T> inline FacetMatrix facetMatrix(Mesh<T> const &mesh) {
  using Index = typename Mesh<T>::Index;
  return mesh
      .withUnsafeIndexPointer([&mesh](auto const *ptr) {
        return Eigen::Map<Eigen::Matrix<Index, 3, Eigen::Dynamic> const>{
            ptr, 3, gsl::narrow<Eigen::Index>(mesh.triangleCount())};
      })
      .transpose();
}

/// Returns a N-vector containing the per vertex labels
template <class T> inline LabelVector labelVector(Mesh<T> const &mesh) {
  using Label = typename Mesh<T>::Label;
  return mesh.withUnsafeLabelPointer([&mesh](auto const *ptr) {
    return Eigen::Map<Eigen::Matrix<Label, Eigen::Dynamic, 1> const>{
        ptr, gsl::narrow<Eigen::Index>(mesh.vertexCount())};
  });
}

/// Returns a Nx3 matrix with per-vertex normals
template <class DerivedV, class DerivedF>
inline NormalMatrix<typename DerivedV::Scalar>
perVertexNormalMatrix(Eigen::MatrixBase<DerivedV> const &V,
                      Eigen::MatrixBase<DerivedF> const &F) {
  // output matrix
  NormalMatrix<typename DerivedV::Scalar> normals;

  igl::per_vertex_normals(V, F, igl::PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,
                          normals);

  return normals;
}

/// Returns a Nx3 matrix with per-vertex normals
template <class T>
inline NormalMatrix<T> perVertexNormalMatrix(Mesh<T> const &mesh) {
  return perVertexNormalMatrix(vertexMatrix(mesh), facetMatrix(mesh));
}

/// Returns the NxN sparse laplacian matrix (using cotangent weights)
template <class DerivedV, class DerivedF>
inline LaplacianMatrix<typename DerivedV::Scalar>
laplacianMatrix(Eigen::MatrixBase<DerivedV> const &V,
                Eigen::MatrixBase<DerivedF> const &F) {
  // output matrix
  Eigen::SparseMatrix<typename DerivedV::Scalar> laplacian;
  Expects(V.rows() > 0 && V.cols() == 3);
  Expects(F.rows() > 0 && F.cols() == 3);
  igl::cotmatrix(V, F, laplacian);

  return laplacian;
}

/// Returns the NxN sparse laplacian matrix (using cotangent weights)
template <class T>
inline LaplacianMatrix<T> laplacianMatrix(Mesh<T> const &mesh) {
  return laplacianMatrix(vertexMatrix(mesh), facetMatrix(mesh));
}

} // namespace Internal
} // namespace CortidQCT


#include "poisson.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

static gsl_rng* poisson_rng_g = NULL;

void poisson_init(void)
{
	const gsl_rng_type * T;

	/* create a generator chosen by the
	environment variable GSL_RNG_TYPE */

	gsl_rng_env_setup();

	T = gsl_rng_default;
	poisson_rng_g = gsl_rng_alloc (T);
}

void poisson_fini(void)
{
	gsl_rng_free (poisson_rng_g);
}

unsigned int poisson(double mu)
{
	return gsl_ran_poisson (poisson_rng_g, mu);
}


#pragma once
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cdf.h>

#include <QVector>

#include <algorithm>
#include <tuple>

class Gaussian1DFit
{
    struct data
    {
        double* x;
        double* y;
        size_t n;
    };

    enum
    {
        numOfPara = 4
    };
    /* model function: a * exp( -1/2 * [ (t - b) / c ]^2 ) + d */
private:
    gsl_vector*                      p0; /* initial fitting parameters:amplitude,center,width,offset*/
    gsl_vector*                      p; /* fitting parameters */
    gsl_vector*                      f; /* residual yi-y(xi) */
    gsl_matrix*                      covar; /*covariance matrix, not yet multiplied by sigma, so is not variance-covariance matrix*/

    gsl_multifit_nlinear_fdf         fdf;
    gsl_multifit_nlinear_parameters  fdf_params;
    gsl_multifit_nlinear_workspace*  work;

    data                             fit_data;

    size_t                           max_iter;
    double                           ptol; /* tolerance on fitting parameter p */
    double                           gtol; /* tolerance on gradient */
    double                           ftol;

    int                              info;/*fiting stop reason, for debug*/
    double                           rss; /*mean of sum of residual square, dof corrected*/
    gsl_vector*                      confid95;

public:
    Gaussian1DFit() {};
    Gaussian1DFit(size_t n, double* datax, double* datay,
        double a0, double b0, double c0, double d0,
        size_t max_iter = 200, double ptol = 1.0e-8, double gtol = 1.0e-8, double ftol = 1.0e-8);
    ~Gaussian1DFit();
    void solve_system();
    void set_data(double* datay);
    void set_data(size_t n, double* datax, double* datay, bool free = true);
    void set_initialP(double a0, double b0, double c0, double d0);

    static double gaussian(const double a, const double b, const double c, const double d, const double t);
    QVector<double> calcFittedGaussian();

    const QVector<double> fittedPara() const;
    const QVector<double> confidence95Interval() const;

private:
    static int func_f (const gsl_vector* p, void* datafit, gsl_vector* f);
    static int func_df(const gsl_vector* p, void* datafit, gsl_matrix* J);
    static int func_fvv(const gsl_vector* p, const gsl_vector* v,
        void* datafit, gsl_vector* fvv);

};


#include <math.h>
#define FORCE_OPENBLAS_COMPLEX_STRUCT
#include <cblas.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

typedef int MKL_INT;
typedef openblas_complex_double MKL_Complex16;

#define AVX512_ALIGN 64

#include "gene_bruit_rayleigh_scalaire.c"

int main(void)
{

  int     val;
  int 	  M, N, K;
  int 	  lda, ldb, ldc;
  int     kboucle;
  float   charge;
  char    *p;

  FILE *fichier1;
  fichier1 = fopen("../results/results_openblas_gemm_fp64.dat", "w");

  int has_param_m = 0, has_param_n = 0, has_param_k = 0;
  char transa = CblasNoTrans, transb = CblasNoTrans;

  if ((p = getenv("MATMUL_M"))) {
    M = atoi(p);
    has_param_m=1;
  }
  if ((p = getenv("MATMUL_N"))) {
    N = atoi(p);
    has_param_n=1;
  }
  if ((p = getenv("MATMUL_K"))) {
    K = atoi(p);
    has_param_k=1;
  }

  if ((p = getenv("TRANSA"))) {
    if(*p == 'T')
		transa = CblasTrans;
  }
  if ((p = getenv("TRANSB"))) {
    if(*p == 'T')
		transb = CblasTrans;
  }

  /* Begin huge loop */
  for (kboucle = 1; kboucle < 31; kboucle++) {

    val = 100 * kboucle;
    M = has_param_m?M:val;
    N = has_param_n?N:val;
	K = has_param_k?K:val;

	lda = (transa == CblasNoTrans)?M:K;
    ldb = (transb == CblasNoTrans)?K:N;
    ldc = M;

    printf(">>>>> Matrix size %dx%d (K=%d, TransA=%d TransB=%d) <<<<<<\n", M, N, K,\
		transa - CblasNoTrans, transb - CblasNoTrans);

    int    iboucle, jboucle;
    int    param = 20;
    float *mat_real, *mat_imag;

    // Timers
    struct timespec tpdeb, tpfin, tpcour;
    clockid_t clock_id = CLOCK_REALTIME;
    int status2;

    double dureeloc, dureetot;
    dureetot = 0.0;

    // BLAS
    MKL_Complex16 alpha, beta;
    MKL_Complex16 *A, *B, *C;

    alpha.real = 1.0;
    alpha.imag = 0.0;
    beta.real  = 0.0;
    beta.imag  = 0.0;

    mat_real = (float *)aligned_alloc(AVX512_ALIGN, val * val * sizeof(float));
    mat_imag = (float *)aligned_alloc(AVX512_ALIGN, val * val * sizeof(float));

    A = (MKL_Complex16 *)aligned_alloc(AVX512_ALIGN, M * K * sizeof(MKL_Complex16));
    B = (MKL_Complex16 *)aligned_alloc(AVX512_ALIGN, K * N * sizeof(MKL_Complex16));
    C = (MKL_Complex16 *)aligned_alloc(AVX512_ALIGN, M * N * sizeof(MKL_Complex16));

    for (iboucle = 0; iboucle < val * val; iboucle++) {
      gene_bruit_rayleigh_scalaire(param, mat_real + iboucle,
                                   mat_imag + iboucle);
    }

    // Force hermitian matrix
    for (iboucle = 0; iboucle < K; iboucle++) {
      for (jboucle = 0; jboucle < N; jboucle++) {
        B[(iboucle * N) + jboucle].real = (double)(*(mat_real + (jboucle * K) + iboucle));

        B[(iboucle * N) + jboucle].imag =
            -1.0 * (double)(*(mat_imag + (jboucle * K) + iboucle));
      }
    }

    for (iboucle = 0; iboucle < M * K; iboucle++) {
      A[iboucle].real = (double)mat_real[iboucle];
      A[iboucle].imag = (double)mat_imag[iboucle];
    }

    status2 = clock_gettime(clock_id, &tpdeb); // start timer;

    cblas_zgemm(CblasColMajor, transa, transb, \
        M, N, K, \
        &alpha, A, lda, \
        B, ldb, &beta, C, ldc);

    status2 = clock_gettime(clock_id, &tpfin); // stop timer

    dureeloc = (float)(tpfin.tv_sec - tpdeb.tv_sec) +
               (float)(tpfin.tv_nsec - tpdeb.tv_nsec) * 1.e-9;
    dureetot = dureetot + dureeloc;

    printf("execution time = %f ms\n", dureeloc * 1000);
    charge = (float)M;
    charge = 8.0f * charge * charge * charge;
    charge = charge / dureeloc;
    printf("%f GFLOPS\n", charge / 1e9);
    fprintf(fichier1, "%5.2f\n", charge / 1e9);

    free(mat_real);
    free(mat_imag);

    free(A);
    free(B);
    free(C);
  }
  fclose(fichier1);

  return 0;
}


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

/*
 * This module calculates the solution of the normal equations least squares
 * system:
 *
 * [ J~^T J~ + mu D~^T D~ ] p~ = -J~^T f
 *
 * using the modified Cholesky decomposition. Quantities are scaled
 * according to:
 *
 * J~ = J S
 * D~ = D S
 * p~ = S^{-1} p
 *
 * where S is a diagonal matrix and S_jj = || J_j || and J_j is column
 * j of the Jacobian. This balancing transformation seems to be more
 * numerically stable for some Jacobians.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>

#include "common.c"

typedef struct
{
  gsl_matrix *JTJ;           /* J^T J */
  gsl_matrix *work_JTJ;      /* copy of J^T J */
  gsl_vector *rhs;           /* -J^T f, size p */
  gsl_permutation *perm;     /* permutation matrix for modified Cholesky */
  gsl_vector *work3p;        /* workspace, size 3*p */
  double mu;                 /* current regularization parameter */
} mcholesky_state_t;

static void *mcholesky_alloc (const size_t n, const size_t p);
static int mcholesky_init(const void * vtrust_state, void * vstate);
static int mcholesky_presolve(const double mu, const void * vtrust_state, void * vstate);
static int mcholesky_solve(const gsl_vector * f, gsl_vector *x,
                           const  void * vtrust_state, void *vstate);
static int mcholesky_solve_rhs(const gsl_vector * b, gsl_vector *x, mcholesky_state_t *state);
static int mcholesky_regularize(const double mu, const gsl_vector * diag, gsl_matrix * A,
                                mcholesky_state_t * state);

static void *
mcholesky_alloc (const size_t n, const size_t p)
{
  mcholesky_state_t *state;

  (void)n;

  state = calloc(1, sizeof(mcholesky_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate mcholesky state", GSL_ENOMEM);
    }

  state->JTJ = gsl_matrix_alloc(p, p);
  if (state->JTJ == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for JTJ", GSL_ENOMEM);
    }

  state->work_JTJ = gsl_matrix_alloc(p, p);
  if (state->work_JTJ == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for JTJ workspace",
                      GSL_ENOMEM);
    }

  state->rhs = gsl_vector_alloc(p);
  if (state->rhs == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for rhs", GSL_ENOMEM);
    }

  state->perm = gsl_permutation_alloc(p);
  if (state->perm == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for perm", GSL_ENOMEM);
    }

  state->work3p = gsl_vector_alloc(3 * p);
  if (state->work3p == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for work3p", GSL_ENOMEM);
    }

  state->mu = -1.0;

  return state;
}

static void
mcholesky_free(void *vstate)
{
  mcholesky_state_t *state = (mcholesky_state_t *) vstate;

  if (state->JTJ)
    gsl_matrix_free(state->JTJ);

  if (state->work_JTJ)
    gsl_matrix_free(state->work_JTJ);

  if (state->rhs)
    gsl_vector_free(state->rhs);

  if (state->perm)
    gsl_permutation_free(state->perm);

  if (state->work3p)
    gsl_vector_free(state->work3p);

  free(state);
}

static int
mcholesky_init(const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  mcholesky_state_t *state = (mcholesky_state_t *) vstate;

  /* compute J^T J */
  gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, trust_state->J, 0.0, state->JTJ);

  return GSL_SUCCESS;
}

/*
mcholesky_presolve()
  Compute the modified Cholesky decomposition of J^T J + mu D^T D.
Modified Cholesky is used in case mu = 0 and there are rounding
errors in forming J^T J which could lead to an indefinite matrix.

Inputs: mu     - LM parameter
        vstate - workspace

Notes:
1) On output, state->work_JTJ contains the Cholesky decomposition of
J^T J + mu D^T D
*/

static int
mcholesky_presolve(const double mu, const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  mcholesky_state_t *state = (mcholesky_state_t *) vstate;
  gsl_matrix *JTJ = state->work_JTJ;
  const gsl_vector *diag = trust_state->diag;
  int status;

  /* copy lower triangle of A to workspace */
  gsl_matrix_tricpy(CblasLower, CblasNonUnit, JTJ, state->JTJ);

  /* augment normal equations: A -> A + mu D^T D */
  status = mcholesky_regularize(mu, diag, JTJ, state);
  if (status)
    return status;

  /* compute modified Cholesky decomposition */
  status = gsl_linalg_mcholesky_decomp(JTJ, state->perm, NULL);
  if (status)
    return status;

  state->mu = mu;

  return GSL_SUCCESS;
}

/*
mcholesky_solve()
  Compute (J^T J + mu D^T D) x = -J^T f

Inputs: f      - right hand side vector f
        x      - (output) solution vector
        vstate - mcholesky workspace
*/

static int
mcholesky_solve(const gsl_vector * f, gsl_vector *x,
               const  void * vtrust_state, void *vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  mcholesky_state_t *state = (mcholesky_state_t *) vstate;
  int status;

  /* compute rhs = -J^T f */
  gsl_blas_dgemv(CblasTrans, -1.0, trust_state->J, f, 0.0, state->rhs);

  status = mcholesky_solve_rhs(state->rhs, x, state);
  if (status)
    return status;

  return GSL_SUCCESS;
}

static int
mcholesky_rcond(double * rcond, void * vstate)
{
  int status;
  mcholesky_state_t *state = (mcholesky_state_t *) vstate;
  double rcond_JTJ;

  if (state->mu != 0)
    {
      /*
       * Cholesky decomposition hasn't been computed yet, or was computed
       * with mu > 0 - recompute Cholesky decomposition of J^T J
       */

      /* copy lower triangle of JTJ to workspace */
      gsl_matrix_tricpy(CblasLower, CblasNonUnit, state->work_JTJ, state->JTJ);

      /* compute modified Cholesky decomposition */
      status = gsl_linalg_mcholesky_decomp(state->work_JTJ, state->perm, NULL);
      if (status)
        return status;
    }

  status = gsl_linalg_mcholesky_rcond(state->work_JTJ, state->perm, &rcond_JTJ, state->work3p);
  if (status == GSL_SUCCESS)
    *rcond = sqrt(rcond_JTJ);

  return status;
}

/* solve: (J^T J + mu D^T D) x = b */
static int
mcholesky_solve_rhs(const gsl_vector * b, gsl_vector *x, mcholesky_state_t *state)
{
  int status;
  gsl_matrix *JTJ = state->work_JTJ;

  status = gsl_linalg_mcholesky_solve(JTJ, state->perm, b, x);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/* A <- A + mu D^T D */
static int
mcholesky_regularize(const double mu, const gsl_vector * diag, gsl_matrix * A,
                     mcholesky_state_t * state)
{
  (void) state;

  if (mu != 0.0)
    {
      size_t i;

      for (i = 0; i < diag->size; ++i)
        {
          double di = gsl_vector_get(diag, i);
          double *Aii = gsl_matrix_ptr(A, i, i);
          *Aii += mu * di * di;
        }
    }

  return GSL_SUCCESS;
}

static const gsl_multifit_nlinear_solver mcholesky_type =
{
  "mcholesky",
  mcholesky_alloc,
  mcholesky_init,
  mcholesky_presolve,
  mcholesky_solve,
  mcholesky_rcond,
  mcholesky_free
};

const gsl_multifit_nlinear_solver *gsl_multifit_nlinear_solver_mcholesky = &mcholesky_type;


/*
  #################################################################
  ##   Private MATRIX Library ver 0.3                            ##
  ##   Made by "Gun Woo Park", Email : gunwoo.park@unina.it      ##
  ##   Fellow of SUPOLEN, Marie Curie Actions                    ##
  ##   DICMaPI, University of Naples Federico II                 ##
  #################################################################
*/

// all the integer is converted to MKL_LONG

// @ denoted for tested function
// Lots of functions are based on Call by Reference.
// Notice that for matrix operation, we have to put &.
// Vector can use by rows = 1.

// Note that I seperated library for linear algebra and matrix.
// If some error occurs during compiling, please check to include linear algebra code.

#ifndef MATRIX_H
#define MATRIX_H
#define TRUE 1
#define FALSE 0
#define BIT 64
#include <iostream>
#include <fstream>
#include <mkl.h>
#include <math.h>
//#include <omp.h>

extern "C" {
#include <gsl/gsl_sort_double.h>
}

class MATRIX
{
 public:
  MKL_LONG rows;
  MKL_LONG cols;
  MKL_LONG size;
  double *data;  // 1-dimensional array
  bool VECTOR;
  bool INITIALIZATION;
  bool DIAGONALIZATION;
  double *eigen_value;
  MKL_LONG DIAGONALIZATION_INITIAL()                     //@
  {
    /* eigen_value = new double [rows]; */
    /* eigen_value = (double*) mkl_malloc(rows*sizeof(double), BIT); */
    eigen_value = new double [rows];
    DIAGONALIZATION = TRUE;
    return 0;
  }
  MKL_LONG
    print_eigen();
  MKL_LONG
    print_eigen
    (MKL_LONG n_ele);                                //@
  // Public Member Function
  MKL_LONG
    p_self();                                     //@
  MKL_LONG
    initial();
  MKL_LONG
    initial
    (MKL_LONG N_r, MKL_LONG N_c);                    //@
  MKL_LONG
    initial
    (MKL_LONG N_r, MKL_LONG N_c, double x);
  MKL_LONG
    print();                                      //@
  MKL_LONG
    print
    (MKL_LONG n_row, MKL_LONG n_col);                  //@

  MKL_LONG
    fprint_skip
    (std::ofstream& file, MKL_LONG N_skip);
  MKL_LONG
    fprint_skip_transpose
    (std::ofstream& file, MKL_LONG N_skip);
  MKL_LONG
    fprint_LONG_skip
    (std::ofstream& file, MKL_LONG N_skip);
  MKL_LONG
    fprint_LONG_skip_transpose
    (std::ofstream& file, MKL_LONG N_skip);
  MKL_LONG
    fprint_LONG_skip_transpose_LIMROWS
    (std::ofstream& file, MKL_LONG N_skip, MKL_LONG N_lim_rows);
  MKL_LONG
    fprint
    (std::ofstream& file);                             // ???
  MKL_LONG
    fprint_transpose
    (std::ofstream& file);                             // ???
  MKL_LONG
    fprint_LONG
    (std::ofstream& file);                             // ???
  MKL_LONG
    fprint_LONG_transpose
    (std::ofstream& file);                             // ???
  MKL_LONG
    fprint_row
    (std::ofstream& file, MKL_LONG given_row);
  MKL_LONG
    fprint_out_skip
    (std::ofstream& file, MKL_LONG N_skip);
  MKL_LONG
    fprint_out
    (std::ofstream& file);                             // ???

  MKL_LONG
    test_arr();                                   //@
  MKL_LONG
    test_arr_symm();                              //@
  MKL_LONG
    nonzero();
  MKL_LONG
    ABS_cond
    (double x);
  double
    ABS_sum();
  MKL_LONG
    force_clean(double TOLERANCE);
  double
    sum();
  double
    average();
  MKL_LONG
    add(MATRIX& given_MAT);

  MKL_LONG
  read_exist_data(const char* fn_given_array);

  // Constructor
  MATRIX();                                         //@
  MATRIX
    (MKL_LONG N_r, MKL_LONG N_c);                         //@
  MATRIX
    (MKL_LONG N_r, MKL_LONG N_c, double x);               //@
  // Copy Constructor
  MKL_LONG
    copy_obj
    (const MATRIX& Mat);
  MATRIX
    (const MATRIX& Mat);                        //@
  /* int connect_copy_constructor(const MATRIX& Mat); */
  // Destructor
  virtual
    ~MATRIX();                                        //@

  MKL_LONG
    data_delete();                                //@
  MKL_LONG
    undefined_error();                            //@
  /* MKL_LONG index(MKL_LONG i, MKL_LONG j);                          //@ */ // inlined
  MKL_LONG
    sort();
  MKL_LONG
    sort2
    (MATRIX& given_index);
  /* MKL_LONG sort2(size_t* index); */
  // inlined functions
  // note that function inside class definition is inlined
  // these are frequently called function that have small amount of computation

  double
    norm()
  {
    double result = 0.0;
    for(MKL_LONG  i=0; i<size; i++)
      {
        result += pow(data[i],2.0);
      }
    return sqrt(result);
  }

  MKL_LONG
    set_value
    (double x)
  {
    if(INITIALIZATION)
      {
        for(MKL_LONG  i=0; i<size; i++)
          {
            data[i] = x;
          } // i
      }
    else
      {
        undefined_error();
      }
    return 0;
  }

  MKL_LONG
    index
    (MKL_LONG i, MKL_LONG j)
  {
    return i*cols+j;
  }

  // operator
  // inlined operator overloading
  MATRIX&
    operator=
    (const MATRIX &Mat)
    {
      copy_obj(Mat);
      return *this;
    }

  MATRIX&
    operator+=
    (const MATRIX &Mat)
    {
      // std::cout << "the operator += for MATRIX class is in buggy status\n";
      // std::cout << "if this operator is called, to check the internal problems\n";
      if (size != Mat.size)
        std::cout << "The += operator must happen when two size are the same\n";
      // copy_obj(Mat);
      // for(MKL_LONG i=0; i<size; i++)
      //   {
      //     data[i] += Mat.data[i];
      //   }
      cblas_daxpy(size, 1.0, Mat.data, 1, data, 1);
      return *this;
    }

  double&
    operator()
    (MKL_LONG  i, MKL_LONG  j)
  {
    return data[i*cols+j];
  }

  double&
    operator()
    (MKL_LONG  i)
  {
    return data[i];
  }

};

inline MKL_LONG
make_unit_vector
(MATRIX& given_vec)
{
  double norm = 0.;
  for(MKL_LONG  i=0; i<given_vec.size; i++)
    {
      norm += pow(given_vec.data[i], 2.0);
    }
  norm = sqrt(norm);
  for(MKL_LONG  i=0; i<given_vec.size; i++)
    {
      given_vec.data[i] /= norm;
    }
  return norm;
}

inline MKL_LONG
matrix_mul
(MATRIX& given_vec, double val)
{
  cblas_dscal(given_vec.size, val, given_vec.data, 1);
  return 0;
}

MKL_LONG
nonzero
(const MATRIX &A);

#endif


/*
  NAME:
     splitnmergegauss
  PURPOSE:
     split one gaussian and merge two other gaussians
  CALLING SEQUENCE:
     splitnmergegauss(struct gaussian * gaussians,int K, gsl_matrix * qij,
     int j, int k, int l)
  INPUT:
     gaussians   - model gaussians
     K           - number of gaussians
     qij         - matrix of log(posterior likelihoods)
     j,k         - gaussians that need to be merged
     l           - gaussian that needs to be split
  OUTPUT:
     updated gaussians
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include "proj_gauss_mixtures.h"

void splitnmergegauss(struct gaussian * gaussians,int K,
		      gsl_matrix * qij, int j, int k, int l){
  //get the gaussians to be split 'n' merged
  int d = (gaussians->VV)->size1;//dim of mm
  //int partial_indx[]= {-1,-1,-1};/* dummy argument for logsum */
  //bool * dummy_allfixed = (bool *) calloc(K,sizeof(bool));
  //j,k,l gaussians
  struct gaussian gaussianj, gaussiank, gaussianl;
  gaussianj.mm = gsl_vector_alloc(d);
  gaussianj.VV = gsl_matrix_alloc(d,d);
  gaussiank.mm = gsl_vector_alloc(d);
  gaussiank.VV = gsl_matrix_alloc(d,d);
  gaussianl.mm = gsl_vector_alloc(d);
  gaussianl.VV = gsl_matrix_alloc(d,d);

  gsl_matrix * unitm = gsl_matrix_alloc(d,d);
  gsl_matrix_set_identity(unitm);
  gsl_vector * eps = gsl_vector_alloc(d);
  double qjj,qjk,detVVjl;
  int kk;
  for (kk = 0; kk != K; ++kk){
    if (kk == j){
      gaussianj.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussianj.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussianj.VV,gaussians->VV);
      qjj = exp(logsum(qij,j,false));//,dummy_allfixed));
    }
    if (kk == k){
      gaussiank.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussiank.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussiank.VV,gaussians->VV);
      qjk = exp(logsum(qij,k,false));//,dummy_allfixed));
    }
    if (kk == l){
      gaussianl.alpha = gaussians->alpha;
      gsl_vector_memcpy(gaussianl.mm,gaussians->mm);
      gsl_matrix_memcpy(gaussianl.VV,gaussians->VV);
    }
    ++gaussians;
  }
  gaussians -= K;

  //merge j & k
  gaussianj.alpha += gaussiank.alpha;
  if (qjk == 0. && qjj == 0){
    gsl_vector_add(gaussianj.mm,gaussiank.mm);
    gsl_vector_scale(gaussianj.mm,0.5);
    gsl_matrix_add(gaussianj.VV,gaussiank.VV);
    gsl_matrix_scale(gaussianj.VV,0.5);
  }
  else{
    gsl_vector_scale(gaussianj.mm,qjj/(qjj+qjk));
    gsl_vector_scale(gaussiank.mm,qjk/(qjj+qjk));
    gsl_vector_add(gaussianj.mm,gaussiank.mm);
    gsl_matrix_scale(gaussianj.VV,qjj/(qjj+qjk));
    gsl_matrix_scale(gaussiank.VV,qjk/(qjj+qjk));
    gsl_matrix_add(gaussianj.VV,gaussiank.VV);
  }

  //split l
  gaussianl.alpha /= 2.;
  gaussiank.alpha = gaussianl.alpha;
  detVVjl = bovy_det(gaussianl.VV);
  detVVjl= pow(detVVjl,1./d);
  gsl_matrix_scale(unitm,detVVjl);
  gsl_matrix_memcpy(gaussiank.VV,unitm);
  gsl_matrix_memcpy(gaussianl.VV,unitm);
  gsl_vector_memcpy(gaussiank.mm,gaussianl.mm);
  bovy_randvec(eps,d,sqrt(detVVjl));
  gsl_vector_add(gaussiank.mm,eps);
  bovy_randvec(eps,d,sqrt(detVVjl));
  gsl_vector_add(gaussianl.mm,eps);

  //copy everything back into the right gaussians
  for (kk = 0; kk != K; ++kk){
    if (kk == j){
      gaussians->alpha = gaussianj.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussianj.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussianj.VV);
    }
    if (kk == k){
      gaussians->alpha = gaussiank.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussiank.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussiank.VV);
    }
    if (kk == l){
      gaussians->alpha = gaussianl.alpha;
      gsl_vector_memcpy(gaussians->mm,gaussianl.mm);
      gsl_matrix_memcpy(gaussians->VV,gaussianl.VV);
    }
    ++gaussians;
  }
  gaussians -= K;

  //cleanup
  gsl_matrix_free(unitm);
  gsl_vector_free(eps);
  //free(dummy_allfixed);

  return ;
}


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Of the two methods provided below, I think the Polar method is more
 * efficient, but only when you are actually producing two random
 * deviates.  We don't produce two, because then we'd have to save one
 * in a static variable for the next call, and that would screws up
 * re-entrant or threaded code, so we only produce one.  This makes
 * the Ratio method suddenly more appealing.  There are further tests
 * one can make if the log() is slow.  See Knuth for details */

/* Both methods pass the statistical tests; but the polar method
 * seems to be a touch faster on my home Pentium, EVEN though we
 * are only using half of the available random deviates!
 */

/* Polar (Box-Mueller) method; See Knuth v2, 3rd ed, p122 */

double
gsl_ran_gaussian (const gsl_rng * r, const double sigma)
{
  double x, y, r2;

  do
    {
      /* choose x,y in uniform square (-1,-1) to (+1,+1) */

      x = -1 + 2 * gsl_rng_uniform (r);
      y = -1 + 2 * gsl_rng_uniform (r);

      /* see if it is in the unit circle */
      r2 = x * x + y * y;
    }
  while (r2 > 1.0 || r2 == 0);

  /* Box-Muller transform */
  return sigma * y * sqrt (-2.0 * log (r2) / r2);
}

/* Ratio method (Kinderman-Monahan); see Knuth v2, 3rd ed, p130 */
/* K+M, ACM Trans Math Software 3 (1977) 257-260. */

double
gsl_ran_gaussian_ratio_method (const gsl_rng * r, const double sigma)
{
  double u, v, x;

  do
    {
      v = gsl_rng_uniform (r);
      do
	{
	  u = gsl_rng_uniform (r);
	}
      while (u == 0);
      /* Const 1.715... = sqrt(8/e) */
      x = 1.71552776992141359295 * (v - 0.5) / u;
    }
  while (x * x > -4.0 * log (u));

  return sigma * x;
}

double
gsl_ran_gaussian_pdf (const double x, const double sigma)
{
  double u = x / fabs (sigma);
  double p = (1 / (sqrt (2 * M_PI) * fabs (sigma))) * exp (-u * u / 2);
  return p;
}

double
gsl_ran_ugaussian (const gsl_rng * r)
{
  return gsl_ran_gaussian (r, 1.0);
}

double
gsl_ran_ugaussian_ratio_method (const gsl_rng * r)
{
  return gsl_ran_gaussian_ratio_method (r, 1.0);
}

double
gsl_ran_ugaussian_pdf (const double x)
{
  return gsl_ran_gaussian_pdf (x, 1.0);
}


#include <gsl/gsl_interp.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_version.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics_double.h>


#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>
typedef struct{
  double g;//gravity
  double M;//mass of the car
  double m;//mass of the load
  double r;//length of the rope
  double C;//damping coefficient
  double k;//spring constang

  double F;//input force
  double T;//tension to the rope
  //  double FF;//Fictitious force

  double x,x0;//x position of the load
  double y,y0;//y position of the load
  double X;//X position of the car
  double a;//angle (theta)

  double dx,dx0;
  double dy,dy0;
  double dX,dX0;
  double dr;
  double da,da0;
  double dXmax;
  double ddXmax;
  double Fmax;

  double ddx;
  double ddy;
  double ddX;
  double ddr;
  double dda;
  double h;
  int nh;
  double _h;
  int _dim;
  double *_y;
  double *_y_err;
  double *_dydt_in;
  double *_dydt_out;
  const gsl_odeiv_step_type *_T;
  gsl_odeiv_step *_s;
  gsl_odeiv_system _sys;
  double _t;
} CRANE;

#define dim_crane 4
int cranefunc (double t, const double y[], double f[], void *params)
{
  CRANE *c = (CRANE *)params;

#ifdef ICONIP2014 //wrong dynamics
  f[0] = y[1];
  f[1] = -c->k*(y[0]-y[2])/c->m - c->C*(y[1]-y[3])/c->m;//iconip2014
  f[2] = y[3];
  f[3] = (c->k*(y[0]-y[2]) + c->F)/c->M;//iconip2014
//  f[0] = y[1];
//  f[1] = -c->k*(y[0]-y[2])/c->m - c->C*(y[1]-y[3])/c->m;
//  f[2] = y[3];
//  f[3] = (c->k*(y[0]-y[2]) + c->C*(y[1]-y[3] + c->F)/c->M;
#else //corrected dynamics
  f[0] = y[1];
  f[1] = (-c->k*(y[0]-y[2]) - c->C*(y[1]-y[3])       )/c->m;//iconip2014
  f[2] = y[3];
  f[3] = ( c->k*(y[0]-y[2]) + c->C*(y[1]-y[3]) + c->F)/c->M;//iconip2014
#endif

//  f[0] = y[1];
//  f[1] = -c->k*y[0]/c->m - c->C*y[1]/c->m - c->ddX;
//  f[2] = y[3];
//  f[3] = (c->k*y[0] + c->F)/c->M;
//
//  f[0] = y[1];
//  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
//  f[2] = y[3];
//  f[3] = (c->F+c->T*sin(y[0]))/c->M;
  return GSL_SUCCESS;
}
////////////////
#define square(x) ((x)*(x))
CRANE crane;
int initialize()
{
  crane.g=9.8;
  crane.M=100;
  crane.m=20;//10;//20
  crane.r=5;//5;
  crane.C=10;//5;
  crane.k=15;//
  crane.dr=crane.ddr=0;
  crane.x0=0;
  crane.y0=crane.r;
  crane.dx0=crane.dy0=crane.da0=0;
  crane.dXmax=1.;//??ddXmax=(dXmax-dX)/dt;//??
  crane.ddXmax=0.2;//??ddXmax=(dXmax-dX)/dt;//??
  crane.Fmax=30.0;//check//  crane.Fmax=30;//check
#ifdef CRANESUB
  crane.h=AP_tS;
  crane.m=_crane_m;
  crane.r=_crane_r;
  crane.C=_crane_C;
  crane.k=_crane_k;//
  crane.dXmax=_crane_dXmax;
  if(_AP_umax>0) AP_u_max=crane.ddXmax=_AP_umax;
  else AP_u_max=crane.ddXmax;
  AP_u_min=-AP_u_max;
  //  starttime=0;
  //  totaltime=50;
  rr=AP_r=_AP_r;//10
  rr_kyoyou=_rr_kyoyou;
  //  p=(double *)malloc(buffsize*sizeof(double));
  C_MODE=11;
  //  iteration=_iteration;
#else
  crane.h=0.01;
  //  crane.h=0.0001;
#endif
  crane.nh=10;
  crane._h=crane.h/crane.nh;//0.001
  crane._dim=4;
  if(crane._y==NULL){
    crane._y=(double*)malloc(crane._dim*sizeof(double));
    crane._y_err=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_in=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_out=(double*)malloc(crane._dim*sizeof(double));
  }
  crane._T= gsl_odeiv_step_rk4;
  crane._s= gsl_odeiv_step_alloc (crane._T, crane._dim);
  crane._sys= (gsl_odeiv_system){cranefunc, NULL, crane._dim, &crane};
  //  crane._sys[0]=cranefunc;
  //  crane._sys[1]=NULL;
  //  crane._sys[2]=crane._dim;
  //  crane._sys[3]=&crane;
  //initialize variables
  crane._t=crane._y[0]=crane._y[1]=crane._y[2]=crane._y[3]=0;
  //  crane.FF=0;
  int i;for(i=0;i<crane._dim;i++) crane._dydt_in[i]=0;
  GSL_ODEIV_FN_EVAL(&(crane._sys), crane._t, crane._y, crane._dydt_in);
//  crane.a  =crane._y[0];
//  crane.da =crane._y[1];
//  crane.X  =crane._y[2];
//  crane.dX =crane._y[3];
//  crane.x  =crane.X+crane.r * sin(crane.a); //output y
//  crane.y  =crane.r*cos(crane.a);
//  crane.dXmax=1.;//??ddXmax=(dXmax-dX)/dt;//??
//  crane.ddXmax=0.2;//??ddXmax=(dXmax-dX)/dt;//??
  return(0);
}
#ifndef CRANESUB
char *fn="crane3io.dat";
FILE *fp;
#endif//#ifndef CRANESUB

//double plant(double uu)
double plant(double uu)
{
  if(uu>crane.ddXmax) uu=crane.ddXmax;
  else if(uu<-crane.ddXmax) uu=-crane.ddXmax;

  int n;
  for(n=0;n<crane.nh;n++){
    crane.ddX=uu;//crane.ddX=(uu<crane.ddXmax)?uu:crane.ddXmax;
    if(crane.dX>=crane.dXmax && crane.ddX>0) crane.ddX=0;
    else if(crane.dX<=-crane.dXmax && crane.ddX<0) crane.ddX=0;

    int status = gsl_odeiv_step_apply (crane._s, crane._t, crane._h,
				       crane._y, crane._y_err,
				       crane._dydt_in,
				       crane._dydt_out,
				       &crane._sys);
    if (status != GSL_SUCCESS) {fprintf(stderr,"### failure of plant calc.\n"); break;}

    int i;for(i=0;i<dim_crane;i++) crane._dydt_in[i]=crane._dydt_out[i];
    crane._t+=crane._h;
    crane.x  =crane._y[0];
    crane.dx =crane._y[1];
    crane.X  =crane._y[2];
    crane.dX =crane._y[3];
    //    crane.x  =crane.X+crane.a; //output y
    //    crane.dx=(crane.x-crane.x0)/crane._h;
    //crane.x  =crane.X+crane.r * sin(crane.a); //output y
    //crane.y  =crane.r*cos(crane.a);
    //    crane.dx=(crane.x-crane.x0)/crane._h;
    //    crane.dy=(crane.y-crane.y0)/crane._h;
    crane.ddx=(crane.dx-crane.dx0)/crane._h;
    //    crane.ddX=(crane.dX-crane.dX0)/crane._h;
    //    crane.ddy=(crane.dy-crane.dy0)/crane._h;
    //    crane.T=crane.m*sqrt(square(crane.ddx)+square(crane.ddy-crane.g));
    crane.F=crane.M*crane.ddX-crane.k*(crane.x-crane.X)-crane.C*(crane.dx-crane.dX);;
    //    crane.F=crane.M*crane.ddX-crane.k*(crane.x-crane.X);
    //    crane.F=crane.M*crane.ddX-crane.T*sin(crane.a);
    //    crane.dda=(crane.da-crane.da0)/crane._h;
    crane.x0=crane.x;//?
    //    crane.y0=crane.y;
    crane.dx0=crane.dx;//?
    //    crane.dy0=crane.dy;
    //    crane.da0=crane.da;
    //    crane.FF=crane.ddX;
    crane.dX0=crane.dX;
  }
#ifndef CRANESUB
  fprintf(fp,"%.7e %.7e %.7e", crane._t,crane._y[0],crane._y[1]);//crane.x,crane.dx
  fprintf(fp," %.7e %.7e %.7e" ,crane.X,crane.dX,crane.ddX);
  fprintf(fp," %.7e %.7e %.7e" ,crane.x,crane.dx,crane.ddx);
  fprintf(fp," %.7e %.7e %.7e" ,crane.y,crane.dy,crane.ddy);
  fprintf(fp," %.7e %.7e" ,crane.T,crane.F);
  fprintf(fp,"\n");
#endif //#ifndef CRANESUB
  return(crane.x);//crane.x in [0,AP_y=10?] ==> rt in [0,1]
}
#ifndef CRANESUB
int main(int argc,char **argv)
{
  ////////////////////////////////////////////////////
  /// method 1                                     ///
  /// input ddX                                    ///
  /// output a,x,y,F                               ///
  ////////////////////////////////////////////////////
  //  double h = 0.001;//,hh=0.1; h = 0.0001;//,hh=0.1;
  double T=5;
  double t0=0;     //stationary
  double t1=T+t0;//accelerate(speed up)
  double t2=T+t1;//free run
  double t3=T+t2;//decelerate(slow down)
  double t4=t0+50;   //free run

  initialize();
  int M=(t4/crane.h)+1;
  double *ddX=(double*)malloc(sizeof(double)*M);
  //  double *F  =(double*)malloc(sizeof(double)*M);
  double Vmax=1;//dX/dt=1[m/s]
  //C:10;K:15;M:100;m:10;TS:0.5;/*数式処理の場合、この行を実行しない.m=10,40,70,100にして行う*/
  /*  crane.M=100;
  crane.C=10;
  crane.k=15;
  crane.m=10;*/
  int i;
  for(i=1;i<argc;i++){
    if(strncmp(argv[i],"m:",2)==0){
      sscanf(&argv[i][2],"%lf",&crane.m);
      //      fprintf(stderr,"#m=%g",crane.m);
    }
  }
  fprintf(stderr,"#C:%g K:%g M:%g m:%g\n",crane.C,crane.k,crane.M,crane.m);

  double TS=0.5;
  double umax=10;
  int n;
  double t;
  ////////////////////////////////////////////////////
  ///   set ddX                                    ///
  ////////////////////////////////////////////////////
  int n0 =t0/crane.h+0.5, n4=t4/crane.h+0.5;
  for(t=0;t<t4;t+=crane.h){
    double u;
    n=t/crane.h;
    if(t<=t0) u=0; //zero
    else if(t<=t1) u=umax*0.8; //accelerate
    else if(t<=t2) u=0;            //free move
    else if(t<=t3) u=-umax*0.8;//decelerate
    else u=0;                      //free move
    ddX[n]=u;//??
    //    ddX[n]=u*crane.h;//??
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  fp=fopen(fn,"w");
  fprintf(fp,"#%.7e %d %d %.7e %.7e %.7e %.7e #h,n0,n4,M,n,r\n",crane.h,n0,n4,crane.M,crane.m,crane.r,crane.C);
  //  double dx0=0,dy0=0,da0=0;
  //  for(n=0;n<n4;){
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(ddX[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}
#endif //#ifndef SUB


#ifndef LADMM_H
#define LADMM_H

#include "Matrix.h"

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>
//#include "cmd_line.h"

//This class implements the method LADMM

/*
The optimization problem to solve is:

min \sum_i f^i(A_ix)+ \sum_i h^i(y^i)+ g(x)
s.t. Mx= y
Assumption 1: For each i, f_i is smooth, g(x) is seperable
*/

template<typename L, typename D>
class LADMM
{
private:

  std::vector<D> Ax;
  std::vector<D> old_Ax;

  std::vector<D> Mx;
  std::vector<D> old_Mx;

  std::vector<D> Mty;
  std::vector<D> old_Mty;

  std::vector<D> Mtlambda;
  std::vector<D> old_Mtlambda;

  std::vector<D> gradient;

  std::vector<D> MtMx;

protected:
  Matrix<L, D> data_A;

  Matrix<L, D> data_M;

  std::vector<D> x;

  std::vector<D> old_x;

  std::vector<D> y;

  std::vector<D> old_y;

  std::vector<D> lambda;

  std::vector<D> old_lambda;

  D beta;

  L m_1;

  L m_2;

  L m_3;

  D rho;

  D tau;

  D sigma;

  D L_phi;

  D function_value;

  D infeas;

  L print_every_N_ADMM;

  D running_time_ADMM;

  L nb_outer_iters;

  ofstream samp_ADMM;

public:
  D lambda_f;

  D mu_g;

  D lambda1;

  D lambda2;

  D L_h;

  virtual inline D value_of_f_j(D, L){return D(NULL);}
  virtual inline D value_of_h_j(D, L){return D(NULL);}
  virtual inline D gradient_of_f_j(D, L){return D(NULL);}
  virtual inline D prox_of_h_j(D,D, L){return D(NULL);}
  virtual inline D value_of_g_j(D, L){return D(NULL);}
  virtual inline D prox_of_g_j(D, D, L){return D(NULL);}
  virtual inline void set_matrix_M(){}
  virtual inline void set_matrix_A(){}

/*
  LADMM(const char* matrix_file, const char* matrix_file2)
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(),data_A(matrix_file), data_M(matrix_file2)
  {
  	this->matrix_merge(data_A,data_M);
    this->gamma=1;
  }
 */
  inline void set_L_phi(){
  	if (data_A.nsamples== 0){
  		L_phi= 0;
	  }
	else{
  		L_phi= compute_lambda_max_A(10);
  	}
  }

  D compute_lambda_max_A(L K){

    std::vector<D> bk(data_A.nfeatures);
    for (L j=0;j<data_A.nfeatures;j++)
    {
      bk[j]=1;
    }
    std::vector<D> yk(data_A.nsamples);
    D normk;
    D tmp;
    for(L kk=0;kk<K;kk++){
      for (L i=0;i<data_A.nsamples;i++){
        tmp=0;
        for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];	k++)
        {
          L j=data_A.row_idx[k];
          tmp+=data_A.A[k]*bk[j];
        }
        yk[i]=tmp;
      }
      normk=0;
      for (L j=0;j<data_A.nfeatures;j++){
        bk[j]=0;
        for (L k = data_A.ptr_t[j]; k < data_A.ptr_t[j + 1];	k++)
        {
          L i=data_A.col_idx[k];
          bk[j]+=data_A.A_t[k]*yk[i]*lambda_f;
        }
        normk+=bk[j]*bk[j];
      }
      normk=sqrt(normk);
      for (L j=0;j<data_A.nfeatures;j++)
      {bk[j]=bk[j]/normk; }
    }
    cout<<endl;
    D res=0;
    normk=0;
    for (L i=0;i<data_A.nsamples;i++){
      tmp=0;
      for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];	k++)
      {
        L j=data_A.row_idx[k];
        tmp+=data_A.A[k]*bk[j];
      }
      yk[i]=tmp;
      normk+=yk[i]*yk[i];
    }
    std::vector<D> bk2(data_A.nfeatures);
    for (L j=0;j<data_A.nfeatures;j++){
      bk2[j]=0;
      for (L k = data_A.ptr_t[j]; k < data_A.ptr_t[j + 1];	k++)
      {
        L i=data_A.col_idx[k];
        bk2[j]+=data_A.A_t[k]*yk[i]*lambda_f;
      }
    }
    for (L j=0;j<data_A.nfeatures;j++)
    res+=bk2[j]*bk[j];
    return res;
  }

D compute_lambda_max_M(L K){

    std::vector<D> bk(data_M.nfeatures);
    for (L j=0;j<data_M.nfeatures- 1;j++)
    {
      bk[j]=1;
    }
    bk[data_M.nfeatures- 1]= 2;
    std::vector<D> yk(data_M.nsamples);
    D normk;
    D tmp;
    for(L kk=0;kk<K;kk++){
      for (L i=0;i<data_M.nsamples;i++){
        tmp=0;
        for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];	k++)
        {
          L j=data_M.row_idx[k];
          tmp+=data_M.A[k]*bk[j];
        }
        yk[i]=tmp;
      }
      normk=0;
      for (L j=0;j<data_M.nfeatures;j++){
        bk[j]=0;
        for (L k = data_M.ptr_t[j]; k < data_M.ptr_t[j + 1];	k++)
        {
          L i=data_M.col_idx[k];
          bk[j]+=data_M.A_t[k]*yk[i];
        }
        normk+=bk[j]*bk[j];
      }
      normk=sqrt(normk);
      for (L j=0;j<data_M.nfeatures;j++)
      {bk[j]=bk[j]/normk; }
    }
    D res=0;
    normk=0;
    for (L i=0;i<data_M.nsamples;i++){
      tmp=0;
      for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];	k++)
      {
        L j=data_M.row_idx[k];
        tmp+=data_M.A[k]*bk[j];
      }
      yk[i]=tmp;
      normk+=yk[i]*yk[i];
    }
    std::vector<D> bk2(data_M.nfeatures);
    for (L j=0;j<data_M.nfeatures;j++){
      bk2[j]=0;
      for (L k = data_M.ptr_t[j]; k < data_M.ptr_t[j + 1];	k++)
      {
        L i=data_M.col_idx[k];
        bk2[j]+=data_M.A_t[k]*yk[i];
      }
    }
    for (L j=0;j<data_M.nfeatures;j++)
    res+=bk2[j]*bk[j];
    return res;
  }

  L get_nb_features(){
    return data_A.get_d();
  }

 /* inline D get_lambda1(){return lambda1;}
  inline D get_lambda2(){return lambda2;}
*/

  void update_x(){
    for (L i= 0; i< m_1; i++){
    	gradient[i]= 0;
    	for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
    	{
      		L j=data_A.col_idx[k];
      		gradient[i]+= data_A.A_t[k]*gradient_of_f_j(Ax[j],j);
    	}
    	MtMx[i]= 0;
    	for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    	{
      		L j=data_M.col_idx[k];
      		MtMx[i]+= data_M.A_t[k]*Mx[j];
    	}
    	x[i]= prox_of_g_j(x[i]- (Mtlambda[i]+ gradient[i]+ beta*MtMx[i]- beta*Mty[i])/(beta*tau+ L_phi), beta*tau+ L_phi, i);
	}
	for (L i= 0; i< m_2; i++){
		Ax[i]= compute_AiTx(i);
	}
	for (L i= 0; i< m_3; i++){
		Mx[i]= compute_MiTx(i);
	}
  }

  void update_y(){
  	for (L i=0; i< m_3; i++){
  		y[i]= prox_of_h_j(y[i]+ (lambda[i]+ beta*Mx[i]- beta*y[i])/(beta*sigma), beta*sigma, i);
	  }
	for (L i=0; i< m_1; i++){
		Mty[i]= compute_MTiTy(i);
	}
  }

  D compute_AiTx(L i){
    D res=0;
    for (L k = data_A.ptr[i]; k < data_A.ptr[i + 1];k++)
    {
      L j=data_A.row_idx[k];
      res+= data_A.A[k]*x[j];
    }
    return res;
  }

  D compute_MiTx(L i){
    D res=0;
    for (L k = data_M.ptr[i]; k < data_M.ptr[i + 1];k++)
    {
      L j=data_M.row_idx[k];
      res+= data_M.A[k]*x[j];
    }
    return res;
  }

  D compute_MTiTy(L i){
    D res=0;
    for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    {
      L j=data_M.col_idx[k];
      res+= data_M.A_t[k]*y[j];
    }
    return res;
  }

  D compute_MTiTlambda(L i){
    D res=0;
    for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
    {
      L j=data_M.col_idx[k];
      res+= data_M.A_t[k]*lambda[j];
    }
    return res;
  }

  void compute_function_value(){
    D res= 0;
    for (L i= 0; i< data_A.nfeatures; i++){
      res+= value_of_g_j(x[i],i);
    }
    for (L i= 0; i< data_M.nsamples; i++){
      res+= value_of_h_j(Mx[i],i);
    }
    for (L i= 0; i<data_A.nsamples; i++){
      res+= value_of_f_j(Ax[i],i);
    }
    function_value= res;
  }

  void compute_infeasibility(){
  	D res = 0;
  	for (L i= 0; i< data_M.nsamples; i++){
      res+= (Mx[i]- data_M.b[i])*(Mx[i]- data_M.b[i]);
    }
    infeas= sqrt(res);
  }

void update_lambda(){
  for(L j=0;j<m_3;j++)
  {
    lambda[j]= lambda[j]+ beta*(Mx[j]- y[j]);
  }
  for (L i=0; i< m_1; i++){
		Mtlambda[i]= compute_MTiTlambda(i);
	}
}

void Initialize(D beta_0, D val_rho, vector<D> & x0,vector<D> & y0, vector<D> & lambda0){
  cout<<"start initializing"<<endl;
  set_matrix_M();
  set_matrix_A();

  m_1=data_A.get_d();
  m_2=data_A.get_n();
  m_3=data_M.get_n();
  cout<<"m_1="<<m_1<<endl;
  cout<<"m_2="<<m_2<<endl;
  cout<<"m_3="<<m_3<<endl;

  beta=beta_0;
  //tau= data_A.nsamples;

  tau= 1.02*compute_lambda_max_M(10);
  sigma= 1;

  rho=val_rho;

  x.resize(m_1,0);
  old_x.resize(m_1,0);
  y.resize(m_3,0);
  old_y.resize(m_3,0);
  lambda.resize(m_3,0);
  old_lambda.resize(m_3,0);
  for(L i=0;i<m_1;i++){
    x[i]=x0[i];
  }
  for(L j=0;j<m_3;j++){
    y[j]=y0[j];
  }
  for(L j=0;j<m_3;j++){
    lambda[j]=lambda0[j];
  }

  Ax.clear();
  Ax.resize(m_2,0);
  Mx.clear();
  Mx.resize(m_3,0);
  Mty.clear();
  Mty.resize(m_1,0);
  Mtlambda.clear();
  Mtlambda.resize(m_1,0);
  gradient.clear();
  gradient.resize(m_1,0);
  MtMx.clear();
  MtMx.resize(m_1,0);
  L_phi= 0;
  set_L_phi();
  cout<< "L_phi= "<< L_phi << " beta= "<< beta<< " rho= "<< rho<< " tau= "<< tau<< " sigma= "<< sigma<<endl;
}

void reset_everything(){
  beta*=rho;

}

inline void compute_and_record_res(){
  if(nb_outer_iters%print_every_N_ADMM==0){
    compute_function_value();
    compute_infeasibility();
    cout<<setprecision(9)<<"Iteration: "<<nb_outer_iters<<"; time="<<running_time_ADMM<< "; function value="<<function_value<< "; infeasibility="<< infeas<< endl;
    samp_ADMM<<setprecision(9)<<nb_outer_iters<<" "<<running_time_ADMM<<" "<< function_value<<" "<< infeas<< endl;
  }
}

void ADMM_solve_with_Linear(D beta_0, D val_rho,vector<D> & x0,vector<D> & y0, vector<D> & lambda0, L max_nb_outer, L p_N_1, string filename1, D time){
  Initialize(beta_0,val_rho, x0, y0, lambda0);
  nb_outer_iters=0;
  //string sampname2= ALGparam.data_dir +"/results/L_Katyusha_"+filename2;
  //filename1= ALGparam.data_dir +"/results/ADMM_"+filename1;
  filename1= "results/ADMM_"+filename1;
  samp_ADMM.open(filename1.c_str());
  running_time_ADMM=0;
  print_every_N_ADMM=p_N_1;
  compute_and_record_res();
  D start;
  D res_x, res_y, res_l;
  /*
  for(L i=0;i<m_3;i++){
    old_lambda[i]=lambda[i];
  }
  */
  while(nb_outer_iters<max_nb_outer){
    //rescale();
    for(L i=0;i<m_1;i++){
      old_x[i]=x[i];
    }
    for(L i=0;i<m_3;i++){
      old_y[i]=y[i];
    }
    for(L i=0;i<m_3;i++){
      old_lambda[i]=lambda[i];
    }
    start = std::clock();
    update_x();
    update_y();
    update_lambda();
    nb_outer_iters++;
    running_time_ADMM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    compute_and_record_res();
    /*
    res_x= 0;
    for(L i=0;i<m_1;i++){
      res_x+= (old_x[i]- x[i])*(old_x[i]- x[i]);
    }
    res_y= 0;
    for(L i=0;i<m_3;i++){
      res_y= (old_y[i]- y[i])*(old_y[i]- y[i]);
    }
    res_l= 0;
    for(L i=0;i<m_3;i++){
      res_l= (old_lambda[i]- lambda[i])*(old_lambda[i]- lambda[i]);
    }
    cout<< "res_x= "<< res_x<< " res_y= "<< res_y<< " res_l= "<< res_l<< endl;
    system("pause");
    */
    start = std::clock();
    reset_everything();
    running_time_ADMM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    if (running_time_ADMM> time){
      break;
    }
  }

}
};

#endif /* MIN_SMOOTH_CONVEX_H */


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

/* Runge-Kutta 4th order, Classical */

/* Author:  G. Jungman
 */

/* Reference: Abramowitz & Stegun, section 25.5. equation 25.5.10

   Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
   L.R., Computer methods for ordinary differential and
   differential-algebraic equations, SIAM, Philadelphia, 1998.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *k;
  double *k1;
  double *y0;
  double *ytmp;
  double *y_onestep;
}
rk4_state_t;

static void *
rk4_alloc (size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) malloc (sizeof (rk4_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk4_state", GSL_ENOMEM);
    }

  state->k = (double *) malloc (dim * sizeof (double));

  if (state->k == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k", GSL_ENOMEM);
    }

  state->k1 = (double *) malloc (dim * sizeof (double));

  if (state->k1 == 0)
    {
      free (state->k);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->ytmp);
      free (state->y0);
      free (state->k);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rk4_step (double *y, const rk4_state_t *state,
	  const double h, const double t, const size_t dim,
	  const gsl_odeiv_system *sys)
{
  /* Makes a Runge-Kutta 4th order advance with step size h. */

  /* initial values of variables y. */
  const double *y0 = state->y0;

  /* work space */
  double *ytmp = state->ytmp;

  /* Runge-Kutta coefficients. Contains values of coefficient k1
     in the beginning
  */
  double *k = state->k;

  size_t i;

  /* k1 step */

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + 0.5 * h * k[i];
    }

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 3.0 * k[i];
      ytmp[i] = y0[i] + h * k[i];
    }

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    {
      y[i] += h / 6.0 * k[i];
    }

  return GSL_SUCCESS;
}

static int
rk4_apply (void *vstate,
           size_t dim,
           double t,
           double h,
           double y[],
           double yerr[],
           const double dydt_in[],
           double dydt_out[],
           const gsl_odeiv_system * sys)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  size_t i;

  double *const k = state->k;
  double *const k1 = state->k1;
  double *const y0 = state->y0;
  double *const y_onestep = state->y_onestep;

  DBL_MEMCPY (y0, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y0, k);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

  /* Error estimation is done by step doubling procedure */

  /* Save first point derivatives*/

  DBL_MEMCPY (k1, k, dim);

  /* First traverse h with one step (save to y_onestep) */

  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = rk4_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Then with two steps with half step length (save to y) */

  DBL_MEMCPY (k, k1, dim);

  {
    int s = rk4_step (y, state, h/2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, y0, dim);
	return s;
    }
  }

  /* Update before second step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h/2.0, y, k);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, y0, dim);
	return s;
      }
  }

  /* Save original y0 to k1 for possible failures */
  DBL_MEMCPY (k1, y0, dim);

  /* Update y0 for second step */
  DBL_MEMCPY (y0, y, dim);

  {
    int s = rk4_step (y, state, h/2.0, t + h/2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, k1, dim);
	return s;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL) {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

    if (s != GSL_SUCCESS)
      {
	/* Restore original values */
	DBL_MEMCPY (y, k1, dim);
	return s;
      }
  }

  /* Error estimation

     yerr = C * 0.5 * | y(onestep) - y(twosteps) | / (2^order - 1)

     constant C is approximately 8.0 to ensure 90% of samples lie within
     the error (assuming a gaussian distribution with prior p(sigma)=1/sigma.)

  */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 15.0;
    }

  return GSL_SUCCESS;
}

static int
rk4_reset (void *vstate, size_t dim)
{
  rk4_state_t *state = (rk4_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->k1, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y_onestep, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk4_order (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 4;
}

static void
rk4_free (void *vstate)
{
  rk4_state_t *state = (rk4_state_t *) vstate;
  free (state->k);
  free (state->k1);
  free (state->y0);
  free (state->ytmp);
  free (state->y_onestep);
  free (state);
}

static const gsl_odeiv_step_type rk4_type = { "rk4",    /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk4_alloc,
  &rk4_apply,
  &rk4_reset,
  &rk4_order,
  &rk4_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk4 = &rk4_type;