#include <math.h>
#include <stdlib.h>
#if !defined(__APPLE__)
#include <malloc.h>
#endif
#include <stdio.h>
#include <assert.h>
#include <time.h>
#include <string.h>

#include <fftw3.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

#include "../cosmolike_core_fast/theory/basics.c"
#include "../cosmolike_core_fast/theory/structs.c"
#include "../cosmolike_core_fast/theory/parameters.c"
#include "../cosmolike_core_fast/emu17/P_cb/emu.c"
#include "../cosmolike_core_fast/theory/recompute.c"
#include "../cosmolike_core_fast/theory/cosmo3D.c"
#include "../cosmolike_core_fast/theory/redshift_spline.c"
#include "../cosmolike_core_fast/theory/halo.c"
#include "../cosmolike_core_fast/theory/HOD.c"
#include "../cosmolike_core_fast/theory/pt.c"
#include "../cosmolike_core_fast/theory/cosmo2D_fourier.c"
#include "../cosmolike_core_fast/theory/IA.c"
// #include "../cosmolike_core_fast/theory/cluster.c"
#include "../cosmolike_core_fast/theory/BAO.c"
#include "../cosmolike_core_fast/theory/external_prior.c"
#include "../cosmolike_core_fast/theory/init_baryon.c"
#include "init_LSSxCMB.c"

// Naming convention:
// g = galaxy positions ("g" as in "galaxy")
// k = kappa CMB ("k" as in "kappa")
// s = kappa from source galaxies ("s" as in "shear")
// And alphabetical order

typedef double (*C_tomo_pointer)(double l, int n1, int n2);
void twopoint_via_hankel(double **xi, double *logthetamin, double *logthetamax, C_tomo_pointer C_tomo, int ni, int nj, int N_Bessel);

#include "../cosmolike_core_fast/theory/CMBxLSS_fourier.c"

typedef struct input_cosmo_params_local {
    double omega_m;
    double sigma_8;
    double n_s;
    double w0;
    double wa;
    double omega_b;
    double h0;
    double MGSigma;
    double MGmu;
} input_cosmo_params_local;

typedef struct input_nuisance_params_local {
    double bias[10];
    double source_z_bias[10];
    double source_z_s;
    double lens_z_bias[10];
    double lens_z_s;
    double shear_m[10];
    double A_ia;
    double beta_ia;
    double eta_ia;
    double eta_ia_highz;
    double lf[6];
    double m_lambda[6];
    double bary[3];
} input_nuisance_params_local;

double C_shear_tomo_sys(double ell,int z1,int z2);
// double C_cgl_tomo_sys(double ell_Cluster,int zl,int nN, int zs);
double C_gl_tomo_sys(double ell,int zl,int zs);
double C_ks_sys(double ell, int zs);
void set_data_shear(int Ncl, double *ell, double *data, int start);
void set_data_ggl(int Ncl, double *ell, double *data, int start);
void set_data_clustering(int Ncl, double *ell, double *data, int start);
void set_data_gk(double *ell, double *data, int start);
void set_data_ks(double *ell, double *data, int start);
void set_data_kk(double *ell, double *data, int start);
// void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3);
// double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3);
void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double Q1, double Q2, double Q3);
double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double Q1, double Q2, double Q3);

void write_datavector_wrapper(char *details, input_cosmo_params_local ic, input_nuisance_params_local in);
double log_like_wrapper(input_cosmo_params_local ic, input_nuisance_params_local in);
int get_N_tomo_shear(void);
int get_N_tomo_clustering(void);
int get_N_ggl(void);
int get_N_ell(void);

int get_N_tomo_shear(void){
  return tomo.shear_Nbin;
}
int get_N_tomo_clustering(void){
  return tomo.clustering_Nbin;
}
int get_N_ggl(void){
  return tomo.ggl_Npowerspectra;
}
int get_N_ell(void){
  return like.Ncl;
}

double C_shear_tomo_sys(double ell, int z1, int z2)
{
  double C;
  // C= C_shear_tomo_nointerp(ell,z1,z2);
  // if(like.IA==1) C+=C_II_nointerp(ell,z1,z2)+C_GI_nointerp(ell,z1,z2);

  if(like.IA!=1) C= C_shear_tomo_nointerp(ell,z1,z2);
  //if(like.IA==1) C= C_shear_shear_IA(ell,z1,z2);
  // clock_t t1, t2; t1=clock();
  // if(like.IA==1) C = C_shear_tomo_nointerp(ell,z1,z2)+C_II_nointerp(ell,z1,z2)+C_GI_nointerp(ell,z1,z2);
  if(like.IA==1) C = C_shear_tomo_nointerp_IA1(ell,z1,z2);
  // t2 = clock(); printf("shear %d-%d: %le\n",z1,z2, (double)(t2-t1)/CLOCKS_PER_SEC);
  if(like.IA==2) C += C_II_lin_nointerp(ell,z1,z2)+C_GI_lin_nointerp(ell,z1,z2);
  if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[z1])*(1.0+nuisance.shear_calibration_m[z2]);
  //printf("%le %d %d %le\n",ell,z1,z2,C_shear_tomo_nointerp(ell,z1,z2)+C_II_JB_nointerp(ell,z1,z2)+C_GI_JB_nointerp(ell,z1,z2));
return C;
}

double C_gl_tomo_sys(double ell,int zl,int zs)
{
  double C;
  // C=C_gl_tomo_nointerp(ell,zl,zs);
  // if(like.IA==1) C += C_gI_nointerp(ell,zl,zs);

  if(like.IA!=1) C=C_gl_tomo_nointerp(ell,zl,zs);
  if(like.IA==1) C = C_ggl_IA(ell,zl,zs);
  if(like.IA==2) C += C_gI_lin_nointerp(ell,zl,zs);
  if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[zs]);
return C;
}

// double C_cgl_tomo_sys(double ell_Cluster, int zl,int nN, int zs)
// {
//   double C;
//   C=C_cgl_tomo_nointerp(ell_Cluster,zl,nN,zs);
//   //if(like.IA!=0) C +=
//   if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[zs]);
// return C;
// }

double C_ks_sys(double ell, int zs)
{
   double C;
   C = C_ks_nointerp(ell,zs);
   if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[zs]);
   return C;
}

void set_data_shear(int Ncl, double *ell, double *data, int start)
{
  int i,z1,z2,nz;
  double a;
  for (nz = 0; nz < tomo.shear_Npowerspectra; nz++){
    z1 = Z1(nz); z2 = Z2(nz);
    for (i = 0; i < Ncl; i++){
      if (ell[i] < like.lmax_shear){ data[Ncl*nz+i] = C_shear_tomo_sys(ell[i],z1,z2);}
      else {data[Ncl*nz+i] = 0.;}
    }
  }
}

void set_data_ggl(int Ncl, double *ell, double *data, int start)
{
  int i, zl,zs,nz;
  for (nz = 0; nz < tomo.ggl_Npowerspectra; nz++){
    zl = ZL(nz); zs = ZS(nz);
#ifdef ONESAMPLE
    if(zl==0){
      for (i = 0; i < Ncl; i++){data[start+(Ncl*nz)+i] = 0.;}
    }
    else{
#endif
      for (i = 0; i < Ncl; i++){
        if (test_kmax(ell[i],zl)){
          data[start+(Ncl*nz)+i] = C_gl_tomo_sys(ell[i],zl,zs);
        }
        else{
          data[start+(Ncl*nz)+i] = 0.;
        }
      }
#ifdef ONESAMPLE
    }
#endif
  }
}

void set_data_clustering(int Ncl, double *ell, double *data, int start){
  int i, nz;
  for (nz = 0; nz < tomo.clustering_Npowerspectra; nz++){
    //printf("%d %e %e\n",nz, gbias.b[nz][1],pf_photoz(gbias.b[nz][1],nz));
    for (i = 0; i < Ncl; i++){
      if (test_kmax(ell[i],nz)){data[start+(Ncl*nz)+i] = C_cl_tomo_nointerp(ell[i],nz,nz);}
      else{data[start+(Ncl*nz)+i] = 0.;}
      //printf("%d %d %le %le\n",nz,nz,ell[i],data[Ncl*(tomo.shear_Npowerspectra+tomo.ggl_Npowerspectra + nz)+i]);
    }
  }
}

void set_data_gk(double *ell, double *data, int start)
{
#ifdef ONESAMPLE
  int nz=0;
  for (int i=0; i<like.Ncl; i++) {data[start+(like.Ncl*nz)+i] = 0.;}
  for (nz=1; nz<tomo.clustering_Nbin; nz++){
#else
  for (int nz=0; nz<tomo.clustering_Nbin; nz++){
#endif
    for (int i=0; i<like.Ncl; i++){
       if (ell[i]<like.lmax_kappacmb && test_kmax(ell[i],nz)){
          data[start+(like.Ncl*nz)+i] = C_gk_nointerp(ell[i],nz);
       }
       else{
          data[start+(like.Ncl*nz)+i] = 0.;
       }
    }
  }
}

void set_data_ks(double *ell, double *data, int start)
{
   for (int nz=0; nz<tomo.shear_Nbin; nz++){
      for (int i=0; i<like.Ncl; i++){
         if (ell[i]<like.lmax_kappacmb) {
            data[start+(like.Ncl*nz)+i] = C_ks_sys(ell[i],nz);
         }
         else{
            data[start+(like.Ncl*nz)+i] = 0.;
         }
      }
   }
}

void set_data_kk(double *ell, double *data, int start)
{
   for (int i=0; i<like.Ncl; i++){
      if (ell[i]<like.lmax_kappacmb){
         data[start+i] = C_kk_nointerp(ell[i]);
      }
      else{
         data[start+i] = 0.;
      }
   }
}

int set_cosmology_params(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu)
{
  cosmology.Omega_m=OMM;
  cosmology.Omega_v= 1.0-cosmology.Omega_m;
  cosmology.sigma_8=S8;
  cosmology.n_spec= NS;
  cosmology.w0=W0;
  cosmology.wa=WA;
  cosmology.omb=OMB;
  cosmology.h0=H0;
  cosmology.MGSigma=MGSigma;
  cosmology.MGmu=MGmu;

  if (cosmology.Omega_m < 0.1 || cosmology.Omega_m > 0.6) return 0;
  if (cosmology.omb < 0.04 || cosmology.omb > 0.055) return 0;
  if (cosmology.sigma_8 < 0.5 || cosmology.sigma_8 > 1.1) return 0;
  if (cosmology.n_spec < 0.85 || cosmology.n_spec > 1.05) return 0;
  if (cosmology.w0 < -2.0 || cosmology.w0 > -0.0) return 0;
  if (cosmology.wa < -2.5 || cosmology.wa > 2.5) return 0;
  if (cosmology.h0 < 0.4 || cosmology.h0 > 0.9) return 0;
  if (cosmology.MGmu < -3.0 || cosmology.MGmu > 3.0) return 0;
  if (cosmology.MGSigma < -3.0 || cosmology.MGSigma > 3.0) return 0; // DESY1 extension paper flat priors
  //CH BEGINS
  //CH: to use for running planck15_BA0_w0_wa prior alone)
  //printf("like_fourier.c from WFIRST_forecasts: cosmology bounds set for running with planck15_BA0_w0_wa prior\n");
  //if (cosmology.Omega_m < 0.05 || cosmology.Omega_m > 0.6) return 0;
  //if (cosmology.omb < 0.01 || cosmology.omb > 0.1) return 0;
  //if (cosmology.sigma_8 < 0.5 || cosmology.sigma_8 > 1.1) return 0;
  //if (cosmology.n_spec < 0.84 || cosmology.n_spec > 1.06) return 0;
  //if (cosmology.w0 < -2.1 || cosmology.w0 > 1.5) return 0;
  //if (cosmology.wa < -5.0 || cosmology.wa > 2.6) return 0;
  //if (cosmology.h0 < 0.3 || cosmology.h0 > 0.9) return 0;
  //CH ENDS
  return 1;
}

void set_nuisance_shear_calib(double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10)
{
  nuisance.shear_calibration_m[0] = M1;
  nuisance.shear_calibration_m[1] = M2;
  nuisance.shear_calibration_m[2] = M3;
  nuisance.shear_calibration_m[3] = M4;
  nuisance.shear_calibration_m[4] = M5;
  nuisance.shear_calibration_m[5] = M6;
  nuisance.shear_calibration_m[6] = M7;
  nuisance.shear_calibration_m[7] = M8;
  nuisance.shear_calibration_m[8] = M9;
  nuisance.shear_calibration_m[9] = M10;
}

int set_nuisance_shear_photoz(double SP1,double SP2,double SP3,double SP4,double SP5,double SP6,double SP7,double SP8,double SP9,double SP10,double SPS1)
{
  int i;
  nuisance.bias_zphot_shear[0]=SP1;
  nuisance.bias_zphot_shear[1]=SP2;
  nuisance.bias_zphot_shear[2]=SP3;
  nuisance.bias_zphot_shear[3]=SP4;
  nuisance.bias_zphot_shear[4]=SP5;
  nuisance.bias_zphot_shear[5]=SP6;
  nuisance.bias_zphot_shear[6]=SP7;
  nuisance.bias_zphot_shear[7]=SP8;
  nuisance.bias_zphot_shear[8]=SP9;
  nuisance.bias_zphot_shear[9]=SP10;

  for (i=0;i<tomo.shear_Nbin; i++){
    nuisance.sigma_zphot_shear[i]=SPS1;
    if (nuisance.sigma_zphot_shear[i]<0.0001) return 0;
  }
  return 1;
}

int set_nuisance_clustering_photoz(double CP1,double CP2,double CP3,double CP4,double CP5,double CP6,double CP7,double CP8,double CP9,double CP10,double CPS1)
{
  int i;
  nuisance.bias_zphot_clustering[0]=CP1;
  nuisance.bias_zphot_clustering[1]=CP2;
  nuisance.bias_zphot_clustering[2]=CP3;
  nuisance.bias_zphot_clustering[3]=CP4;
  nuisance.bias_zphot_clustering[4]=CP5;
  nuisance.bias_zphot_clustering[5]=CP6;
  nuisance.bias_zphot_clustering[6]=CP7;
  nuisance.bias_zphot_clustering[7]=CP8;
  nuisance.bias_zphot_clustering[8]=CP9;
  nuisance.bias_zphot_clustering[9]=CP10;

  for (i=0;i<tomo.clustering_Nbin; i++){
    nuisance.sigma_zphot_clustering[i]=CPS1;
    if (nuisance.sigma_zphot_clustering[i]<0.0001) return 0;
  }
  return 1;
}

int set_nuisance_ia(double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q)
{
  nuisance.A_ia=A_ia;
  nuisance.beta_ia=beta_ia;
  nuisance.eta_ia=eta_ia;
  nuisance.eta_ia_highz=eta_ia_highz;
  nuisance.LF_alpha=LF_alpha;
  nuisance.LF_P=LF_P;
  nuisance.LF_Q=LF_Q;
  nuisance.LF_red_alpha=LF_red_alpha;
  nuisance.LF_red_P=LF_red_P;
  nuisance.LF_red_Q=LF_red_Q;
  if (nuisance.A_ia < 0.0 || nuisance.A_ia > 10.0) return 0;
  if (nuisance.beta_ia < -4.0 || nuisance.beta_ia > 6.0) return 0;
  if (nuisance.eta_ia < -10.0 || nuisance.eta_ia> 10.0) return 0;
  if (nuisance.eta_ia_highz < -1.0 || nuisance.eta_ia_highz> 1.0) return 0;
  // if(like.IA!=0){
  //  if (check_LF()) return 0;
  // }
return 1;
}

// int set_nuisance_cluster_Mobs(double cluster_Mobs_lgN0,  double cluster_Mobs_alpha, double cluster_Mobs_beta, double cluster_Mobs_sigma0, double cluster_Mobs_sigma_qm, double cluster_Mobs_sigma_qz)
// {
//   //  nuisance.cluster_Mobs_lgM0 = mass_obs_norm;  //fiducial : 1.72+log(1.e+14*0.7); could use e.g. sigma = 0.2 Gaussian prior
//   //  nuisance.cluster_Mobs_alpha = mass_obs_slope; //fiducial: 1.08; e.g. sigma = 0.1 Gaussian prior
//   //  nuisance.cluster_Mobs_beta = mass_z_slope; //fiducial: 0.0; e.g. sigma = 0.1 Gaussian prior
//   //  nuisance.cluster_Mobs_sigma = mass_obs_scatter; //fiducial 0.25; e.g. sigma = 0.05 Gaussian prior

//   // fiducial values and priors from Murata et al. (2018) except for redshift-related parameters
//   nuisance.cluster_Mobs_lgN0 = cluster_Mobs_lgN0; //fiducial: 3.207, flat prior [0.5, 5.0]
//   nuisance.cluster_Mobs_alpha = cluster_Mobs_alpha; //fiducial: 0.993, flat prior [0.0, 2.0]
//   nuisance.cluster_Mobs_beta = cluster_Mobs_beta; //fiducial: 0.0, flat prior [-1.5, 1.5]
//   nuisance.cluster_Mobs_sigma0 = cluster_Mobs_sigma0; //fiducial: 0.456, flat prior [0.0, 1.5]
//   nuisance.cluster_Mobs_sigma_qm = cluster_Mobs_sigma_qm; //fiducial: -0.169, flat prior [-1.5, 1.5]
//   nuisance.cluster_Mobs_sigma_qz = cluster_Mobs_sigma_qz; //fiducial: 0.0, flat prior [-1.5, 1.5]

//   if (nuisance.cluster_Mobs_lgN0 < 0.5 || nuisance.cluster_Mobs_lgN0 > 5.0) return 0;
//   if (nuisance.cluster_Mobs_alpha < 0.0 || nuisance.cluster_Mobs_alpha > 2.0) return 0;
//   if (nuisance.cluster_Mobs_beta < -1.5 || nuisance.cluster_Mobs_beta > 1.5) return 0;
//   if (nuisance.cluster_Mobs_sigma0 < 0.0|| nuisance.cluster_Mobs_sigma0 > 1.5) return 0;
//   if (nuisance.cluster_Mobs_sigma_qm < -1.5 && nuisance.cluster_Mobs_sigma_qm > 1.5) return 0;
//   if (nuisance.cluster_Mobs_sigma_qz < -1.5 && nuisance.cluster_Mobs_sigma_qz > 1.5)return 0;

// return 1;
// }

int set_nuisance_gbias(double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8,double B9, double B10)
{

  int i;
  gbias.b[0] = B1;
  gbias.b[1] = B2;
  gbias.b[2] = B3;
  gbias.b[3] = B4;
  gbias.b[4] = B5;
  gbias.b[5] = B6;
  gbias.b[6] = B7;
  gbias.b[7] = B8;
  gbias.b[8] = B9;
  gbias.b[9] = B10;
  for (i = 0; i < 10; i++){
  //    printf("in set routine %d %le\n",i,gbias.b[i]);
#ifdef ONESAMPLE
    if (gbias.b[i] < 0.4 || gbias.b[i] > 5.0) return 0;
#else
    if (gbias.b[i] < 0.4 || gbias.b[i] > 3.0) return 0;
#endif
  }

  return 1;
}

// double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3)
double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double Q1, double Q2, double Q3)
{
  int i,j,k,m=0,l;
  static double *pred;
  static double *ell;
  static double *ell_Cluster;
  static double darg;
  double chisqr,a,log_L_prior=0.0, log_L=0.0;;

  if(ell==0){
    pred= create_double_vector(0, like.Ndata-1);
    ell= create_double_vector(0, like.Ncl-1);
    darg=(log(like.lmax)-log(like.lmin))/like.Ncl;
    for (l=0;l<like.Ncl;l++){
      ell[l]=exp(log(like.lmin)+(l+0.5)*darg);
    }
    // ell_Cluster= create_double_vector(0, Cluster.lbin-1);
    // darg=(log(Cluster.l_max)-log(Cluster.l_min))/Cluster.lbin;
    // for (l=0;l<Cluster.lbin;l++){
    //   ell_Cluster[l]=exp(log(Cluster.l_min)+(l+0.5)*darg);
    // }
  }
  if (set_cosmology_params(OMM,S8,NS,W0,WA,OMB,H0,MGSigma,MGmu)==0){
    printf("Cosmology out of bounds\n");
    return -1.0e15;
  }
  set_nuisance_shear_calib(M1,M2,M3,M4,M5,M6,M7,M8,M9,M10);
  if (set_nuisance_shear_photoz(SP1,SP2,SP3,SP4,SP5,SP6,SP7,SP8,SP9,SP10,SPS1)==0){
    printf("Shear photo-z sigma too small\n");
    return -1.0e15;
  }
  if (set_nuisance_clustering_photoz(CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10,CPS1)==0){
    printf("Clustering photo-z sigma too small\n");
    return -1.0e15;
  }
  if (set_nuisance_ia(A_ia,beta_ia,eta_ia,eta_ia_highz,LF_alpha,LF_P,LF_Q,LF_red_alpha,LF_red_P,LF_red_Q)==0){
    printf("IA parameters out of bounds\n");
    return -1.0e15;
  }
  if (set_nuisance_gbias(B1,B2,B3,B4,B5,B6,B7,B8,B9,B10)==0){
    printf("Bias out of bounds\n");
    return -1.0e15;
  }
  // if (set_nuisance_cluster_Mobs(mass_obs_norm, mass_obs_slope, mass_z_slope, mass_obs_scatter_norm, mass_obs_scatter_mass_slope, mass_obs_scatter_z_slope)==0){
  //   printf("Mobs out of bounds\n");
  //   return -1.0e15;
  // }

  printf("like %le %le %le %le %le %le %le %le\n",cosmology.Omega_m, cosmology.Omega_v,cosmology.sigma_8,cosmology.n_spec,cosmology.w0,cosmology.wa,cosmology.omb,cosmology.h0);
  // printf("like %le %le %le %le\n",gbias.b[0][0], gbias.b[1][0], gbias.b[2][0], gbias.b[3][0]);
  // for (i=0; i<10; i++){
  //   printf("nuisance %le %le %le\n",nuisance.shear_calibration_m[i],nuisance.bias_zphot_shear[i],nuisance.sigma_zphot_shear[i]);
  // }

  log_L_prior=0.0;
  // if(like.Aubourg_Planck_BAO_SN==1) log_L_prior+=log_L_Planck_BAO_SN();
  // if(like.SN==1) log_L_prior+=log_L_SN();
  //if(like.BAO==1) log_L_prior+=log_L_BAO();
  // if(like.Planck==1) log_L_prior+=log_L_Planck();
  // if(like.Planck15_BAO_w0wa==1) log_L_prior+=log_L_Planck15_BAO_w0wa();//CH
  //if(like.Planck15_BAO_H070p6_JLA_w0wa==1) log_L_prior+=log_L_Planck15_BAO_H070p6_JLA_w0wa();//CH
  // if(like.IA!=0) log_L_prior+=log_L_ia();
  // if(like.IA!=0) log_L_prior+=log_like_f_red();
  if(like.wlphotoz!=0) log_L_prior+=log_L_wlphotoz();
  if(like.clphotoz!=0) log_L_prior+=log_L_clphotoz();
  if(like.shearcalib==1) log_L_prior+=log_L_shear_calib();
  if(like.IA!=0) {
    log_L = 0.0;
    log_L -= pow((nuisance.A_ia - prior.A_ia[0])/prior.A_ia[1],2.0);
    log_L -= pow((nuisance.beta_ia - prior.beta_ia[0])/prior.beta_ia[1],2.0);
    log_L -= pow((nuisance.eta_ia - prior.eta_ia[0])/prior.eta_ia[1],2.0);
    log_L -= pow((nuisance.eta_ia_highz - prior.eta_ia_highz[0])/prior.eta_ia_highz[1],2.0);
    log_L_prior+=0.5*log_L;
  }
  if(like.baryons==1){;
    log_L = 0.0;
    log_L -= pow((Q1 - prior.bary_Q1[0])/prior.bary_Q1[1],2.0);
    log_L -= pow((Q2 - prior.bary_Q2[0])/prior.bary_Q2[1],2.0);
    log_L -= pow((Q3 - prior.bary_Q3[0])/prior.bary_Q3[1],2.0);
    log_L_prior+=0.5*log_L;
  }

  // if(like.clusterMobs==1) log_L_prior+=log_L_clusterMobs();

  // printf("%d %d %d %d\n",like.BAO,like.wlphotoz,like.clphotoz,like.shearcalib);
  // printf("logl %le %le %le %le\n",log_L_shear_calib(),log_L_wlphotoz(),log_L_clphotoz(),log_L_clusterMobs());
  int start=0;
  // clock_t t1, t2;
  // t1 = clock();
  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  // t2 = clock(); printf("shear: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.shear_pos==1){
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  // t2 = clock(); printf("ggl: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.pos_pos==1){
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }
  // t2 = clock(); printf("gg: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.gk==1) {
    set_data_gk(ell, pred, start);
    start += like.Ncl*tomo.clustering_Nbin;
  }
  // t2 = clock(); printf("gk: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.ks==1) {
    set_data_ks(ell, pred, start);
    start += like.Ncl*tomo.shear_Nbin;
  }
  // t2 = clock(); printf("ks: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.kk==1) {
    set_data_kk(ell, pred, start);
    start += like.Ncl;
  }
  // t2 = clock(); printf("kk: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  chisqr=0.0;
  for (i=0; i<like.Ndata; i++){
    for (j=0; j<like.Ndata; j++){
      a=(pred[i]-data_read(1,i)+Q1*bary_read(1,0,i)+Q2*bary_read(1,1,i)+Q3*bary_read(1,2,i))*invcov_read(1,i,j)*(pred[j]-data_read(1,j)+Q1*bary_read(1,0,j)+Q2*bary_read(1,1,j)+Q3*bary_read(1,2,j));
      //a=(pred[i]-data_read(1,i))*invcov_read(1,i,j)*(pred[j]-data_read(1,j));
      chisqr=chisqr+a;
    }
    // if (fabs(data_read(1,i)) < 1.e-25){
    //    printf("%d %le %le %le\n",i,data_read(1,i),pred[i],invcov_read(1,i,i));
    // }
  }
  // t2 = clock(); printf("chisqr: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if (chisqr<0.0){
    printf("error: chisqr = %le\n",chisqr);
    //exit(EXIT_FAILURE);
  }
  printf("%le\n",chisqr);
  return -0.5*chisqr+log_L_prior;
}

// void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5,double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3)
void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5,double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double Q1, double Q2, double Q3)
{

  int i,j,k,m=0,l;
  static double *pred;
  static double *ell;
  // static double *ell_Cluster;
  static double darg;
  double chisqr,a,log_L_prior=0.0;

  if(ell==0){
    pred= create_double_vector(0, like.Ndata-1);
    ell= create_double_vector(0, like.Ncl-1);
    darg=(log(like.lmax)-log(like.lmin))/like.Ncl;
    for (l=0;l<like.Ncl;l++){
      ell[l]=exp(log(like.lmin)+(l+0.5)*darg);
    }
    // ell_Cluster= create_double_vector(0, Cluster.lbin-1);
    // darg=(log(Cluster.l_max)-log(Cluster.l_min))/Cluster.lbin;
    // for (l=0;l<Cluster.lbin;l++){
    //   ell_Cluster[l]=exp(log(Cluster.l_min)+(l+0.5)*darg);
    // }
  }
// for (l=0;l<like.Ncl;l++){
//   printf("%d %le\n",i,ell[l]);
// }
  // clock_t t1, t2;

  // t1 = clock();
  set_cosmology_params(OMM,S8,NS,W0,WA,OMB,H0,MGSigma,MGmu);
  set_nuisance_shear_calib(M1,M2,M3,M4,M5,M6,M7,M8,M9,M10);
  set_nuisance_shear_photoz(SP1,SP2,SP3,SP4,SP5,SP6,SP7,SP8,SP9,SP10,SPS1);
  set_nuisance_clustering_photoz(CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10,CPS1);
  set_nuisance_ia(A_ia,beta_ia,eta_ia,eta_ia_highz,LF_alpha,LF_P,LF_Q,LF_red_alpha,LF_red_P,LF_red_Q);
  set_nuisance_gbias(B1,B2,B3,B4,B5,B6,B7,B8,B9,B10);
  // set_nuisance_cluster_Mobs(mass_obs_norm,mass_obs_slope,mass_z_slope,mass_obs_scatter_norm,mass_obs_scatter_mass_slope,mass_obs_scatter_z_slope);

  // t2 = clock(); printf("setting: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  int start=0;
  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  // t2 = clock(); printf("shear: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.shear_pos==1){
    //printf("ggl\n");
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  // t2 = clock(); printf("ggl: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.pos_pos==1){
    //printf("clustering\n");
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }
  // t2 = clock(); printf("gg: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;

  if(like.gk==1) {
    printf("Computing data vector: gk\n");
    set_data_gk(ell, pred, start);
    start += like.Ncl * tomo.clustering_Nbin;
  }
  // t2 = clock(); printf("gk: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if(like.ks==1) {
    printf("Computing data vector: ks\n");
    set_data_ks(ell, pred, start);
    start += like.Ncl * tomo.shear_Nbin;
  }
  // t2 = clock(); printf("ks: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;
  if (like.kk) {
    printf("Computing data vector: kk\n");
    set_data_kk(ell, pred, start);
    start += like.Ncl;
  }
  // t2 = clock(); printf("kk: %le\n", (double)(t2-t1)/CLOCKS_PER_SEC); t1 = t2;

  FILE *F;
  char filename[300];
  if (strstr(details,"FM") != NULL){
    sprintf(filename,"%s",details);
  }
  else {sprintf(filename,"datav/%s_%s",like.probes,details);}
  F=fopen(filename,"w");
  for (i=0;i<like.Ndata; i++){
    fprintf(F,"%d %le\n",i,pred[i]);
    //printf("%d %le\n",i,pred[i]);
  }
  fclose(F);
  // printf("&gbias.b1_function %p\n",&gbias.b1_function);
  // printf("gbias.b1_function  %p\n",gbias.b1_function);
  // printf("bgal_z   %p\n",bgal_z);
  // printf("&bgal_z  %p\n",&bgal_z);
  // printf("b1_per_bin   %p\n",b1_per_bin);
  // printf("&b1_per_bin  %p\n",&b1_per_bin);

}

void write_datavector_wrapper(char *details, input_cosmo_params_local ic, input_nuisance_params_local in)
{
  // compute_data_vector(details, ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
  //   in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
  //   in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
  //   in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
  //   in.source_z_s,
  //   in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
  //   in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
  //   in.lens_z_s,
  //   in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
  //   in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
  //   in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
  //   in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
  //   in.m_lambda[0], in.m_lambda[1], in.m_lambda[2], in.m_lambda[3],
  //   in.m_lambda[4], in.m_lambda[5],in.bary[0], in.bary[1], in.bary[2]);
  compute_data_vector(details, ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
    in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
    in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
    in.lens_z_s,
    in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
    in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
    in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
    in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
    in.bary[0], in.bary[1], in.bary[2]);
}

double log_like_wrapper(input_cosmo_params_local ic, input_nuisance_params_local in)
{
  // double like = log_multi_like(ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
  //   in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
  //   in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
  //   in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
  //   in.source_z_s,
  //   in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
  //   in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
  //   in.lens_z_s,
  //   in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
  //   in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
  //   in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
  //   in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
  //   in.m_lambda[0], in.m_lambda[1], in.m_lambda[2], in.m_lambda[3],
  //   in.m_lambda[4], in.m_lambda[5],in.bary[0], in.bary[1], in.bary[2]);
  double like = log_multi_like(ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
    in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
    in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
    in.lens_z_s,
    in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
    in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
    in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
    in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
    in.bary[0], in.bary[1], in.bary[2]);
  return like;
}

double log_like_wrapper_1sample(input_cosmo_params_local ic, input_nuisance_params_local in) // Only used in sampling, not in datav
{
  double like = log_multi_like(ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
    in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
    in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
    in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
    in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
    in.bary[0], in.bary[1], in.bary[2]);
  return like;
}

void save_zdistr_sources(int zs){
  double z,dz =(redshift.shear_zdistrpar_zmax-redshift.shear_zdistrpar_zmin)/300.0;
  printf("Printing redshift distribution n(z) for source redshift bin %d\n",zs);

   FILE *F1;
   char filename[300];
   sprintf(filename,"zdistris/zdist_sources_bin%d.txt",zs);
   F1 = fopen(filename,"w");
   for (z =redshift.shear_zdistrpar_zmin; z< redshift.shear_zdistrpar_zmax; z+= dz){
      fprintf(F1,"%e %e\n", z, zdistr_photoz(z,zs));
   }
}

void save_zdistr_lenses(int zl){
   double z,dz =(redshift.clustering_zdistrpar_zmax-redshift.clustering_zdistrpar_zmin)/300.0;
  printf("Printing redshift distribution n(z) and bias b(z) for lens redshift bin %d\n",zl);

   FILE *F1;
   char filename[300];
   sprintf(filename,"zdistris/zdist_lenses_bin%d.txt", zl);
   F1 = fopen(filename,"w");
   for (z =redshift.clustering_zdistrpar_zmin; z< redshift.clustering_zdistrpar_zmax; z+= dz){
      fprintf(F1,"%e %e\n", z, pf_photoz(z,zl));
   }
}

 int main(int argc, char** argv)
{
  int i;
  char arg1[400],arg2[400],arg3[400];
/* here, do your time-consuming job */
  int sce=atoi(argv[1]);

  int N_scenarios=2;

  double sigma_zphot_shear[3]={0.05,0.05};
  double sigma_zphot_clustering[3]={0.03,0.03};

  double area_table[2]={12300.0,16500.0}; // Y1 corresponds to DESC SRD Y1, Y6 corresponds to assuming that we cover the full SO area=0.4*fsky and at a depth of 26.1 which is in a range of reasonable scenarios (see https://github.com/LSSTDESC/ObsStrat/tree/static/static )
  double nsource_table[2]={11.0,23.0};
  double nlens_table[2]={18.0,41.0};

  char survey_designation[2][200]={"LSSTxSO_Y1","LSSTxSO_Y6"};
  char tomo_binning_source[2][200]={"source_std","source_std"};
  // even for lens=src, this setting is valid, because the input lens/src zfiles are generated with the same z-cut:(0.2,1.2)

  char source_zfile[2][400]={"src_LSSTY1ext_2-island","src_LSSTY6ext_2-island"};
#ifdef ONESAMPLE
  char lens_zfile[2][400]={"src_LSSTY1ext_2-island","src_LSSTY6ext_2-island"};
  char tomo_binning_lens[2][200]={"lens=src","lens=src"};
  nlens_table[0] = nsource_table[0];
  nlens_table[1] = nsource_table[1];
  sigma_zphot_clustering[0] = sigma_zphot_shear[0];
  sigma_zphot_clustering[1] = sigma_zphot_shear[1];
#else
  char lens_zfile[2][400]={"lens_LSSTY1ext_2-island", "lens_LSSTY6ext_2-island"};
  char tomo_binning_lens[2][200]={"LSST_gold","LSST_gold"};
#endif
  char cmb_yr[2][100]={"so_Y1","so_Y5"};

  init_cosmo_runmode("halofit");
  init_bary(argv[2]);
  init_binning_fourier(15,20.0,3000.0,3000.0,21.0,10,10);

  init_priors(0.002,sigma_zphot_shear[sce],0.001,0.001,sigma_zphot_clustering[sce],0.001,0.001,3.0,1.2,3.8,2.0,16.0,5.0,0.8);
  init_survey(survey_designation[sce],nsource_table[sce],nlens_table[sce],area_table[sce]);
  sprintf(arg1,"zdistris/%s",source_zfile[sce]);
  sprintf(arg2,"zdistris/%s",lens_zfile[sce]);
  init_galaxies(arg1,arg2,"outlier_sim","outlier_sim",tomo_binning_source[sce],tomo_binning_lens[sce]);
  init_IA("NLA_HF","GAMA");
  init_probes("6x2pt");
  init_cmb(cmb_yr[sce]);

#ifdef ONESAMPLE
  sprintf(arg3,"%s_1sample_%s_outext_2-island",survey_designation[sce],argv[2]);
#else
  sprintf(arg3,"%s_%s_outext_2-island",survey_designation[sce],argv[2]);
#endif
  compute_data_vector(arg3,0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,\
    gbias.b[0],gbias.b[1],gbias.b[2],gbias.b[3],gbias.b[4],\
    gbias.b[5],gbias.b[6],gbias.b[7],gbias.b[8],gbias.b[9],\
    0.0,0.0,0.0,0.0,0.0,\
    0.0,0.0,0.0,0.0,0.0,\
    sigma_zphot_shear[sce],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,sigma_zphot_clustering[sce],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);

  return 0;
}


/*******************************************************************************
*
*       This file is part of the General Hidden Markov Model Library,
*       GHMM version __VERSION__, see http://ghmm.org
*
*       Filename: ghmm/ghmm/matrixop.c
*       Authors:  Christoph Hafemeister
*
*       Copyright (C) 2007-2008 Alexander Schliep
*	Copyright (C) 2007-2008 Max-Planck-Institut fuer Molekulare Genetik,
*                               Berlin
*
*       Contact: schliep@ghmm.org
*
*       This library is free software; you can redistribute it and/or
*       modify it under the terms of the GNU Library General Public
*       License as published by the Free Software Foundation; either
*       version 2 of the License, or (at your option) any later version.
*
*       This library 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
*       Library General Public License for more details.
*
*       You should have received a copy of the GNU Library General Public
*       License along with this library; if not, write to the Free
*       Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*******************************************************************************/

#ifdef HAVE_CONFIG_H
#  include "../config.h"
#endif

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

#include "matrixop.h"

#ifdef DO_WITH_GSL
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#endif

#ifdef DO_WITH_ATLAS
#include <clapack.h>
#endif

/*============================================================================*/
int ighmm_invert_det(double *sigmainv, double *det, int length, double *cov)
{
#define CUR_PROC "invert_det"
#ifdef DO_WITH_GSL
  int i, j, s;
  gsl_matrix *tmp;
  gsl_matrix *inv;
  tmp = gsl_matrix_alloc(length, length);
  inv = gsl_matrix_alloc(length, length);
  gsl_permutation *permutation = gsl_permutation_calloc(length);

  for (i=0; i<length; ++i) {
    for (j=0; j<length; ++j) {
#ifdef DO_WITH_GSL_DIAGONAL_HACK
        if (i == j){
          gsl_matrix_set(tmp, i, j, cov[i*length+j]);
        }else{
          gsl_matrix_set(tmp, i, j, 0.0);
        }
#else
        gsl_matrix_set(tmp, i, j, cov[i*length+j]);
#endif
    }
  }

  gsl_linalg_LU_decomp(tmp, permutation, &s);
  gsl_linalg_LU_invert(tmp, permutation, inv);
  *det = gsl_linalg_LU_det(tmp, s);
  gsl_matrix_free(tmp);
  gsl_permutation_free(permutation);

  for (i=0; i<length; ++i) {
    for (j=0; j<length; ++j) {
       sigmainv[i*length+j] = gsl_matrix_get(inv, i, j);
    }
  }

  gsl_matrix_free(inv);
#elif defined HAVE_CLAPACK_DGETRF && HAVE_CLAPACK_DGETRI
  char sign;
  int info, i;
  int *ipiv;
  double det_tmp;

  ipiv = malloc(length * sizeof *ipiv);

  /* copy cov. matrix entries to result matrix, the rest is done in-place */
  memcpy(sigmainv, cov, length * length * sizeof *cov);

  /* perform in-place LU factorization of covariance matrix */
  info = clapack_dgetrf(CblasRowMajor, length, length, sigmainv, length, ipiv);

  /* determinant */
  sign = 1;
  for( i=0; i<length; ++i)
    if( ipiv[i]!=i )
      sign *= -1;
  det_tmp = sigmainv[0];
  for( i=length+1; i<(length*length); i+=length+1 )
    det_tmp *= sigmainv[i];
  *det = det_tmp * sign;

  /* use the LU factorization to get inverse */
  info = clapack_dgetri(CblasRowMajor, length, sigmainv, length, ipiv);

  free(ipiv);
#else
  *det = ighmm_determinant(cov, length);
  ighmm_inverse(cov, length, *det, sigmainv);
#endif

  return 0;
#undef CUR_PROC
}

/*============================================================================*/
/* calculate determinant of a square matrix */
double ighmm_determinant(double *cov, int n)
{
  int j1, i, j, jm;
  double * m;
  double det;

  if (n == 1)
    return cov[0];
  if (n == 2)
    return cov[0] * cov[3] - cov[1] * cov[2];
  /* matrix dimension is bigger than 2 - we have to do some work */
  det = 0;
  for (j1=0;j1<n;j1++) {
    m = malloc((n-1)*(n-1)*sizeof(double));
    for (i=1;i<n;i++) {
      jm = 0;
      for (j=0;j<n;j++) {
        if (j == j1)
          continue;
        m[(i-1)*(n-1)+jm] = cov[i*n+j];
        jm++;
      }
    }
    det += pow(-1.0,1.0+j1+1.0) * cov[j1] * ighmm_determinant(m,n-1);
    free(m);
  }
  return det;
}

/*============================================================================*/
/*
  The inverse of a square matrix A with a non zero determinant is the adjoint
  matrix divided by the determinant.
  The adjoint matrix is the transpose of the cofactor matrix.
  The cofactor matrix is the matrix of determinants of the minors Aij
  multiplied by -1^(i+j).
  The i,j'th minor of A is the matrix A without the i'th column or
  the j'th row.
*/
int ighmm_inverse(double *cov, int n, double det, double *inv)
{
  int i, j, ic, jc, actrow, actcol;
  double * m;

  if (n == 1) {
    inv[0] = 1/cov[0];
    return 0;
  }
  if (n == 2) {
    inv[0] = cov[3] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[1] = - cov[1] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[2] = - cov[2] / (cov[0]*cov[3] - cov[1]*cov[2]);
    inv[3] = cov[0] / (cov[0]*cov[3] - cov[1]*cov[2]);
    return 0;
  }
  for (i=0;i<n;i++) {
    for (j=0;j<n;j++) {
      /* calculate minor i,j */
      m = malloc((n-1)*(n-1)*sizeof(double));
      actrow = 0;
      for (ic=0;ic<n;ic++) {
        if (ic == i)
          continue;
        actcol = 0;
        for (jc=0;jc<n;jc++) {
          if (jc == j)
            continue;
          m[actrow*(n-1)+actcol] = cov[ic*n+jc];
          actcol++;
        }
        actrow++;
      }
      /* cofactor i,j is determinant of m times -1^(i+j) */
      inv[j*n+i] = pow(-1.0,i+j+2.0) * ighmm_determinant(m,n-1) / det;
      free(m);
    }
  }
  return 0;
}

/*============================================================================*/
int ighmm_cholesky_decomposition (double *sigmacd, int dim, double *cov) {
#ifdef DO_WITH_GSL
  int i, j;
  gsl_matrix *tmp = gsl_matrix_alloc(dim, dim);

  /* temp matrix */
  for (i=0;i<dim;i++) {
    for (j=0;j<dim;j++) {
      gsl_matrix_set(tmp, i, j, cov[i*dim+j]);
    }
  }
  /* cholesky decomposition of temp matrix */
  gsl_linalg_cholesky_decomp(tmp);
  /* copy to sigmacd and set upper triangular to 0 */
  for (i=0;i<dim;i++) {
    for (j=0;j<dim;j++) {
      if (j>i)
        sigmacd[i*dim+j] = 0;
      else
        sigmacd[i*dim+j] = gsl_matrix_get(tmp, i, j);
    }
  }
  gsl_matrix_free(tmp);

#elif defined HAVE_CLAPACK_DPOTRF
  int info;
  /* copy cov. matrix entries to result matrix, the rest is done in-place */
  memcpy(sigmacd, cov, dim * dim * sizeof *cov);
  info = clapack_dpotrf(CblasRowMajor, CblasUpper, dim, sigmacd, dim);
#else
  int row, j, k;
  double sum;

  /* copy cov to sigmacd */
  for (row=0; row<dim; row++) {
    for (j=0; j<dim; j++) {
      sigmacd[row*dim+j] = cov[row*dim+j];
    }
  }

  for (row=0; row<dim; row++) {
    /* First compute U[row][row] */
    sum = cov[row*dim+row];
    for (j=0; j<(row-1); j++) sum -= sigmacd[j*dim+row]*sigmacd[j*dim+row];
    if (sum > DBL_MIN) {
      sigmacd[row*dim+row] = sqrt(sum);
      /* Now find elements sigmacd[row*dim+k], k > row. */
      for (k=(row+1); k<dim; k++) {
        sum = cov[row*dim+k];
        for (j=0; j<(row-1); j++)
          sum -= sigmacd[j*dim+row]*sigmacd[j*dim+k];
        sigmacd[row*dim+k] = sum/sigmacd[row*dim+row];
      }
    }
    else {  /* blast off the entire row. */
      for (k=row; k<dim; k++) sigmacd[row*dim+k] = 0.0;
    }
  }
#endif

  return 0;
}


/* applying a t-model to performance data to obtain a robust
   estimate of the mean via ML of the loglikelihood function */

/* PREPROCESSOR FLAGS:
   NR - Algorithms from Numerical Recipes
     NELDER - Nelder-Mead algorithm, not working
     POWELL - Powell's method
     QUASI_NEWTON - quasi-Newton (Newton-Raphson) method
   GSL - algrithms from GSL library
     (select Conjugate gradient, BFGS or steepest decent in function
     gsl_minimize_loglikelihood)
   MAIN - compile with main() and test data
   SUMMARY - short output of results
   DEBUG - turns on various output
*/

// Nealder and Mead: does not work
//gcc -DNR -DMAIN -DNELDER robust.c dfpmin.c lnsrch.c nrutil.c amoeba.c amotry.c -I/usr/include/gsl -lm -L/usr/local/lib -lgsl -lgslcblas -g  -o test.x

// quasi-Newton:
//gcc -DNR -DMAIN -DQUASI_NEWTON robust.c dfpmin.c lnsrch.c nrutil.c -I/usr/include/gsl -lm -L/usr/local/lib -lgsl -lgslcblas -g  -o test.x

// Powell:
// gcc -DNR -DMAIN -DPOWELL robust.c  nrutil.c  powell.c linmin.c f1dim.c mnbrak.c brent.c -I/usr/include/gsl -lm -L/usr/local/lib -lgsl -lgslcblas -g  -o test.x

// GSL:
// gcc -DGSL -DMAIN robust.c dfpmin.c lnsrch.c nrutil.c -I/usr/include/gsl -lm -L/usr/local/lib -lgsl -lgslcblas -g  -o test.x
#if defined(GSL) || defined(NR)

#ifdef NR
#include <stdlib.h>
#include <stdio.h>
#define NRANSI
#include "nr.h"
#include "nrutil.h"
#endif

#ifdef GSL
#include <gsl_math.h>
#include <gsl_sort.h>
#include <gsl_sf_gamma.h>
#include <gsl_sf_psi.h>
#include <gsl_sf_log.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl_statistics_double.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl_multimin.h>
#endif

#ifdef NR
void get_starting_value(float* x0);
void output_results(float* x0, int iter, float* val);
#endif
#ifdef GSL
void get_starting_value(gsl_vector* x0, void* params);
void output_results(gsl_vector* x0, int iter, double* val, void* params);

void loglikelihood_fdf (const gsl_vector *x, void *params, double *f,
   gsl_vector *df);
int  gsl_minimize_loglikelihood(const double step_size, const double tol,
   const double eps_abs, const int maxiter, gsl_vector* x0, double* val, int* iter,
   void* params);
int  gsl_minimize_loglikelihood_old(const double step_size, const double tol,
   const double eps_abs, const int maxiter, gsl_vector* x0, double* val, int* iter,
   void* params);
#endif

#ifdef NR
void nelder_mead(float* x0, float (*func)(float []), float* ftol,
   float* val);
#endif

static int nfunc;  // number of function calls
static int ndfunc; // number of calls to compute derivative
double nu=3;
int with_nu;
double nustart=4.0;
int ndim;

#ifdef MAIN
/* speed of light data set */
double c_data[66] = {
      28.0, 26.0, 33.0, 24.0, 34.0, -44.0, 27.0, 16.0, 40.0, -2.0,
      29.0, 22.0, 24.0, 21.0, 25.0, 30.0, 23.0, 29.0, 31.0, 19.0,
      24.0, 20.0, 36.0, 32.0, 36.0, 28.0, 25.0, 21.0, 28.0, 29.0,
      37.0, 25.0, 28.0, 26.0, 30.0, 32.0, 36.0, 26.0, 30.0, 22.0,
      36.0, 23.0, 27.0, 27.0, 28.0, 27.0, 31.0, 27.0, 26.0, 33.0,
      26.0, 32.0, 32.0, 24.0, 39.0, 28.0, 24.0, 25.0, 32.0, 25.0,
      29.0, 27.0, 28.0, 29.0, 16.0, 23.0};
#endif

int     ndata;    // #data points
double *data;

/*****************************************************************************/
#ifdef NR
float loglikelihood_f(float x[])
#endif
#ifdef GSL
double loglikelihood_f(const gsl_vector *x, void *params)
#endif
/*****************************************************************************/
{
   double mu, // estimate of location
     sigma2;  // estimate of scale
   double delta, g, psi;

#ifdef GSL
   int ndata;
   double nu, *data;
   double * tmp = (double *) params;
#endif
   double y;  // temporary variable
   int i;

   nfunc++;

#ifdef NR
   mu      = x[1];
   sigma2  = x[2];
   if (with_nu) nu = x[3];
#else
   /* extract arguments and parameters */
   mu     = gsl_vector_get(x, 0);
   sigma2 = gsl_vector_get(x, 1);
   nu     = tmp[0];
   if (nu == 0){
      with_nu = 1;
      nu = gsl_vector_get(x,2);
   }
   ndata   = (int) tmp[1];
   data    = &(tmp[2]);
   //printf("%d %f %f\n", n, nu, data[0]);
#endif

   y = - 0.5 * gsl_sf_log(sigma2) * ndata;
   for (i=0; i<ndata; i++){
     delta = (data[i] - mu)*( data[i] - mu) / (nu * sigma2);
     y = y - 0.5 * (nu + 1.) * gsl_sf_log( 1+delta );
   }

   if (with_nu){ /* compute whole log likelyhood function */
      g =  gsl_sf_lngamma(0.5*(nu+1.)) - gsl_sf_lngamma(0.5*nu)
           - 0.5 * gsl_sf_log(nu);
      y = y + ndata * g;
   }

#ifdef DEBUG
   if (with_nu)
      printf("mu=%.12f, sigma=%.12f, nu=%.12f, y=%.12f\n", mu, sqrt(sigma2), nu, y);
   else
      printf("mu=%.12f, sigma=%.12f, y=%.12f\n", mu, sqrt(sigma2), y);
#endif

   return -y; /* since we're minimizing, not maximizing */

}

/*****************************************************************************/
#ifdef NR
void loglikelihood_df(float x[],float df[])
#endif
#ifdef GSL
void loglikelihood_df(const gsl_vector *x, void *params, gsl_vector *df)
#endif
/*****************************************************************************/
/* this function should store the n-dimensional gradient
g_i = d f(x,params) / d x_i in the vector g for argument x and parameters
params, returning an appropriate error code if the function cannot be computed. */
{
   double mu, sigma2;

   int i, k;
   double dldtheta, dldphi, dldnu, y, r, t, d, g, s1, s2;
#ifdef GSL
   int ndata, with_nu = 0;
   double nu, *data;
   double * tmp = (double *) params;
#endif

   ndfunc++;

#ifdef NR
   /* extract arguments and parameters */
   mu     = x[1];
   sigma2 = x[2];
   if (with_nu)    nu = x[3];
#endif
#ifdef GSL
   /* extract arguments and parameters */
   mu       = gsl_vector_get(x, 0);
   sigma2   = gsl_vector_get(x, 1);
   nu       = tmp[0];
   if (nu == 0){
      with_nu = 1;
      nu = gsl_vector_get(x,2);
   }
   ndata    = (int) tmp[1];
   data     = &(tmp[2]);
   //printf("%d %f %f\n", ndata, nu, data[0]);
   //printf("df mu=%lf sigma=%lf nu=%lf \n", mu, sqrt(sigma2), nu);
#endif

   dldtheta = 0.;
   dldphi   = 0.;
   if (with_nu) {
      dldnu = 0.;
      s1 = 0.;
      s2 = 0.;
   }

   /* calculate terms depending on data[i] */
   for (i=0; i<ndata; i++){
      y = data[i];
      d = y - mu;
      r = d*d / (sigma2*nu);

      dldtheta = dldtheta + d / ( 1 + r );
      dldphi   = dldphi + r / (1+r); // + 0.5/sigma2;
      if (with_nu){
         s1 = s1 +  r / (1+r);  //t * d;
         s2 = s2 +  gsl_sf_log(1+r) ;
      }
   }

   /* add constant terms */
   dldtheta = (nu+1)/(sigma2*nu) * dldtheta;
   dldphi   = - 0.5*ndata/sigma2 + 0.5*(nu+1)/sigma2 * dldphi;
   if (with_nu) {
      /* digamma(z) = psi(z) = d/dz( ln gamma(z) ) */
      g =  0.5 * ndata * ( gsl_sf_psi((nu+1)/2) - gsl_sf_psi(0.5*nu) - 1./nu );
      dldnu    = g + 0.5*(nu+1)/nu * s1 - 0.5 * s2;
   }

   /* change signs since we are minimizing, not maximizing */
#ifdef NR
   df[1] = - dldtheta;
   df[2] = - dldphi;
   if (with_nu)  df[3] = - dldnu;
#endif
#ifdef GSL
   gsl_vector_set(df, 0, - dldtheta);
   gsl_vector_set(df, 1, - dldphi);
   if (with_nu) {
      gsl_vector_set(df, 2, - dldnu);
   }
#endif

#ifdef DEBUG
#ifdef NR
   if (with_nu) printf("  df %lf %lf %lf\n", df[1], df[2], df[3]);
   else printf("  df %lf %lf \n", df[1], df[2]);
#endif
#ifdef GSL
   if (with_nu) printf("  df %lf %lf %lf\n", gsl_vector_get(df, 0), gsl_vector_get(df, 1),
       gsl_vector_get(df, 2));
   else printf("  df %lf %lf \n", gsl_vector_get(df, 0), gsl_vector_get(df, 1));
#endif
#endif
}

/*****************************************************************************/
#ifdef NR
int ml(const int ndata_loc,  double* const data_loc, double* nu, double* mu,
       double* sigma, float* val)
#endif
#ifdef GSL
int ml(const int ndata,      double* const data,     double* nu, double* mu,
       double* sigma, double* val)
#endif
/*****************************************************************************/
/* performs a ML method for a t model
   ndata  (in)      size of data
   data   (in)      data / measurements
   nu     (in/out)  if nu is 0, optimize also for nu and return
                    optimized value
                    if nu is != 0, accept nu as parameter
   mu     (out)     optimized value for mu
   sigma  (out)     optimized value for sigma
   val    (out)     if nu is variable, value of log likelihood function
                    if nu is fixed, value of simplified log likelihood
                    function*/
{
#ifdef NR
   float *x0;            // starting point / maximum
#endif
#ifdef GSL
   gsl_vector *x0;       // starting point / maximum
   double *params;

   double step_size, tol, eps_abs;
#endif
   int maxiter = 1000;
   int iter = 0, i, j;

   /* init starting point */
   if (*nu == 0)
      ndim = 3;
   else
      ndim = 2;

#ifdef NR
   ndata = ndata_loc;
   data  = data_loc;

   x0  = vector(1,ndim);
#endif

#ifdef GSL
   /* init parameters */
   params = malloc((ndata+2)*sizeof(double));
   params[0] = *nu;
   params[1] = (double) ndata;
   for (i=0; i<ndata; i++){
      params[2+i] = data[i];
   }

   x0  = gsl_vector_alloc(ndim);
#endif

#ifdef NR
   get_starting_value(x0);
#endif
#ifdef GSL
   get_starting_value(x0, params);
#endif

   nfunc = ndfunc = 0;

#ifdef NR

#ifdef NELDER
   float ftol = 1.0e-6;
   nelder_mead(x0, loglikelihood_f, &(ftol), val);
#endif

#ifdef POWELL
   float ftol = 1.0e-6;
   float **p;

   p=matrix(1,ndim,1,ndim);
   for (i=1;i<=ndim;i++)
      for (j=1;j<=ndim;j++)
         p[i][j]=(i == j ? 1. : 0.0);
   if (with_nu)
      p[3][3] = 0.01;

   iter = maxiter;
   powell(x0,p,ndim,ftol,&iter,val,loglikelihood_f);

   free_matrix(p,1,ndim,1,ndim);
#endif

#ifdef QUASI_NEWTON
   float gtol = 1.0e-8;
   dfpmin(x0,ndim,gtol,&iter,val,loglikelihood_f,loglikelihood_df);
#endif

// endif NR
#endif

#ifdef GSL
   step_size = 0.1;   // size of first trial step
   tol       = 0.1;   // accuracy of line minimization, 0.1 will do
   eps_abs   = 1e-8;

   gsl_minimize_loglikelihood(step_size, tol, eps_abs, maxiter, x0, val,
      &(iter), params);
#endif

#ifdef SUMMARY
#ifdef NR
   output_results(x0, iter, val);
#endif
#ifdef GSL
   output_results(x0, iter, val, params);
#endif
#endif

#ifdef NR
   *mu     = (double) x0[1];
   *sigma = sqrt((double) x0[2]);
   if (with_nu)
      *nu = (double) x0[3];

   free_vector(x0,1,ndim);
#endif
#ifdef GSL
   *mu    = gsl_vector_get(x0, 0);
   *sigma = sqrt(gsl_vector_get(x0, 1));
   if (with_nu)
      *nu = gsl_vector_get(x0, 2);

   gsl_vector_free(x0);
   free (params);
#endif

   return 0;

}

#ifdef MAIN
/*****************************************************************************/
int main()
/*****************************************************************************/
{
   int i, varnu=0;
#ifdef NR
   float val;
#endif
#ifdef GSL
   double val;
#endif
   double mu, sigma;

   const int ndata_loc = 66;
   double *data_loc = &(c_data[0]);

   //nu = 0.0; with_nu = 1;
   nu =  4.0; with_nu = 0;

   ml(ndata_loc, data_loc, &(nu), &(mu), &(sigma), &(val));

#ifdef SUMMARY
   printf("\n\nReturn values:\n");
   printf("nu=%.5lf mu=%.5lf sigma=%.5lf val=%.5lf\n", nu, mu, sigma, val);
#endif

   return 0;

}
#endif

/*****************************************************************************/
#ifdef NR
void get_starting_value(float* x0)
#endif
#ifdef GSL
void get_starting_value(gsl_vector* x0, void* params)
#endif
/*****************************************************************************/
/* computes an estimate of location and scale                                */
/*****************************************************************************/
{
   double mean, variance;
   double median, *madv, mad;
   int i;

#ifdef GSL
   double* tmp = (double*) params;
   int ndata   = (int) tmp[1];
   double* data    = &(tmp[2]);
#endif

   /* Possiblity I: Use mean and variance as starting value
                    this is probably going to crash when outliers are present */
   mean = gsl_stats_mean(&(data[0]), (size_t) 1,(size_t) ndata);
   variance = gsl_stats_variance_with_fixed_mean(&(data[0]),
      (size_t) 1, (size_t) ndata, mean);
#ifdef NR
   x0[1] = mean;
   x0[2] = variance;
#endif
#ifdef GSL
   gsl_vector_set(x0, 0, mean);
   gsl_vector_set(x0, 1, variance);
#endif

   /* Possiblity II: Use median and 1.483*MAD as starting value
                     Absolute Deviatons MAD(x) = med_i {|x_i - med_j (x_j)|} */
   gsl_sort(data, (size_t) 1, (size_t) ndata);
   median = gsl_stats_median_from_sorted_data(data, (size_t) 1, (size_t) ndata);
   madv = (double*) malloc(ndata * sizeof(double));
   for (i=0; i<ndata; i++){
       madv[i] = abs(data[i] - median);
   }
   gsl_sort(madv, (size_t) 1, (size_t) ndata);
   mad = 1.483 * gsl_stats_median_from_sorted_data(madv, (size_t) 1, (size_t) ndata);
   //printf("median: %20.5f, mad     : %20.5f\n", median, mad);
   //mad = 2 * median; //5.25 * gsl_stats_median_from_sorted_data(madv, (size_t) 1, (size_t) ndata);
   free(madv);

#ifdef NR
   x0[1] = median;
   x0[2] = 0.7*mad*mad;
   if (with_nu)
      x0[3] = nustart;
#ifdef SUMMARY
   printf("\nStarting vector        : ");
   for (i=1;i<=ndim;i++) printf("%12.6f",x0[i]);
#endif
#endif

#ifdef GSL
   gsl_vector_set(x0, 0, median);
   gsl_vector_set(x0, 1, 0.7*mad*mad);
   if (with_nu)
      gsl_vector_set(x0, 2, nustart);
#ifdef SUMMARY
   printf("\nStarting vector        : ");
   for (i=0;i<ndim;i++) printf("%12.6f", gsl_vector_get(x0, i));
#endif
#endif

}

/*****************************************************************************/
#ifdef NR
void output_results(float* x0, int iter, float* val)
#endif
#ifdef GSL
void output_results(gsl_vector* x0, int iter, double* val, void* params)
#endif
/*****************************************************************************/
/* output of minimum  and statistics                                         */
/*****************************************************************************/
{
   int i;
#ifdef NR
   float      *xdf;        // gradient at maximum
   xdf = vector(1,ndim);
#endif

#ifdef GSL
   int with_nu = 0;
   gsl_vector *xdf;      // gradient at maximum
   double* tmp = (double*) params;
   double nu;

   xdf = gsl_vector_alloc(ndim);

   nu = tmp[0];
   if (nu == 0.0) with_nu = 1;
#endif

   printf("\n\nIterations             : %3d\n",iter);
   printf("Func. evals            : %3d\n",nfunc);
#ifdef QUASI_NEWTON
   printf("Deriv. evals           : %3d\n",ndfunc);
#endif

   printf("\nMaximum found at       : ");
#ifdef NR
   x0[2] = sqrt(x0[2]);
   for (i=1;i<=ndim;i++) printf("%12.6f",x0[i]);
   x0[2] = x0[2] * x0[2];
#endif
#ifdef GSL
   gsl_vector_set(x0, 1, sqrt(gsl_vector_get(x0, 1)));
   for (i=0;i<ndim;i++) printf("%12.6f", gsl_vector_get(x0, i));
   gsl_vector_set(x0, 1, gsl_vector_get(x0, 1) * gsl_vector_get(x0, 1));
#endif

   printf("\nMaximum function value : %12.6f\n",*val);

   printf("Derivative at maximum  : ");
#ifdef NR
   loglikelihood_df(x0, xdf);
   for (i=1;i<=ndim;i++) printf("%12.6f",xdf[i]);
#endif
#ifdef GSL
   loglikelihood_df(x0, params, xdf);
   for (i=0;i<ndim;i++) printf("%12.6f",gsl_vector_get(xdf, i));
#endif

#ifdef MAIN
   if (with_nu){ // for variable nu
       printf("\n\nTrue maximum is at (mu_hat, sigma_hat, vu) = (27.40, 3.81, 2.13)\n");
#ifdef NR
       x0[1] =  27.40;
       x0[2] =  3.81*3.81;
       x0[3] =  2.13;
       printf("Test: function value at maximum = %12.6f\n",  loglikelihood_f(x0));
       printf("Derivative at maximum           : ");
       loglikelihood_df(x0, xdf);
       for (i=1;i<=ndim;i++) printf("%12.6f",xdf[i]);
#endif
#ifdef GSL
       gsl_vector_set(x0, 0, 27.40);
       gsl_vector_set(x0, 1, 3.81*3.81);
       gsl_vector_set(x0, 2, 2.13);

       printf("Test: function value at maximum = %12.6f\n",  loglikelihood_f(x0, params));
       loglikelihood_df(x0, params, xdf);
       printf("Derivative at maximum           : ");
       for (i=0;i<ndim;i++) printf("%12.6f",gsl_vector_get(xdf, i));
#endif
   } else {         // for fixed nu=4:
       printf("\n\nTrue maximum is at (mu_hat, sigma_hat) = (27.49, 4.51)\n");
#ifdef NR
       x0[1] =  27.49;
       x0[2] =  4.51 * 4.51;
       printf("Test: function value at maximum : %12.6f\n",  loglikelihood_f(x0));
       printf("Derivative at true maximum      : ");
       loglikelihood_df(x0, xdf);
       for (i=1;i<=ndim;i++) printf("%12.6f",xdf[i]);
#endif
#ifdef GSL
       gsl_vector_set(x0, 0, 27.40);
       gsl_vector_set(x0, 1, 4.51 * 4.51);
       printf("Test: function value at maximum : %12.6f\n",  loglikelihood_f(x0, params));
       loglikelihood_df(x0, params, xdf);
       printf("Derivative at true maximum      : ");
       for (i=0;i<ndim;i++) printf("%12.6f",gsl_vector_get(xdf, i));
#endif
   }
#endif

#ifdef NR
   free_vector(xdf,1,ndim);
#endif
#ifdef GSL
   gsl_vector_free(xdf);
#endif

}

#ifdef NR
#ifdef NELDER
/*****************************************************************************/
void nelder_mead(float* x0, float(*func)(float []), float* ftol,
   float* val)
/*****************************************************************************/
/* computes minimum with method of Nelder and Mead from numerical recipes    */
/*
/* parameters:
/* x0    on input: starting value; on output: minimum
/* func  function
/* ftol  tolerance
/* val   function value at minimum */
/*****************************************************************************/
{
   int i, j;
   float *x, *y, **p;
   double delta_mu, delta_sigma2;

   if (ndim != 2) {
      printf("ndim != 2 not implemented\n");
      exit(0);
   }

   x=vector(1,ndim);
   y=vector(1,ndim+1);
   p=matrix(1,ndim+1,1,ndim);

   delta_mu = 0.5e0;
   delta_sigma2 = 1e0;

   for (i=1;i<=ndim+1;i++) {
      x[1] = p[i][1]=(i == (j+1) ? x0[1] + delta_mu     : x0[1] - delta_mu    );
      x[2] = p[i][2]=(i == (j+1) ? x0[2] + delta_sigma2 : x0[2] - delta_sigma2);
      y[i]=loglikelihood_f(x);
   }

   amoeba(p,y,ndim,*ftol,func,&nfunc);
   y[2] = sqrt(y[2]);

   printf("Vertices of final 3-d simplex and\n");
   printf("function values at the vertices:\n\n");
   printf("%3s %10s %12s %14s\n\n",
      "i","x[i]","y[i]", "function");
   for (i=1;i<=ndim+1;i++) {
      printf("%3d ",i);
      for (j=1;j<=ndim;j++) printf("%12.6f ",p[i][j]);
         printf("%12.6f\n",y[i]);
   }

   for (j=1;j<=ndim;j++) {
      // sum up vertices of final simplex
      x0[j] = p[1][j];
      for (i=2;i<=ndim+1;i++)
         x0[j] = x0[j] + p[i][j];
      // ... and average ...
      x0[j] = x0[j]/(ndim+1);
   }

   // calculate average function value at maximum
   *val = y[1];
   for (i=2;i<=ndim+1;i++)
      *val = *val + y[i];
   *val = *val / (ndim+1);

   free_vector(x,1,ndim);
   free_matrix(p,1,ndim+1,1,ndim);
   free_vector(y,1,ndim+1);
}
//endif NELDER
#endif

//endif NR
#endif

#ifdef GSL
/* Compute both f and df together. */
void loglikelihood_fdf (const gsl_vector *x, void *params, double *f,
   gsl_vector *df)
{
     *f = loglikelihood_f(x, params);
     loglikelihood_df(x, params, df);

     //printf("fdf =%lf %lf %lf %lf \n", gsl_vector_get(x,0), gsl_vector_get(x,1),
     //   gsl_vector_get(df,0), gsl_vector_get(df,1));
}

int  gsl_minimize_loglikelihood(const double step_size, const double tol,
   const double eps_abs, const int maxiter, gsl_vector* x0, double* val, int* iter,
   void* params){
   /*
   s       (in)      GSL multimin fminimizer object
   eps_abs (in)
   maxiter (in)      maximum number of iterations
   iter    (in/out)  number of iterations             */

   const gsl_multimin_fdfminimizer_type *T;
   gsl_multimin_fdfminimizer *s;
   gsl_multimin_function_fdf my_func;
   double* tmp = (double*) params;
   double nu = tmp[0];
   double sigma2;
   int my_iter = 0;
   int status;

   /* init function object */
   if (nu != 0.0) my_func.n = 2;
   else           my_func.n = 3;
   my_func.f   = &loglikelihood_f;
   my_func.df  = &loglikelihood_df;
   my_func.fdf = &loglikelihood_fdf;
   my_func.params = params;

   /* set up solver */
   T = gsl_multimin_fdfminimizer_conjugate_fr;
   // T = gsl_multimin_fdfminimizer_vector_bfgs;
   // T = gsl_multimin_fdfminimizer_steepest_descent;
   if (nu != 0.0)  s = gsl_multimin_fdfminimizer_alloc(T,2);
   else            s = gsl_multimin_fdfminimizer_alloc(T,3);

   /* call minimizer */
   //s->f = my_func->f;
   //gsl_multimin_fminimizer_set(s, &my_func, x0, step_size)
   gsl_multimin_fdfminimizer_set(s, &my_func, x0, step_size, tol);

   /* let gsl minimizer iterate */
   do
   {
      status = gsl_multimin_fdfminimizer_iterate(s);
      if (status) break;

      status = gsl_multimin_test_gradient(s->gradient, eps_abs);
      /* if (status == GSL_SUCCESS){
         printf("Maximum found after %d iterations at:\n", *iter);
      }
      printf ("%5d %.5f %.5f %10.5f\n", *iter,
        gsl_vector_get (s->x, 0), sqrt(gsl_vector_get (s->x, 1)), s->f); */
      (*iter)++;
   } while (status == GSL_CONTINUE && *iter < maxiter);

   /* just precaution */
   sigma2 =  gsl_vector_get(x0, 1);
   if (isnan(sigma2)){
     printf("!!! sigma2 is nan\n");
     printf("%5d %15.5f %15.5f", *iter,
     gsl_vector_get(s->x, 0),
     sqrt(gsl_vector_get(s->x, 1)));
     if (nu == 0.0) printf("%15.5f", gsl_vector_get(s->x, 2));
     printf("%15.5f\n", -gsl_multimin_fdfminimizer_minimum(s));

     exit;
   }

   /* set return values */
   gsl_vector_set(x0, 0, gsl_vector_get(s->x, 0));
   gsl_vector_set(x0, 1, gsl_vector_get(s->x, 1));
   if (nu == 0.0)  /* return optimized value */
       gsl_vector_set(x0, 2, gsl_vector_get(s->x, 2));
   *val   = -gsl_multimin_fdfminimizer_minimum(s);

   /* clean up */
   //gsl_multimin_fminimizer_free(s);
   gsl_multimin_fdfminimizer_free(s);

   return 0;
}

// obsolete
int  gsl_minimize_loglikelihood_old(const double step_size, const double tol,
   const double eps_abs, const int maxiter, gsl_vector* x0, double* val, int* iter,
   void* params){
   /*
   s       (in)      GSL multimin fminimizer object
   eps_abs (in)
   maxiter (in)      maximum number of iterations
   iter    (in/out)  number of iterations             */

   const gsl_multimin_fdfminimizer_type *T;
   gsl_multimin_fdfminimizer *s;
   gsl_multimin_function_fdf my_func;
   double* tmp = (double*) params;
   double nu = tmp[0];
   double sigma2;
   int my_iter = 0;
   int status;

   int converged = 0;
   double start=0.4;
   double median = gsl_vector_get(x0, 0);
   double mad    = gsl_vector_get(x0, 1);
   mad = sqrt(mad/0.7);

   /* init function object */
   if (nu != 0.0) my_func.n = 2;
   else           my_func.n = 3;
   my_func.f   = &loglikelihood_f;
   my_func.df  = &loglikelihood_df;
   my_func.fdf = &loglikelihood_fdf;
   my_func.params = params;

   /* set up solver */
   T = gsl_multimin_fdfminimizer_conjugate_fr;
   // T = gsl_multimin_fdfminimizer_vector_bfgs;
   // T = gsl_multimin_fdfminimizer_steepest_descent;
   if (nu != 0.0)  s = gsl_multimin_fdfminimizer_alloc(T,2);
   else            s = gsl_multimin_fdfminimizer_alloc(T,3);

   /* call minimizer */
   //s->f = my_func->f;
   //gsl_multimin_fminimizer_set(s, &my_func, x0, step_size)

   gsl_multimin_fdfminimizer_set(s, &my_func, x0, step_size, tol);

   do {//while (!converged && my_iter < 10){
      converged = 1;

      /* let gsl minimizer iterate */
      do
      {
         status = gsl_multimin_fdfminimizer_iterate(s);
         if (status) break;

         status = gsl_multimin_test_gradient(s->gradient, eps_abs);
         /* if (status == GSL_SUCCESS){
            printf("Maximum found after %d iterations at:\n", *iter);
         }
         printf ("%5d %.5f %.5f %10.5f\n", *iter,
            gsl_vector_get (s->x, 0), sqrt(gsl_vector_get (s->x, 1)), s->f); */
         (*iter)++;
      } while (status == GSL_CONTINUE && *iter < maxiter);

      sigma2 =  gsl_vector_get(x0, 1);
      if (!isnan(sigma2)) break;

      gsl_vector_set(x0, 0, median);
      gsl_vector_set(x0, 1, start*mad*mad);
      gsl_multimin_fdfminimizer_set (s, &my_func, x0, step_size, tol);
      start = start + 0.5;
      my_iter = my_iter + 1;
      if (my_iter > 1) printf("-- it=%d, sigma=%lf\n", my_iter, sqrt(sigma2));
   } while (my_iter < 10);
  if (*iter > maxiter){
     printf("!!! sigma2 is nan\n");
     printf("%5d %15.5f %15.5f", *iter,
     gsl_vector_get(s->x, 0),
     sqrt(gsl_vector_get(s->x, 1)));
     if (nu == 0.0) printf("%15.5f", gsl_vector_get(s->x, 2));
     printf("%15.5f\n", -gsl_multimin_fdfminimizer_minimum(s));
   }

   gsl_vector_set(x0, 0, gsl_vector_get(s->x, 0));
   gsl_vector_set(x0, 1, gsl_vector_get(s->x, 1));
   if (nu == 0.0)  /* return optimized value */
       gsl_vector_set(x0, 2, gsl_vector_get(s->x, 2));
   *val   = -gsl_multimin_fdfminimizer_minimum(s);

   /* clean up */
   //gsl_multimin_fminimizer_free(s);
   gsl_multimin_fdfminimizer_free(s);

   return 0;
}
//endif GSL
#endif

#endif
// GSL || NR


/* Copyright 2016. The Regents of the University of California.
 * Copyright 2016-2020. Uecker Lab. University Medical Center Göttingen.
 * All rights reserved. Use of this source code is governed by
 * a BSD-style license which can be found in the LICENSE file.
 *
 * Authors:
 * 2016 Jonathan Tamir <jtamir@eecs.berkeley.edu>
 * 2016-2020 Martin Uecker <martin.uecker@med.uni-goettingen.de>
 * 2019-2020 Moritz Blumenthal
 */

#include <assert.h>
#include <complex.h>
#include <stdbool.h>

#include "misc/misc.h"

#ifdef USE_MACPORTS
#include <cblas_openblas.h>
#elif USE_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

#ifdef _OPENMP
#include <omp.h>
#endif

#ifdef USE_CUDA
#include <cuComplex.h>
#include <cublas_v2.h>

#include "num/gpuops.h"
#endif

#include "blas.h"

#ifdef USE_CUDA
//blas2_* means, we use the new blas interface, i.e. scalar parameters are written an read by pointers.
//These pointers can point to cpu or gpu memory.

//blas_* uses the old interface where scalar parameters/results are provided/written by value/return.

static void cublas_error(int line, cublasStatus_t code)
{
	error("cublas error: %d in line %d \n", code, line);
}

#define CUBLAS_ERROR(x)	({ cublasStatus_t errval = (x); if (CUBLAS_STATUS_SUCCESS != errval) cublas_error(__LINE__, errval); })

static cublasHandle_t handle;
static _Bool handle_created = false;

static cublasHandle_t get_handle(void)
{
	if (!handle_created)
		CUBLAS_ERROR(cublasCreate(&handle));

	handle_created = true;

	return handle;
}

static void destroy_handle(void)
{
	CUBLAS_ERROR(cublasDestroy(handle));
	handle_created = false;
}

static void cublas_set_pointer_host(void)
{
	(void)get_handle();

	CUBLAS_ERROR(cublasSetPointerMode(handle, CUBLAS_POINTER_MODE_HOST));
}

static void cublas_set_pointer_device(void)
{
	(void)get_handle();

	CUBLAS_ERROR(cublasSetPointerMode(handle, CUBLAS_POINTER_MODE_DEVICE));
}

static cublasOperation_t cublas_trans(char trans)
{
	if (('N' == trans) || ('n'==trans))
		return CUBLAS_OP_N;

	if (('T' == trans) || ('t'==trans))
		return CUBLAS_OP_T;

	if (('C' == trans) || ('c'==trans))
		return CUBLAS_OP_C;

	assert(0);
}
#endif

static void openblas_set_threads(void)
{
#ifndef USE_OPENBLAS
	return;
#else

#ifndef _OPENMP
	return;
#else
	if (1 != openblas_get_parallel())
		return; //pthread version of openblas

	#pragma omp critical
	openblas_set_num_threads(omp_in_parallel() ? 1 : omp_get_max_threads());
#endif
#endif
}

void blas2_cgemm(char transa, char transb, long M, long N, long K, const complex float* alpha, long lda, const complex float* A, long ldb, const complex float* B, const complex float* beta, long ldc, complex float* C)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasCgemm(get_handle(), cublas_trans(transa), cublas_trans(transb), M, N, K, (const cuComplex*)alpha,
			    (const cuComplex*)A, lda, (const cuComplex*)B, ldb, (const cuComplex*)beta, (cuComplex*)C, ldc);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cgemm(CblasColMajor, ('T' == transa) ? CblasTrans : (('C' == transa) ? CblasConjTrans : CblasNoTrans),
		    ('T' == transb) ? CblasTrans : (('C' == transb) ? CblasConjTrans : CblasNoTrans),
		    M, N, K, (void*)alpha, (void*)A, lda, (void*)B, ldb, (void*)beta, (void*)C, ldc);
}

void blas_cgemm(char transa, char transb, long M, long N,  long K, const complex float alpha, long lda, const complex float* A, long ldb, const complex float* B, const complex float beta, long ldc, complex float* C)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasCgemm(get_handle(), cublas_trans(transa), cublas_trans(transb), M, N, K, (const cuComplex*)(&alpha),
			    (const cuComplex*)A, lda, (const cuComplex*)B, ldb, (const cuComplex*)(&beta), (cuComplex*)C, ldc);

		return;
	}
#endif

	openblas_set_threads();

	cblas_cgemm(CblasColMajor, ('T' == transa) ? CblasTrans : (('C' == transa) ? CblasConjTrans : CblasNoTrans),
		    ('T' == transb) ? CblasTrans : (('C' == transb) ? CblasConjTrans : CblasNoTrans),
		    M, N, K, (void*)(&alpha), (void*)A, lda, (void*)B, ldb, (void*)(&beta), (void*)C, ldc);
}

void blas2_cgemv(char trans, long M, long N, const complex float* alpha, long lda, const complex float* A, long incx, const complex float* x, complex float* beta, long incy, complex float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasCgemv(get_handle(), cublas_trans(trans), M, N, (const cuComplex*)alpha,
			    (const cuComplex*)A, lda, (const cuComplex*)x, incx, (const cuComplex*)beta, (cuComplex*)y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cgemv(CblasColMajor, ('T' == trans) ? CblasTrans : (('C' == trans) ? CblasConjTrans : CblasNoTrans),
		    M, N, (void*)alpha, (void*)A, lda, (void*)x, incx, (void*)beta, (void*)y, incy);
}

void blas_cgemv(char trans, long M, long N, complex float alpha, long lda, const complex float* A, long incx, const complex float* x, complex float beta, long incy, complex float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasCgemv(get_handle(), cublas_trans(trans), M, N, (const cuComplex*)&alpha,
			    (const cuComplex*)A, lda, (const cuComplex*)x, incx, (const cuComplex*)&beta, (cuComplex*)y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cgemv(CblasColMajor, ('T' == trans) ? CblasTrans : (('C' == trans) ? CblasConjTrans : CblasNoTrans),
		    M, N, (void*)&alpha, (void*)A, lda, (void*)x, incx, (void*)&beta, (void*)y, incy);
}

void blas2_cgeru(long M, long N, const complex float* alpha, long incx, const complex float* x, long incy, const complex float* y, long lda, complex float* A)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasCgeru(get_handle(), M, N, (const cuComplex*)alpha,
			    (const cuComplex*)x, incx, (const cuComplex*)y, incy, (cuComplex*)A, lda);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cgeru(CblasColMajor, M, N, alpha, x, incx, y, incy, A, lda);
}

void blas_cgeru(long M, long N, complex float alpha, long incx, const complex float* x, long incy, const complex float* y, long lda, complex float* A)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasCgeru(get_handle(), M, N, (const cuComplex*)&alpha,
			    (const cuComplex*)x, incx, (const cuComplex*)y, incy, (cuComplex*)A, lda);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cgeru(CblasColMajor, M, N, &alpha, x, incx, y, incy, (float*)A, lda);
}

void blas2_caxpy(long N, const complex float* alpha, long incx, const complex float* x, long incy, complex float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasCaxpy(get_handle(), N, (const cuComplex*)alpha, (const cuComplex*)x, incx, (cuComplex*)y, incy);

		return;
	}
#endif

	openblas_set_threads();

	cblas_caxpy(N, alpha, x, incx, y, incy);
}

void blas_caxpy(long N, const complex float alpha, long incx, const complex float* x, long incy, complex float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_host();

		cublasCaxpy(get_handle(), N, (const cuComplex*)&alpha, (const cuComplex*)x, incx, (cuComplex*)y, incy);

		return;
	}
#endif

	openblas_set_threads();

	cblas_caxpy(N, &alpha, x, incx, y, incy);
}

void blas2_cscal(long N, const complex float* alpha, long incx, complex float* x)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasCscal(get_handle(), N, (const cuComplex*)alpha, (cuComplex*)x, incx);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cscal(N, alpha, x, incx);
}

void blas_cscal(long N, const complex float alpha, long incx, complex float* x)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_host();

		cublasCscal(get_handle(), N, (const cuComplex*)&alpha, (cuComplex*)x, incx);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cscal(N, &alpha, x, incx);
}

void blas2_cdotu(complex float* result, long N, long incx, const complex float* x, long incy, const complex float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasCdotu(get_handle(), N, (const cuComplex*)x, incx, (const cuComplex*)y, incy, (cuComplex*)result);

		return;
	}
#endif
	openblas_set_threads();

	cblas_cdotu_sub(N, x, incx, y, incy, (void*)result);
}

void blas2_sgemm(char transa, char transb, long M, long N, long K, const float* alpha, long lda, const float* A, long ldb, const float* B, const float* beta, long ldc, float* C)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasSgemm(get_handle(), cublas_trans(transa), cublas_trans(transb), M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sgemm(CblasColMajor, ('T' == transa) ? CblasTrans : (('C' == transa) ? CblasConjTrans : CblasNoTrans),
		    ('T' == transb) ? CblasTrans : (('C' == transb) ? CblasConjTrans : CblasNoTrans),
		    M, N, K, *alpha, A, lda, B, ldb, *beta, C, ldc);
}

void blas_sgemm(char transa, char transb, long M, long N,  long K, const float alpha, long lda, const float* A, long ldb, const float* B, const float beta, long ldc, float* C)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasSgemm(get_handle(), cublas_trans(transa), cublas_trans(transb), M, N, K, &alpha, A, lda, B, ldb, &beta, C, ldc);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sgemm(CblasColMajor, ('T' == transa) ? CblasTrans : (('C' == transa) ? CblasConjTrans : CblasNoTrans),
		    ('T' == transb) ? CblasTrans : (('C' == transb) ? CblasConjTrans : CblasNoTrans),
		    M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);
}

void blas2_sgemv(char trans, long M, long N, const float* alpha, long lda, const float* A, long incx, const float* x, float* beta, long incy, float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasSgemv(get_handle(), cublas_trans(trans), M, N, alpha,
			    (const float*)A, lda, x, incx, beta, y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sgemv(CblasColMajor, ('T' == trans) ? CblasTrans : CblasNoTrans, M, N, *alpha,
		    (const float*)A, lda, x, incx, *beta, y, incy);
}

void blas_sgemv(char trans, long M, long N, const float alpha, long lda, const float* A, long incx, const float* x, float beta, long incy, float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasSgemv(get_handle(), cublas_trans(trans), M, N, &alpha,
			    (const float*)A, lda, x, incx, &beta, y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sgemv(CblasColMajor, ('T' == trans) ? CblasTrans : CblasNoTrans, M, N, alpha,
		    (const float*)A, lda, x, incx, beta, y, incy);
}

void blas2_sger(long M, long N, const float* alpha, long incx, const float* x, long incy, const float* y, long lda, float* A)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_device();

		cublasSger(get_handle(), M, N, alpha, x, incx, y, incy, (float*)A, lda);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sger(CblasColMajor, M, N, *alpha, x, incx, y, incy, (float*)A, lda);
}

void blas_sger(long M, long N, const float alpha, long incx, const float* x, long incy, const float* y, long lda, float* A)
{
#ifdef USE_CUDA

	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		cublasSger(get_handle(), M, N, &alpha, x, incx, y, incy, (float*)A, lda);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sger(CblasColMajor, M, N, alpha, x, incx, y, incy, (float*)A, lda);
}

void blas2_saxpy(long N, const float* alpha, long incx, const float* x, long incy, float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasSaxpy(get_handle(), N, alpha, x, incx, y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_saxpy(N, *alpha, x, incx, y, incy);
}

void blas_saxpy(long N, const float alpha, long incx, const float* x, long incy, float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_host();

		cublasSaxpy(get_handle(), N, &alpha, x, incx, y, incy);

		return;
	}
#endif
	openblas_set_threads();

	cblas_saxpy(N, alpha, x, incx, y, incy);
}

void blas2_sscal(long N, const float* alpha, long incx, float* x)
{
#ifdef USE_CUDA

	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasSscal(get_handle(), N, alpha, x, incx);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sscal(N, *alpha, x, incx);
}

void blas_sscal(long N, float alpha, long incx, float* x)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_host();

		cublasSscal(get_handle(), N, &alpha, x, incx);

		return;
	}
#endif
	openblas_set_threads();

	cblas_sscal(N, alpha, x, incx);
}

void blas2_sdot(float* result, long N, long incx, const float* x, long incy, const float* y)
{
#ifdef USE_CUDA
	if (cuda_ondevice(x)) {

		cublas_set_pointer_device();

		cublasSdot(get_handle(), N, x, incx, y, incy, result);

		return;
	}
#endif
	openblas_set_threads();

	*result = cblas_sdot(N, x, incx, y, incy);
}

void blas_cdgmm(long M, long N, _Bool left_mul, const complex float* A, long lda, const complex float* x, long incx, complex float* C, long ldc)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublasCdgmm(get_handle(), left_mul ? CUBLAS_SIDE_LEFT : CUBLAS_SIDE_RIGHT,
			    M, N, (const cuComplex*)A, lda, (const cuComplex*)x, incx, (cuComplex*)C, ldc);

		return;
	}
#endif
	UNUSED(M);
	UNUSED(N);
	UNUSED(left_mul);
	UNUSED(A);
	UNUSED(lda);
	UNUSED(x);
	UNUSED(incx);
	UNUSED(C);
	UNUSED(ldc);

	assert(0);
}

void blas_sdgmm(long M, long N, _Bool left_mul, const float* A, long lda, const float* x, long incx, float* C, long ldc)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublasSdgmm(get_handle(), left_mul ? CUBLAS_SIDE_LEFT : CUBLAS_SIDE_RIGHT,
			    M, N, A, lda, x, incx, C, ldc);

		return;
	}
#endif
	UNUSED(M);
	UNUSED(N);
	UNUSED(left_mul);
	UNUSED(A);
	UNUSED(lda);
	UNUSED(x);
	UNUSED(incx);
	UNUSED(C);
	UNUSED(ldc);

	assert(0);
}

//B = alpha * op(A)
void blas_cmatcopy(char trans, long M, long N, complex float alpha, const complex float* A, long lda, complex float* B, long ldb)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		complex float zero = 0.;

		cublasCgeam(get_handle(), cublas_trans(trans), cublas_trans('N'),
			    M, N, (const cuComplex*)&alpha, (const cuComplex*)A, lda, (const cuComplex*)&zero, (const cuComplex*)B, ldb, (cuComplex*)B, ldb);

		return;
	}
#endif
	UNUSED(trans);
	UNUSED(M);
	UNUSED(N);
	UNUSED(alpha);
	UNUSED(lda);
	UNUSED(A);
	UNUSED(ldb);
	UNUSED(B);

	assert(0);
}

//B = alpha * op(A)
void blas2_cmatcopy(char trans, long M, long N, const complex float* alpha, const complex float* A, long lda, complex float* B, long ldb)
{
#ifdef USE_CUDA

	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		complex float* zero = cuda_malloc(8);
		cuda_clear(8, zero);

		cublasCgeam(get_handle(), cublas_trans(trans), cublas_trans('N'),
			    M, N, (const cuComplex*)alpha, (const cuComplex*)A, lda, (const cuComplex*)zero, (const cuComplex*)B, ldb, (cuComplex*)B, ldb);

		cuda_free(zero);

		return;
	}
#endif
	UNUSED(trans);
	UNUSED(M);
	UNUSED(N);
	UNUSED(alpha);
	UNUSED(lda);
	UNUSED(A);
	UNUSED(ldb);
	UNUSED(B);

	assert(0);
}

//B = alpha * op(A)
void blas_smatcopy(char trans, long M, long N, float alpha, const float* A, long lda, float* B, long ldb)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		float zero = 0.;

		cublasSgeam(get_handle(), cublas_trans(trans), cublas_trans('N'),
			    M, N, &alpha, A, lda, &zero, B, ldb, B, ldb);

		return;
	}
#endif
	UNUSED(trans);
	UNUSED(M);
	UNUSED(N);
	UNUSED(alpha);
	UNUSED(lda);
	UNUSED(A);
	UNUSED(ldb);
	UNUSED(B);

	assert(0);
}

//B = alpha * op(A)
void blas2_smatcopy(char trans, long M, long N, const float* alpha, const float* A, long lda, float* B, long ldb)
{
#ifdef USE_CUDA
	if (cuda_ondevice(A)) {

		cublas_set_pointer_host();

		float* zero = cuda_malloc(4);
		cuda_clear(4, zero);

		cublasSgeam(get_handle(), cublas_trans(trans), cublas_trans('N'),
			    M, N, alpha, A, lda, zero, B, ldb, B, ldb);

		cuda_free(zero);

		return;
	}
#endif
	UNUSED(trans);
	UNUSED(M);
	UNUSED(N);
	UNUSED(alpha);
	UNUSED(lda);
	UNUSED(A);
	UNUSED(ldb);
	UNUSED(B);

	assert(0);
}

void blas_csyrk(char uplo, char trans, long N, long K, const complex float alpha, long lda, const complex float A[][lda], complex float beta, long ldc, complex float C[][ldc])
{
	assert('U' == uplo);
	assert(('T' == trans) || ('N' == trans));

	cblas_csyrk(CblasColMajor, CblasUpper, ('T' == trans) ? CblasTrans : CblasNoTrans, N, K, (void*)&alpha, (void*)A, lda, (void*)&beta, (void*)C, ldc);
}

void blas_sger_fmac(long M, long N, float* A, const float* x, const float* y)
{
	blas_sger(M, N, 1., 1, x, 1, y, M, A);
}

void blas_gemv_zfmac(long M, long N, complex float* y, const complex float* A, char trans, const complex float* x)
{
	assert((trans == 'N') || (trans == 'T') || (trans == 'C'));

	blas_cgemv(trans,M, N, 1., M, A, 1, x, 1., 1, y);
}

void blas_gemv_fmac(long M, long N, float* y, const float* A, char trans, const float* x)
{
	assert((trans == 'N') || (trans == 'T'));

	blas_sgemv(trans,M, N, 1., M, A, 1, x, 1., 1, y);
}

void blas_matrix_multiply(long M, long N, long K, complex float C[N][M], const complex float A[K][M], const complex float B[N][K])
{
	blas_cgemm('N', 'N', M, N, K, 1. , M, (const complex float*)A, K, (const complex float*)B, 0., M, (complex float*)C);
}

void blas_matrix_zfmac(long M, long N, long K, complex float* C, const complex float* A, char transa, const complex float* B, char transb)
{
	assert((transa == 'N') || (transa == 'T') || (transa == 'C'));
	assert((transb == 'N') || (transb == 'T') || (transb == 'C'));

	long lda = (transa == 'N' ? M: K);
	long ldb = (transb == 'N' ? K: N);

	blas_cgemm(transa, transb, M, N, K, 1., lda, A, ldb, B, 1., M, C);
}


#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <signal.h>
#include <string.h>
#include <math.h>
#include "cblas.h"
#include <lapacke.h>
#include <unistd.h>
#include <sys/types.h>

#include "spidriver_host.h"
#include "adcdriver_host.h"
#include "matrix_utils.h"

#define PI 3.1415926535

// Length of data buffer
#define NUMPTS 128

// Number of signal vectors
#define PSIG 2

// Sampling frequency.  Must match the sampling frequency commanded
// to the A/D.
#define FSAMP 15625

// Parameters used in searching for peak
#define MAXRECURSIONS 5
#define NGRID 25

//===========================================================
// Utility functions for MUSIC

//-----------------------------------------------------
void find_bracket(int N, float *u, int *ileft, int *iright) {
  // Given input vector y, this finds the max element,
  // then returns the indices of the vectors to its
  // left and right.

  int i;

  i = maxeltf(N, u);
  if (i == 0) {
    *ileft = 0;
    *iright = 1;
  } else if (i == (N-1)) {
    *ileft = N-2;
    *iright = N-1;
  } else {
    *ileft = i-1;
    *iright = i+1;
  }
  return;
}

//-----------------------------------------------------
void extract_noise_vectors(float *A, int m, int n, int c, float *E) {
  // This fcn takes input matrix A of size mxn.  It extracts the vectors
  // of A to the right, starting at col c, and puts the extracted
  // vectors into E.  E has size [m, n-c]

  //printf("Entered extract_noise_vectors, m = %d, n = %d, c = %d\n", m, n, c);
  int i, j, l;
  for (i = 0; i < m; i++) {
    for (j = c; j < n; j++) {
      // printf("i = %d, j = %d, A[i,j] = %f\n", i, j, MATRIX_ELEMENT(A, m, n, i, j));
      l = lindex(m, n-c, i, j-c);
      // printf("lindex(m, n-c, i,j-c) = %d\n", l);
      E[l] = MATRIX_ELEMENT(A, m, n, i, j);
    }
  }
}

//-----------------------------------------------------
float music_sum(float f, float *v, int Mr, int Mc) {
  // This performs the sum over noise space vectors.
  // Since complex numbers are not supported, I split
  // the computation into two parts, real and imag.

  int i, j;

  float *er;
  float *ei;
  float *vi;
  float s, tr, ti;

  // printf("--> Entered music_sum, [row, col] = [%d, %d]\n", Mr, Mc);

  er = (float*) malloc(Mr*sizeof(float));
  ei = (float*) malloc(Mr*sizeof(float));
  vi = (float*) malloc(Mr*sizeof(float));

  // Create e vector
  for(i=0; i<Mr; i++) {
    er[i] = cos(2*PI*i*f);
    ei[i] = sin(2*PI*i*f);
  }

  // Compute denominator
  s = 0;
  for(i=0; i<Mc; i++) {     // iterate over columns.

    // v is a matrix, so extract noise vector vi as column vector.
    for(j=0; j<Mr; j++) {   // iterate over rows
      vi[j] = v[lindex(Mr, Mc, j, i)];
    }
    tr = cblas_sdot(Mr, er, 1, vi, 1);
    ti = cblas_sdot(Mr, ei, 1, vi, 1);
    s = s + tr*tr + ti*ti;
  }

  free(er);
  free(ei);
  free(vi);

  // Must guard against returning inf or nan.
  return (1.0f/s);
}

//-----------------------------------------------------
// Called when Ctrl+C is pressed - triggers the program to stop.
void stopHandler(int sig) {
  adc_quit();
  exit(0);
}

//==========================================================
// This is the main program.  It runs a loop, takes a buffer
// of data from the A/D, then uses the MUSIC algorithm to
// compute the frequency of the input sine wave.
int main (void)
{
  // Loop variables
  uint32_t i, j;

  // Buffers for tx and rx data from A/D registers.
  uint32_t tx_buf[3];
  uint32_t rx_buf[4];

  // Used in LAPACK computations
  int m = NUMPTS;
  int info;
  float superb[NUMPTS-1];

  // Measured voltages from A/D
  float v[NUMPTS];           // Vector of measurements
  float Rxx[NUMPTS*NUMPTS];  // Covariance matrix.
  float U[NUMPTS * NUMPTS];
  float S[NUMPTS];
  float VT[NUMPTS * NUMPTS];

  float Nu[NUMPTS * (NUMPTS-PSIG)];  // Vector of noises

  // Used in finding peak corresponding to dominant frequency
  float f[NGRID];
  float Pmu[NGRID];
  int ileft, iright;
  float fleft, fright, fpeak;

  // Stuff used with "hit return when ready..."
  char dummy[8];

  printf("------------   Starting main.....   -------------\n");

  // Run until Ctrl+C pressed:
  signal(SIGINT, stopHandler);

  // Sanity check user.
  if(getuid()!=0){
     printf("You must run this program as root. Exiting.\n");
     exit(EXIT_FAILURE);
  }

  // Initialize A/D converter
  adc_config();
  adc_set_samplerate(SAMP_RATE_15625);
  adc_set_chan0();

  // Now loop forever, read buffer, and compute frequency.
  // printf("--------------------------------------------------\n");
  while(1) {

    // Do A/D read to fill buffer with NUMPTS measurements.
    adc_read_multiple(NUMPTS, v);
    //printf("Values read = \n");
    //for (i=0; i<NUMPTS; i++) {
    //  printf("i = %d, v = %e\n", i, v[i]);
    //}

    // Zero out Rxx prior to filling it using sger
    zeros(m, m, Rxx);

    // Create covariance matrix by doing outer product of v with
    // itself.
    cblas_sger(CblasRowMajor,    /* Row-major storage */
               m,                /* Row count for Rxx  */
               m,                /* Col count for Rxx  */
               1.0f,             /* scale factor to apply to v*v' */
               v,
               1,                /* stride between elements of v. */
               v,
               1,                /* stride between elements of v. */
               Rxx,
               m);               /* leading dimension of matrix Rxx. */
    //printf("\nMatrix Rxx (%d x %d) =\n", m, m);
    //print_matrix(Rxx, m, m);

    // Now compute SVD of Rxx.
    info = LAPACKE_sgesvd(LAPACK_ROW_MAJOR, 'A', 'A',
                  m, m, Rxx,
                  m, S, U, m,
                  VT, m, superb);
    if (info != 0)  {
      fprintf(stderr, "Error: dgesvd returned with a non-zero status (info = %d)\n", info);
      return(-1);
    }

    //printf("\nMatrix U (%d x %d) is:\n", m, m);
    //print_matrix(U, m, m);

    //printf("\nVector S (%d x %d) is:\n", m, 1);
    //print_matrix(S, m, 1);

    //printf("\nMatrix VT (%d x %d) is:\n", m, m);
    //print_matrix(VT, m, m);

    // Extract noise vectors here.  The noise vectors are held in Nu
    extract_noise_vectors(U, m, m, PSIG, Nu);
    //printf("\nMatrix Nu (%d x %d) is:\n", m, NUMPTS-PSIG);
    //print_matrix(Nu, m, NUMPTS-PSIG);

    // Now find max freq.  Set up initial grid endpoints.  Freqs are
    // in units of Hz.
    fleft = 0.0f;
    fright = FSAMP/2.0f;

    for (j=0; j<MAXRECURSIONS; j++) {
      // printf("fleft = %f, fright = %f\n", fleft, fright);

      // Set up search grid
      linspace(fleft, fright, NGRID, f);
      //printf("\nVector f =\n");
      //print_matrix(f, NGRID, 1);

      // Compute vector of amplitudes Pmu on grid.  music_sum wants normalized
      // frequencies
      for (i = 0; i < NGRID; i++) {
        Pmu[i] = music_sum(f[i]/FSAMP, Nu, NUMPTS, NUMPTS-PSIG);
      }
      //printf("\nVector Pmu =\n");
      //print_matrix(Pmu, NGRID, 1);

      find_bracket(NGRID, Pmu, &ileft, &iright);
      fleft = f[ileft];
      fright = f[iright];
    }
    fpeak = (fleft+fright)/2.0f;   // Assume peak is average of fleft and fright
    printf("Peak frequency found at f = %f Hz\n", fpeak);

    // usleep(500000);   // delay 1/2 sec.
  }

}