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

#ifndef __GSL_MIN_H__
#define __GSL_MIN_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_math.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
  {
    const char *name;
    size_t size;
    int (*set) (void *state, gsl_function * f, double x_minimum, double f_minimum, double x_lower, double f_lower, double x_upper, double f_upper);
    int (*iterate) (void *state, gsl_function * f, double * x_minimum, double * f_minimum, double * x_lower, double * f_lower, double * x_upper, double * f_upper);
  }
gsl_min_fminimizer_type;

typedef struct
  {
    const gsl_min_fminimizer_type * type;
    gsl_function * function ;
    double x_minimum ;
    double x_lower ;
    double x_upper ;
    double f_minimum, f_lower, f_upper;
    void *state;
  }
gsl_min_fminimizer;

gsl_min_fminimizer *
gsl_min_fminimizer_alloc (const gsl_min_fminimizer_type * T) ;

void gsl_min_fminimizer_free (gsl_min_fminimizer * s);

int gsl_min_fminimizer_set (gsl_min_fminimizer * s,
                            gsl_function * f, double x_minimum,
                            double x_lower, double x_upper);

int gsl_min_fminimizer_set_with_values (gsl_min_fminimizer * s,
                                        gsl_function * f,
                                        double x_minimum, double f_minimum,
                                        double x_lower, double f_lower,
                                        double x_upper, double f_upper);

int gsl_min_fminimizer_iterate (gsl_min_fminimizer * s);

const char * gsl_min_fminimizer_name (const gsl_min_fminimizer * s);

double gsl_min_fminimizer_x_minimum (const gsl_min_fminimizer * s);
double gsl_min_fminimizer_x_lower (const gsl_min_fminimizer * s);
double gsl_min_fminimizer_x_upper (const gsl_min_fminimizer * s);
double gsl_min_fminimizer_f_minimum (const gsl_min_fminimizer * s);
double gsl_min_fminimizer_f_lower (const gsl_min_fminimizer * s);
double gsl_min_fminimizer_f_upper (const gsl_min_fminimizer * s);

/* Deprecated, use x_minimum instead */
double gsl_min_fminimizer_minimum (const gsl_min_fminimizer * s);

int
gsl_min_test_interval (double x_lower, double x_upper, double epsabs, double epsrel);

GSL_VAR const gsl_min_fminimizer_type  * gsl_min_fminimizer_goldensection;
GSL_VAR const gsl_min_fminimizer_type  * gsl_min_fminimizer_brent;
GSL_VAR const gsl_min_fminimizer_type  * gsl_min_fminimizer_quad_golden;

typedef
int (*gsl_min_bracketing_function)(gsl_function *f,
                                   double *x_minimum,double * f_minimum,
                                   double *x_lower, double * f_lower,
                                   double *x_upper, double * f_upper,
                                   size_t eval_max);

int
gsl_min_find_bracket(gsl_function *f,double *x_minimum,double * f_minimum,
                     double *x_lower, double * f_lower,
                     double *x_upper, double * f_upper,
                     size_t eval_max);

__END_DECLS

#endif /* __GSL_MIN_H__ */


#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/theory/basics.c"
#include "../cosmolike_core/theory/structs.c"
#include "../cosmolike_core/theory/parameters.c"
#include "../cosmolike_core/emu17/P_cb/emu.c"
#include "../cosmolike_core/theory/recompute.c"
#include "../cosmolike_core/theory/cosmo3D.c"
#include "../cosmolike_core/theory/redshift_spline.c"
#include "../cosmolike_core/theory/halo.c"
#include "../cosmolike_core/theory/HOD.c"
#include "../cosmolike_core/theory/pt.c"
#include "../cosmolike_core/theory/cosmo2D_fourier.c"
#include "../cosmolike_core/theory/IA.c"
#include "../cosmolike_core/theory/cluster.c"
#include "../cosmolike_core/theory/BAO.c"
#include "../cosmolike_core/theory/external_prior.c"
#include "../cosmolike_core/theory/init_baryon.c"
#include "init_emu.c"

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);
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_cluster_N(double *data, int start);
void set_data_cgl(double *ell_Cluster, 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 write_vector_wrapper(char *details, input_cosmo_params ic, input_nuisance_params in);
double log_like_wrapper(input_cosmo_params ic, input_nuisance_params 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);
  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==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;
}

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);
    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.;
      }
    }
  }
}

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_cluster_N(double *data, int start){
  int nN, nz;
  for (nz = 0; nz < tomo.cluster_Nbin; nz++){
    for (nN = 0; nN < Cluster.N200_Nbin; nN++){
      data[start+Cluster.N200_Nbin*nz+nN] = N_N200(nz, nN);
    }
  }
}

void set_data_cgl(double *ell_Cluster, double *data, int start)
{
  int zl,zs,nN,nz,i,j;
  for(nN = 0; nN < Cluster.N200_Nbin; nN++){
    for (nz = 0; nz < tomo.cgl_Npowerspectra; nz++){
      zl = ZC(nz); zs = ZSC(nz);
      for (i = 0; i < Cluster.lbin; i++){
        j = start;
        j += (nz*Cluster.N200_Nbin+nN)*Cluster.lbin +i;
        data[j] = C_cgl_tomo_sys(ell_Cluster[i],zl,nN,zs);
      }
    }
  }
}

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.04 || cosmology.Omega_m > 0.7) return 0;
  if (cosmology.omb < 0.04 || cosmology.omb > 0.055) return 0;
  if (cosmology.sigma_8 < 0.4 || cosmology.sigma_8 > 1.2) return 0;
  if (cosmology.n_spec < 0.84 || cosmology.n_spec > 1.06) return 0;
  if (cosmology.w0 < -2.1 || cosmology.w0 > -0.0) return 0;
  if (cosmology.wa < -2.6 || cosmology.wa > 2.6) return 0;
  if (cosmology.h0 < 0.4 || cosmology.h0 > 0.9) return 0;

  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.000001) 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 < -1.0 || nuisance.beta_ia > 3.0) return 0;
  if (nuisance.eta_ia < -3.0 || nuisance.eta_ia> 3.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;
  if(like.bias==1){
    for (i = 0; i < 10; i++){
      if (gbias.b[i] < 0.8 || gbias.b[i] > 3.0) return 0;
    }
  }
  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)
{
  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;
  }

  //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;

  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  if(like.shear_pos==1){
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  if(like.pos_pos==1){
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }

  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));
    // }
  }
  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)
{

  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]);
// }

  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);

  int start=0;
  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  if(like.shear_pos==1){
    //printf("ggl\n");
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  if(like.pos_pos==1){
    //printf("clustering\n");
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }

  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 ic, input_nuisance_params 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]);
}

double log_like_wrapper(input_cosmo_params ic, input_nuisance_params 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]);

  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)
{
  clock_t begin, end;
  double time_spent;
  int i;
  char arg1[400],arg2[400],arg3[400];
/* here, do your time-consuming job */
  int sce=atoi(argv[1]);

  int N_scenarios=36;

  double area_table[36]={7623.22,14786.3,9931.47,8585.43,17681.8,15126.9,9747.99,8335.08,9533.42,18331.3,12867.8,17418.9,19783.1,12538.8,15260.0,16540.7,19636.8,11112.7,10385.5,16140.2,18920.1,17976.2,11352.0,9214.77,16910.7,11995.6,16199.8,14395.1,8133.86,13510.5,19122.3,15684.5,12014.8,14059.7,10919.3,13212.7};

  double nsource_table[36]={13.991,33.3975,17.069,28.4875,35.3643,10.3802,11.1105,29.5904,31.4608,15.7463,13.3066,9.07218,9.58719,16.3234,25.8327,10.7415,38.0412,32.8821,19.8893,27.0369,15.1711,14.2418,19.1851,26.926,22.0012,12.6553,18.6304,11.9787,36.8728,22.5265,17.4381,12.3424,10.0095,23.5979,20.8771,24.8956};

  double nlens_table[36]={22.9726 ,61.5948 ,28.7809 ,51.4354 ,65.7226 ,16.3777 ,17.6899 ,53.6984 ,57.562 ,26.266 ,21.703 ,14.0587 ,14.9667 ,27.36 ,46.0364 ,17.0253 ,71.39 ,60.5184 ,34.2283 ,48.4767 ,25.1812 ,23.44 ,32.8578 ,48.2513 ,38.3766 ,20.5029 ,31.783 ,19.2647 ,68.9094 ,39.4168 ,29.4874 ,19.9292 ,15.7162 ,41.5487 ,36.1616 ,44.148};

  char survey_designation[1][200]={"LSST"};
  char tomo_binning_source[1][200]={"source_std"};
  char tomo_binning_lens[1][200]={"LSST_gold"};

  char source_zfile[36][400]={"wl_redshift_model0_WLz01.880307e-01_WLalpha8.485694e-01.txt", "wl_redshift_model1_WLz01.731166e-01_WLalpha7.662434e-01.txt", "wl_redshift_model2_WLz01.846221e-01_WLalpha8.297540e-01.txt", "wl_redshift_model3_WLz01.758423e-01_WLalpha7.812893e-01.txt", "wl_redshift_model4_WLz01.721357e-01_WLalpha7.608290e-01.txt", "wl_redshift_model5_WLz01.931476e-01_WLalpha8.768147e-01.txt", "wl_redshift_model6_WLz01.919821e-01_WLalpha8.703814e-01.txt", "wl_redshift_model7_WLz01.751912e-01_WLalpha7.776952e-01.txt", "wl_redshift_model8_WLz01.741405e-01_WLalpha7.718958e-01.txt", "wl_redshift_model9_WLz01.860048e-01_WLalpha8.373865e-01.txt", "wl_redshift_model10_WLz01.888904e-01_WLalpha8.533151e-01.txt", "wl_redshift_model11_WLz01.954564e-01_WLalpha8.895592e-01.txt", "wl_redshift_model12_WLz01.945099e-01_WLalpha8.843347e-01.txt", "wl_redshift_model13_WLz01.853877e-01_WLalpha8.339802e-01.txt", "wl_redshift_model14_WLz01.775192e-01_WLalpha7.905458e-01.txt", "wl_redshift_model15_WLz01.925611e-01_WLalpha8.735775e-01.txt", "wl_redshift_model16_WLz01.708849e-01_WLalpha7.539247e-01.txt", "wl_redshift_model17_WLz01.733832e-01_WLalpha7.677150e-01.txt", "wl_redshift_model18_WLz01.820009e-01_WLalpha8.152851e-01.txt", "wl_redshift_model19_WLz01.767381e-01_WLalpha7.862345e-01.txt", "wl_redshift_model20_WLz01.866426e-01_WLalpha8.409073e-01.txt", "wl_redshift_model21_WLz01.877261e-01_WLalpha8.468882e-01.txt", "wl_redshift_model22_WLz01.826188e-01_WLalpha8.186960e-01.txt", "wl_redshift_model23_WLz01.768086e-01_WLalpha7.866234e-01.txt", "wl_redshift_model24_WLz01.802711e-01_WLalpha8.057362e-01.txt", "wl_redshift_model25_WLz01.897506e-01_WLalpha8.580632e-01.txt", "wl_redshift_model26_WLz01.831217e-01_WLalpha8.214720e-01.txt", "wl_redshift_model27_WLz01.906926e-01_WLalpha8.632630e-01.txt", "wl_redshift_model28_WLz01.714197e-01_WLalpha7.568766e-01.txt", "wl_redshift_model29_WLz01.798667e-01_WLalpha8.035040e-01.txt", "wl_redshift_model30_WLz01.842554e-01_WLalpha8.277299e-01.txt", "wl_redshift_model31_WLz01.901797e-01_WLalpha8.604322e-01.txt", "wl_redshift_model32_WLz01.937710e-01_WLalpha8.802561e-01.txt", "wl_redshift_model33_WLz01.790701e-01_WLalpha7.991072e-01.txt", "wl_redshift_model34_WLz01.811701e-01_WLalpha8.106987e-01.txt", "wl_redshift_model35_WLz01.781525e-01_WLalpha7.940419e-01.txt"};

  char lens_zfile[36][400]={"LSS_redshift_model0_LSSz02.629496e-01_LSSalpha9.285983e-01.txt","LSS_redshift_model1_LSSz02.852923e-01_LSSalpha8.985943e-01.txt","LSS_redshift_model2_LSSz02.664822e-01_LSSalpha9.186036e-01.txt","LSS_redshift_model3_LSSz02.798758e-01_LSSalpha9.014201e-01.txt","LSS_redshift_model4_LSSz02.873873e-01_LSSalpha8.978683e-01.txt","LSS_redshift_model5_LSSz02.593970e-01_LSSalpha9.470921e-01.txt","LSS_redshift_model6_LSSz02.600213e-01_LSSalpha9.425114e-01.txt","LSS_redshift_model7_LSSz02.811155e-01_LSSalpha9.006370e-01.txt","LSS_redshift_model8_LSSz02.831875e-01_LSSalpha8.995165e-01.txt","LSS_redshift_model9_LSSz02.649368e-01_LSSalpha9.224338e-01.txt","LSS_redshift_model10_LSSz02.622058e-01_LSSalpha9.314128e-01.txt","LSS_redshift_model11_LSSz02.584820e-01_LSSalpha9.568082e-01.txt","LSS_redshift_model12_LSSz02.588053e-01_LSSalpha9.527220e-01.txt","LSS_redshift_model13_LSSz02.656075e-01_LSSalpha9.206866e-01.txt","LSS_redshift_model14_LSSz02.768397e-01_LSSalpha9.037492e-01.txt","LSS_redshift_model15_LSSz02.596975e-01_LSSalpha9.447600e-01.txt","LSS_redshift_model16_LSSz02.901709e-01_LSSalpha8.971658e-01.txt","LSS_redshift_model17_LSSz02.847362e-01_LSSalpha8.988183e-01.txt","LSS_redshift_model18_LSSz02.698330e-01_LSSalpha9.121821e-01.txt","LSS_redshift_model19_LSSz02.782257e-01_LSSalpha9.026084e-01.txt","LSS_redshift_model20_LSSz02.642756e-01_LSSalpha9.243038e-01.txt","LSS_redshift_model21_LSSz02.632273e-01_LSSalpha9.276296e-01.txt","LSS_redshift_model22_LSSz02.689934e-01_LSSalpha9.135968e-01.txt","LSS_redshift_model23_LSSz02.780987e-01_LSSalpha9.027073e-01.txt","LSS_redshift_model24_LSSz02.723464e-01_LSSalpha9.085463e-01.txt","LSS_redshift_model25_LSSz02.615210e-01_LSSalpha9.343470e-01.txt","LSS_redshift_model26_LSSz02.683327e-01_LSSalpha9.147934e-01.txt","LSS_redshift_model27_LSSz02.608392e-01_LSSalpha9.376962e-01.txt","LSS_redshift_model28_LSSz02.889655e-01_LSSalpha8.974355e-01.txt","LSS_redshift_model29_LSSz02.729686e-01_LSSalpha9.077654e-01.txt","LSS_redshift_model30_LSSz02.669178e-01_LSSalpha9.176391e-01.txt","LSS_redshift_model31_LSSz02.612016e-01_LSSalpha9.358553e-01.txt","LSS_redshift_model32_LSSz02.591077e-01_LSSalpha9.496317e-01.txt","LSS_redshift_model33_LSSz02.742326e-01_LSSalpha9.063038e-01.txt","LSS_redshift_model34_LSSz02.710103e-01_LSSalpha9.103760e-01.txt","LSS_redshift_model35_LSSz02.757517e-01_LSSalpha9.047459e-01.txt"};

double shear_prior[36]={0.00891915,0.0104498 ,0.0145972 ,0.0191916 ,0.00450246 ,0.00567828 ,0.00294841 ,0.00530922 ,0.0118632 ,0.0151849 ,0.00410151 ,0.0170622 ,0.0197331 ,0.0106615 ,0.0124445 ,0.00994507 ,0.0136251 ,0.0143491 ,0.0164314 ,0.016962 ,0.0186608 ,0.00945903 ,0.0113246 ,0.0155225 ,0.00800846 ,0.00732104 ,0.00649453 ,0.00243976 ,0.0125932 ,0.0182587 ,0.00335859 ,0.00682287 ,0.0177269 ,0.0035219 ,0.00773304 ,0.0134886};

double delta_z_prior[36]={0.0032537,0.00135316,0.00168787,0.00215043,0.00406031,0.00222358,0.00334993,0.00255186,0.00266499,0.00159226,0.00183664,0.00384965,0.00427765,0.00314377,0.00456113,0.00347868,0.00487938,0.00418152,0.00469911,0.00367598,0.0028009,0.00234161,0.00194964,0.00200982,0.00122739,0.00310886,0.00275168,0.00492736,0.00437241,0.00113931,0.00104864,0.00292328,0.00452082,0.00394114,0.00150756,0.003613};

double sigma_z[36]={0.0849973 ,0.0986032 ,0.0875521 ,0.0968222 ,0.0225239 ,0.0718278 ,0.0733675 ,0.0385274 ,0.0425549 ,0.0605867 ,0.0178555 ,0.0853407 ,0.0124119 ,0.0531027 ,0.0304032 ,0.0503145 ,0.0132213 ,0.0941765 ,0.0416444 ,0.0668198 ,0.063227 ,0.0291332 ,0.0481633 ,0.0595606 ,0.0818742 ,0.0472518 ,0.0270185 ,0.0767401 ,0.0219945 ,0.0902663 ,0.0779705 ,0.0337666 ,0.0362358 ,0.0692429 ,0.0558841 ,0.0150457};

double sigma_z_prior[36]={0.00331909,0.00529541,0.00478151,0.00437497,0.00443062,0.00486333,0.00467423,0.0036723,0.00426963,0.00515357,0.0054553,0.00310132,0.00305971,0.00406327,0.00594293,0.00348709,0.00562526,0.00396025,0.00540537,0.00500447,0.00318595,0.00460592,0.00412137,0.00336418,0.00524988,0.00390092,0.00498349,0.0056667,0.0036384,0.00455861,0.00554822,0.00381061,0.0057615,0.00357705,0.00590572,0.00422393};

  init_cosmo_runmode("halofit");
  init_bary("dmo");
  init_binning_fourier(15,20.0,3000.0,3000.0,21.0,10,10);
  init_priors(shear_prior[sce],sigma_z[sce],delta_z_prior[sce],sigma_z_prior[sce],sigma_z[sce]*0.6,delta_z_prior[sce],sigma_z_prior[sce],3.0,1.2,3.8,2.0,16.0,5.0,0.8);
  init_survey(survey_designation[0],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,"gaussian","gaussian",tomo_binning_source[0],tomo_binning_lens[0]);
  init_IA("NLA_HF","GAMA");
  init_probes("3x2pt");

  //init_Pdelta("emu",0.8,0.35);
  // init_Pdelta("linear",0.8,0.35);

  // for (i =0; i< 10; i++){
  //   save_zdistr_sources(i);
  // }
  // for (i =0; i< 10; i++){
  //   save_zdistr_lenses(i);
  // }
//   double Omega;
//   char filename[300];
// for (i=0;i<200; i++){
//   Omega=0.05+i*0.002;
//   sprintf(filename,"test_fid_%d", i);
//compute_data_vector(filename,Omega,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0,2.1,2.2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,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,3.207,0.993,0.0,0.456,0.0,0.0);
// }

sprintf(arg3,"Y10_area%le",area_table[sce]);
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_z[sce],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,sigma_z[sce]*0.6,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,3.207,0.993,0.0,0.456,0.0,0.0,0.0,0.0,0.0);

// compute_data_vector("mu1_Sigma0",0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,1.,1.35,1.5,1.65,1.8,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.01,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,1.72+log(1.e+14*0.7),1.08,0.0,0.25,0.9,0.9,0.9,0.9);
  // compute_data_vector("mu1_Sigma1",0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,1.,1.,1.35,1.5,1.65,1.8,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.01,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,1.72+log(1.e+14*0.7),1.08,0.0,0.25,0.9,0.9,0.9,0.9);

  // init_data_inv("cov/WFIRST_3x2pt_clusterN_clusterWL_inv","datav/WFIRST_all_2pt_clusterN_clusterWL_fid");

  // begin = clock();
  // log_multi_like(0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0,2.1,2.2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,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,3.207,0.993,0.0,0.456,0.0,0.0);
  // // printf("knonlin %le\n",nonlinear_scale_computation(1.0));
  // // printf("knonlin %le\n",nonlinear_scale_computation(0.5));
  // end = clock();
  // time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
  // printf("timespent %le\n",time_spent);

  return 0;
}


#ifndef PDCD_SMSVM_H
#define PDCD_SMSVM_H

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

//This class implements the method SMART_CD.

/*
The optimization problem to solve is of the form f(x)+g(x)+h(Mx) ;
 where f(x)=sum_{j=1}^m lambda_f[j] phi_j(<A_j,x>) and g(x)=sum_{i=1}^n g_i(x_i), h(x)= |x|_1. We all assume that each phi_j is 1-smooth.
*/

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

  std::vector<D> Au;
  std::vector<D> Az;

  std::vector<D> Mu;
  std::vector<D> Mz;

  std::vector<D> Mx;
  std::vector<D> Ax;
  std::vector<D> lambda_f;

protected:

  // parameters

  D mu_f;

  D mu_psi;

  D mu_g;

  L n;   // x\in \R^n

  L tau;  //number of threads on each node/computer

  D sumofLi;

  // variables
  std::vector<D> u;

  std::vector<D> z;

  std::vector<D> x;

  std::vector<D> v;

  std::vector<D> L_M;

  std::vector<D> L_f;

  std::vector<D> t;

  D gamma;

  D theta;

  D theta0;

  L m_1;

  L m_2;

  // sampling variables

  std::vector<D> proba_vector;

  std::vector<D> S;

  std::vector<D> all_n;

  std::vector<D> sampled;

  D max_p;

  D min_p;

  // auxiliary variables

  L max_nb_loops;

  L evaluation;

  ofstream samp;

  L nb_iters;

  L nb_of_iters_per_loop;

  L print_every_N;

  D running_time;

  Matrix<L,D> data_A;

  Matrix<L, D> data_M;

  std::vector<D> lambda;

  std::vector<D> M_tlambda;

  D beta_s;

  D function_value;

  D residual1;

  D residual2;

public:

  gsl_rng * rng;

  virtual inline D gradient_of_phi_j(D, L){return D(NULL);}

  virtual inline D value_of_g_i(D, L){return D(NULL);}
  virtual inline D value_of_phi_j(D, L){return D(NULL);}

  virtual inline D prox_of_g_i(D,D,D, L){return D(NULL);}
  virtual inline D value_of_phistar_i(D,L) {return D(NULL);}

  virtual inline void set_matrix_M(){}

  virtual inline void set_matrix_A(){}
  PDCD_SMSVM()
  {

  }

  void set_rng()
  {
    gsl_rng_env_setup();
    const gsl_rng_type * T;
    T = gsl_rng_default;
    rng = gsl_rng_alloc(T);
    gsl_rng_set(rng,time(NULL));
    //gsl_rng_set(rng, 27432042);

  }

// sample i with probability pi=proba_vector[i]
  L sampling()
  {
    //L i=(floor)(gsl_rng_uniform(rng)*n);
    L i=gsl_rng_uniform_int(rng, n);
    if(tau==1)
    {
      D y=gsl_rng_uniform(rng);
      while(y*max_p>proba_vector[i])
      {
        i=(floor)(gsl_rng_uniform(rng)*n);
        y=gsl_rng_uniform(rng);
      }
    }
    return i;
  }

// sample S
  void batch_sampling()
  {
    if(tau<n)
    {
      L i=sampling();
      for(L k=0;k<tau;k++)
      {
        while(sampled[i]==1)
        {
          i=sampling();
        }
        sampled[i]=1;
        S[k]=i;
      }
      for(L k=0;k<tau;k++)
      {
        sampled[S[k]]=0;
      }
    }
    else {
      S=all_n;
      //cout<<"s=all_n"<<endl;
    }
  }

  void compute_x()
  {
    for(L i=0;i<n;i++){
    x[i]=gamma*u[i]+z[i];
	}
	for(L j=0;j<m_2;j++){
        Mx[j]=gamma*Mu[j]+Mz[j];
    }
    for(L j=0;j<m_1;j++){
        Ax[j]=gamma*Au[j]+Az[j];
    }
  }

  inline void compute_function_value() {
     D res=0;
     D res2= 0;
     D tmp1= 0;
     for(L i=0;i<this->n;i++){
       res+=value_of_g_i(x[i],i);
     }
     for(L j=0;j<m_1;j++){
       res+=lambda_f[j]*value_of_phi_j(Ax[j],j);
     }
     for(L j=0;j<m_2;j++){
     	if (1- Mx[j]> tmp1){
     		tmp1= 1- Mx[j];
		 }
        res2+=max(0.0,1- Mx[j]);
     }
     residual1= tmp1;
     residual2= res2/m_2;
     function_value=res+ m_2*residual2;;
   }

  void initialize(D beta_0, vector<D> & x0, vector<D> & y0, L val_tau, D p_N, D val_lambda_f)
  {
  	set_matrix_M();
    set_matrix_A();

    this->tau=val_tau;
    m_1=data_A.get_n();
    m_2=data_M.get_n();
    cout<<"m_1="<<m_1<<endl;
    cout<<"m_2="<<m_2<<endl;
    lambda_f.resize(m_1,val_lambda_f);
    n=data_A.nfeatures;
    tau=val_tau;
    all_n.resize(n,0);
    for(L i=0;i<n;i++)
    all_n[i]=i;
    print_every_N= p_N;
    nb_of_iters_per_loop=floor(max(1.,n/(tau+0.0)));
    lambda.resize(m_2,0);
    for(L j=0;j<m_2;j++)
           lambda[j]=y0[j];

    u.clear();
    u.resize(n,0);
    z.clear();
    z.resize(n,0);
    x.clear();
    x.resize(n,0);
    for(L i=0;i<n;i++)
    {
      z[i]=x0[i];
      x[i]=x0[i];
    }
    sampled.clear();
    sampled.resize(n,0);
    S.clear();
    S.resize(tau,0);
    t.clear();
    t.resize(n,0);
    M_tlambda.resize(n,0);
    compute_Mty();

    Au.clear();
    Au.resize(m_1,0);
    Az.clear();
    Az.resize(m_1,0);
    Ax.clear();
    Ax.resize(m_1,0);
    Mu.clear();
    Mu.resize(m_2,0);
    Mz.clear();
    Mz.resize(m_2,0);
    Mx.clear();
    Mx.resize(m_2,0);
    compute_Az(x0);
    compute_Mz(x0);
    compute_Ax(x0);
    compute_Mx(x0);
    beta_s= beta_0;
    set_v();
    set_p();
    set_rng();

    theta0= min_p;
    theta= theta0;
    cout<< "theta0= "<< theta0<< endl;
    gamma= 1- theta0;

    cout<<"finished SMART_CD initializing"<<endl;
  }

  inline void compute_and_record_res(){
  	    if (nb_iters%print_every_N== 0){
  	    cout<< "gamma= "<< gamma<< " theta= "<< theta<< " beta_s= "<< beta_s<< endl;
        //compute_KKT_residual();
        compute_function_value();
        cout<<setprecision(9)<<"Iteration: "<<nb_iters<<"; time="<<running_time<<"; infeasibility= "<< residual1<<" "<< residual2<<"; function value="<<function_value<< endl;
        samp<<setprecision(9)<< nb_iters<<" "<<running_time<<" "<<residual1<< " "<< residual2<< " "<< function_value<<" "<<endl;
        }
   }

  inline D partial_i_of_f(L i)
  {
  	D res=0;
      for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
      {
        L j=data_A.col_idx[k];
        D tmp=lambda_f[j]*gradient_of_phi_j(gamma*Au[j]+Az[j], j);
        res+=data_A.A_t[k]*tmp;
      }
      for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
      {
        L j=data_M.col_idx[k];
        D tmp=(gamma*Mu[j]+Mz[j]- 1)/beta_s+lambda[j];
        if (tmp> 0){
        	tmp= 0;
		}
		else if( tmp< -1){
			tmp= -1;
		}
        res+=data_M.A_t[k]*tmp;
      }
      return res;
  }

  inline void set_v()
   {
     v.resize(n,0);
     L_f.resize(n,0);
     L_M.resize(n,0);
     D maxv=0;
     D minv=std::numeric_limits<double>::max();
     D sumv=0;
     D sumvi1=0;
     L sumw=0;
     L maxw=0;
     L minw=n;
     sumofLi=0;
     for(L j=0;j<m_1;j++)
     {
       sumw+=data_A.w_t[j];
       maxw=max(maxw,data_A.w_t[j]);
       minw=min(minw,data_A.w_t[j]);
     }
     cout<<"sumw="<<sumw<<";  maxw="<<maxw<<"; minw="<<minw<<endl;
     for(L i=0;i<n;i++)
     {
       D lfi=0;
       D lmi=0;
       D lfi1= 0;
       D lmi1= 0;
       for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
       {
         L j=data_A.col_idx[k];
         lfi+=(1.+(data_A.w_t[j]-1.)*(tau-1.)/max(n-1.,1.))*data_A.A_t[k]*data_A.A_t[k]*lambda_f[j];
         lfi1+=data_A.A_t[k]*data_A.A_t[k]*lambda_f[j];
       }
       for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
       {
         L j=data_M.col_idx[k];
         lmi+=(1.+(data_M.w_t[j]-1.)*(tau-1.)/max(n-1.,1.))*data_M.A_t[k]*data_M.A_t[k];
         lmi1+=data_M.A_t[k]*data_M.A_t[k];
       }
       L_f[i]=lfi;
       L_M[i]= lmi;
       sumv+=lfi;
       sumvi1+=lfi1;
       if(maxv<lfi) maxv=lfi;
       if(minv>lfi) minv=lfi;
     }
     if(tau==n){
          for(L i=0;i<n;i++)
              L_f[i]=sumvi1;
     }
     sumofLi=sumvi1;
     cout<<"  max of v: "<<maxv<<" ;  min of v: "<<minv<<" ;  sumofv: "<<sumv<<" sumofLi="<<sumofLi<<endl;
   }

   inline void set_p(){
     proba_vector.resize(n,0.0);
     D res= 0;
     D tmp= 1;
     D tmp2= 0;
     for (L i=0; i< n; i++){
     	res+= sqrt(L_f[i]+ L_M[i]/beta_s);
	 }
	 for (L i= 0; i< n; i++){
	 	proba_vector[i]= sqrt(L_f[i]+ L_M[i]/beta_s)/res;
	 	if (proba_vector[i]< tmp){
	 		tmp= proba_vector[i];
		 }
		if (proba_vector[i]> tmp2){
	 		tmp2= proba_vector[i];
		 }
	 }
	 if (tmp== 0){
	 	for (L i= 0; i< n; i++){
	 	proba_vector[i]= 1.0/n;
	    }
	    min_p= 1.0/n;
	    max_p= 1.0/n;
	 }
	 else{
	    min_p= tmp;
	    max_p= tmp2;
     }
	 cout<< "max_p= "<< max_p<< "; min_p= "<< min_p<< endl;
   }

  inline void update_z_coordinate( L i, D dz){
     z[i]+=dz;
     for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       L j=data_A.col_idx[k];
       Az[j]+=dz*data_A.A_t[k];
     }
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       L j=data_M.col_idx[k];
       Mz[j]+=dz*data_M.A_t[k];
       //if(j==2869) cout<<"gamma="<<gamma<<"; "<<dz<<"; "<<Mz[j]<<endl;
     }
   }

    inline void update_x_coordinate( L i, D dx){
     x[i]+=dx;
     x[i]=x[i];
     L j;
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       j=data_M.col_idx[k];
       Mx[j]+=dx*data_M.A_t[k];
     }
      for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       j=data_A.col_idx[k];
       Ax[j]+=dx*data_A.A_t[k];
     }
   }

   inline void update_u_coordinate( L i, D du){
     u[i]+=du;
     for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       L j=data_A.col_idx[k];
       Au[j]+=du*data_A.A_t[k];
     }
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       L j=data_M.col_idx[k];
       Mu[j]+=du*data_M.A_t[k];
     }
   }

   void compute_Au(){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Au[j]+=u[kj]*data_A.A[k];
       }
   }

   void compute_Mu(){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mu[j]+=u[kj]*data_M.A[k];
       }
   }

   void compute_Az(){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Az[j]+=z[kj]*data_A.A[k];
       }
   }
   void compute_Mz(){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mz[j]+=z[kj]*data_M.A[k];
       }
   }

   void compute_Au(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Au[j]+=x0[kj]*data_A.A[k];
       }

   }

   void compute_Az(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Az[j]+=x0[kj]*data_A.A[k];
       }
   }
   void compute_Mu(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mu[j]+=x0[kj]*data_M.A[k];
       }

   }

   void compute_Mz(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mz[j]+=x0[kj]*data_M.A[k];
       }
   }

     void compute_Mx(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mx[j]+=x0[kj]*data_M.A[k];
       }
   }

   void compute_Ax(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Ax[j]+=x0[kj]*data_A.A[k];
       }
   }

  void compute_Mty(){
        M_tlambda.clear();
        M_tlambda.resize(this->n,0);
        for(L i=0;i<this->n;i++)
        {
          for(L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++){
            L j=data_M.col_idx[k];
            M_tlambda[i]+=lambda[j]*data_M.A_t[k];
           }
        }

    }

    void update_theta()
  {

	 theta= (sqrt(theta*theta*theta*theta+ 4*theta*theta)- theta*theta)/2;

  }

  void reset(){
  	beta_s= beta_s/(1+ theta);
  }

  void PDCD_SMSVM_solver(D beta_0, vector<D> & x0,vector<D> & y0, L val_tau, L max_nb_epoch, L p_N, D val_lambda_f,string filename1, D time2)
  {
    initialize(beta_0, x0, y0, val_tau,  p_N, val_lambda_f);
    cout<<"running SMART_CD"<<" ; "<<filename1<<" max_nb_epoch "<<max_nb_epoch<<endl;
    nb_iters=0;
    filename1="results/PDCD_SMSVM_"+filename1;
    samp.open(filename1.c_str());
    //srand48(27432042);
    srand(time(NULL));
    running_time= 0;
    print_every_N=p_N;
    compute_and_record_res();
    D start;
    start = std::clock();
    while(nb_iters<max_nb_epoch)
    {
      start = std::clock();
      D ti=0;
      //D si=0;
      D gi=0;
      D Li=0;
      L i=0;
      for(L it=0;it<nb_of_iters_per_loop;it++)
      {
        gamma*=(1-theta);
        if(theta==1) gamma=1;
        batch_sampling();
        for(L it_S=0;it_S<tau;it_S++)
        {
          i=S[it_S];
          gi=partial_i_of_f(i);
          Li=(L_f[i]+ L_M[i]/beta_s)*theta/theta0;
          t[i]=prox_of_g_i(gi, Li, z[i],i);
        }
        for(L it_S=0;it_S<tau;it_S++)
        {
          i=S[it_S];
          ti=t[i];
          update_z_coordinate(i, ti);
          update_u_coordinate(i, -(1- theta/theta0)/gamma*ti);
        }
        update_theta();
        reset();
      }
      running_time+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
      nb_iters++;
      compute_x();
      compute_and_record_res();
      if (running_time> time2){
      	break;
	  }
    }

  }

};

#endif


#include <stdio.h>
#include <time.h>
#include <linux/limits.h>
#include <sys/stat.h>
#include <unistd.h>
#include <math.h>
#include "Globals.h"
#include "constitutive_equations.h"
#include "Watershed.h"
#include "manufactured_solution.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>

extern void GetLGLWeights(int N, double* w);

void *xcalloc(int items, int size)
{
	void *ptr = calloc(items, size);
	if (ptr == NULL)
	{
		printf("Unable to allocate memory\n");
		exit(EXIT_FAILURE);
	}
	return ptr;
}

static int firstOutput = 1;
static char folderName[500];
static char ChannelHeightFileName[PATH_MAX + 1];
static char ChannelQFileName[PATH_MAX + 1];
static char KinFileName[PATH_MAX + 1];
static char JuncHeightFileName[PATH_MAX + 1];
static char JuncQFileName[PATH_MAX + 1];
static char FpHeightFileName[PATH_MAX + 1];
static char FpQFileName[PATH_MAX + 1];

void outputFloodplainDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

	// Output Floodplain data
	FILE *file1 = fopen(FpHeightFileName, openFileFormat);
	FILE *file2 = fopen(FpQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Floodplains = %d\n", NumFloodplains);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "Number of Floodplains = %d\n", NumFloodplains);
		for (int j = 0; j < NumFloodplains; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumEl);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumEl);
		}
	}
	for (int i=0; i < NumFloodplains; i++)
	{
		fprintf(file1, "# Average H and Zeta at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Average Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j < FloodplainList[i]->NumEl; ++j)
		{
			double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
			for(int k=0; k<3; ++k)
			{
				double zeta = FloodplainList[i]->zeta[j][k];
				double Qx = FloodplainList[i]->Qx[j][k];
				double Qy = FloodplainList[i]->Qy[j][k];
				zarr[k]= FloodplainList[i]->NodalZ[j][k];
				double height = zeta + zarr[k];

				avgZeta += zeta;
				avgHeight += height;
				avgQx += Qx;
				avgQy += Qy;
				avgz += zarr[k];
			}
			fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
		}
	}
	fclose(file1);
	fclose(file2);

}

void output2DNodalError(double time)
{
	char *fileName1 = "NodalErrorsH.out";
	char *fileName2 = "NodalErrorsQ.out";
	FILE* ef1 = fopen(fileName1, "w");
	FILE* ef2 = fopen(fileName2, "w");

	int i = 0;
	for (int j=0; j < FloodplainList[i]->NumVerts; ++j)
	{
		double xval = FloodplainList[i]->Vx[j];
		double yval = FloodplainList[i]->Vy[j];
		int num_conn_els = FloodplainList[i]->ElCount[i];
		double nodalZeta = 0;
		double nodalQx = 0;
		double nodalQy = 0;
		for (int k = 0; k < num_conn_els; k++)
		{
			int curr_el = FloodplainList[i]->VtoEl[j][k];
			int loc_v;
			// find the local vertex number for curr_el
			for (int v = 0; v < 3; v++)
			{
				if (j == FloodplainList[i]->EltoVert[curr_el*3+v])
				{
					loc_v = v;
					break;
				}
			}

			int loc_node = FloodplainList[i]->VtoNode[curr_el][loc_v];
			nodalZeta += FloodplainList[i]->zeta[curr_el][loc_node];
			nodalQx += FloodplainList[i]->Qx[curr_el][loc_node];
			nodalQy += FloodplainList[i]->Qy[curr_el][loc_node];

		}
		nodalZeta /= num_conn_els;
		nodalQx /= num_conn_els;
		nodalQy /= num_conn_els;

		double manzeta = getmanH(xval, yval, time);
		double manQx = getQx(xval, yval, time);
		double manQy = getQy(xval, yval, time);

		double er1 = -nodalZeta + manzeta;
		double er2 = -nodalQx + manQx;
		double er3 = -nodalQy + manQy;

		fprintf(ef1, "%d \t %.13f\n", j, er1);
		fprintf(ef2, "%d \t %.13f \t %.13f\n", j, er2, er3);
	}

	fclose(ef1);
	fclose(ef2);

}

void outputFloodplainNodalDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

	// Output Floodplain data
	FILE *file1 = fopen(FpHeightFileName, openFileFormat);
	FILE *file2 = fopen(FpQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Floodplains = %d\n", NumFloodplains);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "Number of Floodplains = %d\n", NumFloodplains);
		for (int j = 0; j < NumFloodplains; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumVerts);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumVerts);
		}
	}
	for (int i=0; i < NumFloodplains; i++)
	{
		fprintf(file1, "# Nodal H at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Nodal Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j < FloodplainList[i]->NumVerts; ++j)
		{
			int num_conn_els = FloodplainList[i]->ElCount[i];
			double nodalZeta = 0;
			double nodalQx = 0;
			double nodalQy = 0;
			for (int k = 0; k < num_conn_els; k++)
			{
				int curr_el = FloodplainList[i]->VtoEl[j][k];
				int loc_v;
				// find the local vertex number for curr_el
				for (int v = 0; v < 3; v++)
				{
					if (j == FloodplainList[i]->EltoVert[curr_el*3+v])
					{
						loc_v = v;
						break;
					}
				}

				int loc_node = FloodplainList[i]->VtoNode[curr_el][loc_v];
				nodalZeta += FloodplainList[i]->zeta[curr_el][loc_node];
				nodalQx += FloodplainList[i]->Qx[curr_el][loc_node];
				nodalQy += FloodplainList[i]->Qy[curr_el][loc_node];

			}
			nodalZeta /= num_conn_els;
			nodalQx /= num_conn_els;
			nodalQy /= num_conn_els;

			fprintf(file1, "%d \t %.13f\n", j, nodalZeta);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, nodalQx, nodalQy );
		}
	}
	fclose(file1);
	fclose(file2);

}
void outputDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(ChannelHeightFileName, "%s/Channels.63", folderName);
		sprintf(ChannelQFileName, "%s/Channels.64", folderName);
		sprintf(KinFileName, "%s/KinField.63", folderName);
		sprintf(JuncHeightFileName, "%s/Junctions.63", folderName);
		sprintf(JuncQFileName, "%s/Junctions.64", folderName);
		//sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		//sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		// Move junction grid data file to this newly created folder
		/*char NewJunctionGridFileName[PATH_MAX + 1];
		sprintf(NewJunctionGridFileName, "%s/JunctionMesh.14", folderName);
		int ret = rename("./Output/JunctionMesh.14", NewJunctionGridFileName);
		if (ret != 0)
			printf("Error: unable to move the junction grid file\n");
*/
		// Move flowpaths file to this newly created folder
		int ret;
		char NewFlowPathsFileName[PATH_MAX + 1];
		sprintf(NewFlowPathsFileName, "%s/FlowPaths.out", folderName);
		ret = rename("./Output/FlowPaths.out", NewFlowPathsFileName);
		if (ret != 0)
			printf("Error: unable to move the flow paths file\n");

		// Move ChannelNodes.14 to this newly created folder
/*		char NewChannelsGridFileName[PATH_MAX + 1];
		sprintf(NewChannelsGridFileName, "%s/Channels.14", folderName);
		ret = rename("./Output/Channels.14", NewChannelsGridFileName);
		if (ret != 0)
			printf("Error: unable to move channels grid file\n");
*/
		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

		//char cwd[PATH_MAX+1];
	//if (getcwd(cwd, sizeof(cwd))==NULL)
	//	perror("Error while getting current working directory");

	//Output Channels data
	FILE* file1;
	FILE* file2;
	file1 = fopen(ChannelHeightFileName, openFileFormat);
	file2 = fopen(ChannelQFileName, openFileFormat);
	//if (!file1 || !file2)
	//{
	//	printf("Could not open file for writing. Exiting now\n");
	//	exit(1);
	//}
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals+1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;
		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Channels = %d\n", NumChannels);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "NumChannels = %d\n", NumChannels);
		for (int c = 0; c < NumChannels; c++)
		{
			fprintf(file1, "Number of data points for channel %d = %d\n", c, ChannelList[c]->NumNodes);
			fprintf(file2, "Number of data points for channel %d = %d\n", c, ChannelList[c]->NumNodes);
		}
	}
	for (int i=0; i<NumChannels; i++)
	{
		fprintf(file1, "# H and Zeta at time %.3f for Channel %d\n", currtime, i);
		fprintf(file2, "#Q at time %.3f for Channel %d\n", currtime, i);

		int NumNodes = ChannelList[i]->NumNodes;

		for (int j = 0; j<NumNodes; j++)
		{
			double bval = ChannelList[i]->NodalB[j];
			double zval = ChannelList[i]->NodalZ[j];
			double x_val = ChannelList[i]->NodalX[j];
			double y_val = ChannelList[i]->NodalY[j];
			double A = ChannelList[i]->A[j+1];
			double Q = ChannelList[i]->Q[j+1];
			double m1val = ChannelList[i]->Nodalm1[j];
			double m2val = ChannelList[i]->Nodalm2[j];
			double H = getH(A, bval, m1val, m2val);
			double zeta = H - zval;
			fprintf(file1, "%.13f \t %.13f\n", H, zeta);
			fprintf(file2, "%.13f\n", Q);

		}

	}

	fclose(file1);
	fclose(file2);

	// Output Junction data
	file1 = fopen(JuncHeightFileName, openFileFormat);
	file2 = fopen(JuncQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Junctions = %d\n", NumJunctions);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "NumJunctions = %d\n", NumJunctions);
		for (int j = 0; j < NumJunctions; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, JunctionList[j]->NumEl);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, JunctionList[j]->NumEl);
		}
	}
	for (int i=0; i<NumJunctions; i++)
	{

		fprintf(file1, "# Average H and Zeta at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Average Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j<JunctionList[i]->NumEl; ++j)
		{
			double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
			for(int k=0; k<3; ++k)
			{
				double zeta = JunctionList[i]->zeta[j][k];
				double Qx = JunctionList[i]->Qx[j][k];
				double Qy = JunctionList[i]->Qy[j][k];
				zarr[k]= JunctionList[i]->NodalZ[j][k];
				double height = zeta + zarr[k];

				avgZeta += zeta;
				avgHeight += height;
				avgQx += Qx;
				avgQy += Qy;
				avgz += zarr[k];

			}
			fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
		}
	}
	fclose(file1);
	fclose(file2);

	// Output Kinematic Field Data
	// Here we assume that we are working with first order polynomials only
	if (FloodplainList[0]->floodedStatus == 0)
	{
		file1 = fopen(KinFileName, openFileFormat);
		if (writeHeader)
		{
			int numRecords;
			if (fmod(finalTime, recordTimeIntervals) == 0)
				numRecords = finalTime/recordTimeIntervals + 1;
			else
				numRecords = finalTime/recordTimeIntervals + 2;

			fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		}
		fprintf(file1,"# Average height at time %3.3f in Kinematic Field\n", currtime);

		int NumEl = FloodplainList[0]->NumEl;
		for (int i = 0; i < NumEl; i++)
		{
			if (KinematicElList[i]->isActive == 1)
			{
				double weq = KinematicElList[i]->weq;
				double height = KinematicElList[i]->A[0]/weq;
				if (KinematicElList[i]->numUpstreamEls > 0)
				{
					int numUpstreamEls = KinematicElList[i]->numUpstreamEls;
					height = KinematicElList[i]->A[0]/weq;
					for (int j = 0; j < numUpstreamEls; j++)
					{
						int el = KinematicElList[i]->upstreamEls[j];
						height += KinematicElList[el]->A[1]/weq;
					}
					height = height/(numUpstreamEls+1);
				}
				//printf("%d %.13f\n", i, height);
				fprintf(file1, "%d %.13f\n", i, height);
			}
			else
				fprintf(file1, "%d %1.13f\n", i, 1e-7);
		}
		fclose(file1);
	}

	//static int previouslyFlooded = 0;
	//static char fpOpenFileFormat[1];
	//int writefpHeader = 0;

	//// Output Floodplain data if the channels have flooded
	//if (FloodplainList[0]->floodedStatus == 1)
	//{
	//	if (previouslyFlooded)
	//	{
	//		fpOpenFileFormat[0] = 'a';
	//	}
	//	else
	//	{
	//		fpOpenFileFormat[0] = 'w';
	//		writefpHeader = 1;
	//	}

	//	file1 = fopen(FpHeightFileName, fpOpenFileFormat);
	//	file2 = fopen(FpQFileName, fpOpenFileFormat);

	//
	//	if (!file1)
	//		printf("Error while opening file1\n");
	//	if (!file2)
	//		printf("Error while opening file2\n");
	//	if (writefpHeader)
	//	{
	//		int numRecords;
	//		if (fmod(finalTime, recordTimeIntervals) == 0)
	//			numRecords = finalTime/recordTimeIntervals + 1;
	//		else
	//			numRecords = finalTime/recordTimeIntervals + 2;

	//		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
	//		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
	//		fprintf(file1, "Number of data points for floodplain = %d\n", FloodplainList[0]->NumEl);
	//		fprintf(file2, "Number of data points for floodplain = %d\n", FloodplainList[0]->NumEl);
	//	}
	//
	//	fprintf(file1, "# Average H and Zeta at time %.3f\n", currtime);
	//	fprintf(file2, "# Average Qx and Qy at time %.3f\n", currtime);
	//
	//	for (int j=0; j<FloodplainList[0]->NumEl; ++j)
	//	{
	//		double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
	//		for(int k=0; k<3; ++k)
	//		{
	//			double zeta = FloodplainList[0]->zeta[j][k];
	//			double Qx = FloodplainList[0]->Qx[j][k];
	//			double Qy = FloodplainList[0]->Qy[j][k];
	//			zarr[k]= FloodplainList[0]->NodalZ[j][k];
	//			double height = zeta + zarr[k];
	//
	//			avgZeta += zeta;
	//			avgHeight += height;
	//			avgQx += Qx;
	//			avgQy += Qy;
	//			avgz += zarr[k];

	//		}
	//		fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
	//		fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
	//	}
	//	fclose(file1);
	//	fclose(file2);

	//	previouslyFlooded = 1;

	//}

}

void calculate_l2_error_2d(double time)
{
	int NumEl = FloodplainList[0]->NumEl;
	int Np = FloodplainList[0]->Np;
	double errsqH = 0, errsqQx = 0, errsqQy = 0;

	gsl_vector *diffH = gsl_vector_alloc(Np);
	gsl_vector *diffQx = gsl_vector_alloc(Np);
	gsl_vector *diffQy = gsl_vector_alloc(Np);

	//double max = 0;
	for (int i = 0; i < NumEl; i++)
	{
		for (int j = 0; j < Np; j++)
		{
			double xval = FloodplainList[0]->NodalX[i][j];
			double yval = FloodplainList[0]->NodalY[i][j];
			double exactH = getmanH(xval, yval, time);
			double exactQx = getQx(xval, yval, time);
			double exactQy = getQy(xval, yval, time);
			double compH = FloodplainList[0]->zeta[i][j];
			double compQx =  FloodplainList[0]->Qx[i][j];
			double compQy = FloodplainList[0]->Qy[i][j];
			gsl_vector_set(diffH, j, fabs(exactH-compH));
			gsl_vector_set(diffQx, j, fabs(exactQx-compQx));
			gsl_vector_set(diffQy, j, fabs(exactQy-compQy));
			//gsl_vector_set(diffH, j, RHSZeta[i*Np+j]);
			//gsl_vector_set(diffQx, j,  RHSQx[i*Np+j]);
			//gsl_vector_set(diffQy,j, RHSQy[i*Np+j]);
			//max = fmax(fabs(exactQx - compQx), max);

		}

		gsl_vector *tmpH = gsl_vector_alloc(Np);
		gsl_vector *tmpQx = gsl_vector_alloc(Np);
		gsl_vector *tmpQy = gsl_vector_alloc(Np);
		double jac = FloodplainList[0]->jac[i];
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffH, 0.0, tmpH);
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffQx, 0.0, tmpQx);
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffQy, 0.0, tmpQy);

		double elerrHsq, elerrQxsq, elerrQysq;
		gsl_blas_ddot(diffH, tmpH, &elerrHsq);
		gsl_blas_ddot(diffQx, tmpQx, &elerrQxsq);
		gsl_blas_ddot(diffQy, tmpQy, &elerrQysq);

		errsqH += elerrHsq;
		errsqQx += elerrQxsq;
		errsqQy += elerrQysq;

		gsl_vector_free(tmpH);
		gsl_vector_free(tmpQx);
		gsl_vector_free(tmpQy);
	}

	gsl_vector_free(diffH);
	gsl_vector_free(diffQx);
	gsl_vector_free(diffQy);

	printf("time at error computation = %lf\n", time);
	printf("l2 error in H = %lf \n", sqrt(errsqH));
	printf("l2 error in Qx = %lf \n", sqrt(errsqQx));
	printf("l2 error in Qy = %lf \n", sqrt(errsqQy));
	//printf("maximum difference in Qx = %lf\n", max);

}

void append_to_file(char *fileName, double time, double data)
{
	FILE* file = fopen(fileName, "a");
	fprintf(file, "%lf %lf\n", time,data);
	fclose(file);
}

double calculateTotalWater(struct channel *Chan)
{
	int NumEl = Chan->NumEl;
	int Np = Chan->Np;
	int P = Chan->P;

	double totalWater = 0;
	double LGLWeight[Np];
	GetLGLWeights(P, LGLWeight);
	for (int k = 0; k < NumEl; k++)
	{
		double water = 0;
		double dh = Chan->dh[k];
		for (int i = 0; i < Np; i++)
		{
			double m1val = Chan->Nodalm1[k*Np+i];
			double m2val = Chan->Nodalm2[k*Np+i];
			double bval = Chan->NodalB[k*Np+i];
			double A = Chan->A[k*Np+i+1];
			double h = getH(A, bval, m1val, m2val);
			water += LGLWeight[i]*(h-0.0000001);
		}
		totalWater += 0.5*dh*water;
	}

	return totalWater;

}

double ftTom(double ft)
{
	double meter = ft*0.3048;
	return meter;
}

double mToft (double m)
{
	double ft = m*3.28084;
	return ft;

}


#include <stdio.h>
#include <unistd.h>
#include <assert.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include "dlib.h"
#include "svec.h"
#include "rng.h"

const char *usage = "Usage: scode [OPTIONS] < file\n"
  "file should have columns of arbitrary tokens\n"
  "-r RESTART: number of restarts (default 1)\n"
  "-i NITER: number of iterations over data (default UINT32_MAX)\n"
  "-t THRESHOLD: quit if logL increase for iter <= this (default .001)\n"
  "-d NDIM: number of dimensions (default 25)\n"
  "-z Z: partition function approximation (default 0.166)\n"
  "-p PHI0: learning rate parameter (default 50.0)\n"
  "-u ETA0: learning rate parameter (default 0.2)\n"
  "-s SEED: random seed (default 0)\n"
  "-c calculate real Z (default false)\n"
  "-w The first line of the input is weights (default false)\n"
  "-v verbose messages (default false)\n";

//typedef uint32_t u32;
//typedef uint64_t u64;
u32 RESTART = 1;
u32 NITER = UINT32_MAX;
double THRESHOLD = 0.001;
u32 NDIM = 25;
double Z = 0.166;
double PHI0 = 50.0;
double ETA0 = 0.2;
unsigned long int SEED = 0;
bool CALCZ = false;
bool WEIGHT = false;
bool VERBOSE = false;

u32 NTOK = 0;
u64 NTUPLE = 0;

const gsl_rng_type *rng_T;
gsl_rng *rng_R = NULL;
darr_t data;
u64 **update_cnt;
double * weight = NULL;
double * uweight = NULL; /*Updated weights*/
u64 **cnt;
#define frq(i,j) ((double)cnt[i][j]*NTOK/len(data))
svec **vec;
svec **best_vec;
svec dummy_vec;
sym_t qmax;
sym_t NULLFEATID;
#define NULLFEATMARKER "/XX/"

int main(int argc, char **argv);
void init_rng();
void free_rng();
u64 init_data();
u32 init_weight();
void free_weight();
void randomize_vectors();
void copy_best_vec();
void free_data();
void update_tuple(sym_t *t);
double logL();
double calcZ();

#define vmsg(...) if(VERBOSE)msg(__VA_ARGS__)

int main(int argc, char **argv) {
  int opt;
  while((opt = getopt(argc, argv, "r:i:t:d:z:p:u:s:cwv")) != -1) {
    switch(opt) {
    case 'r': RESTART = atoi(optarg); break;
    case 'i': NITER = atoi(optarg); break;
    case 't': THRESHOLD = atof(optarg); break;
    case 'd': NDIM = atoi(optarg); break;
    case 'z': Z = atof(optarg); break;
    case 'p': PHI0 = atof(optarg); break;
    case 'u': ETA0 = atof(optarg); break;
    case 's': SEED = atoi(optarg); break;
    case 'c': CALCZ = true; break;
    case 'w': WEIGHT = true; break;
    case 'v': VERBOSE = true; break;
    default: die("%s",usage);
    }
  }

  vmsg("scode -r %u -i %u -t %g -d %u -z %g -p %g -u %g -s %lu %s%s%s",
       RESTART, NITER, THRESHOLD, NDIM, Z, PHI0, ETA0, SEED,
       (CALCZ ? "-c " : ""), (WEIGHT ? "-w " : ""), (VERBOSE ? "-v " : ""));

  init_rng();
  if (SEED) gsl_rng_set(rng_R, SEED);
  if (WEIGHT) NTOK = init_weight();
  NTUPLE = init_data();
  vmsg("Read %zu tuples %u uniq tokens", NTUPLE, qmax);

  double best_logL = 0;
  for (u32 start = 0; start < RESTART; start++) {
    randomize_vectors();
    double ll = logL();
    vmsg("Restart %u/%u logL0=%g best=%g", 1+start, RESTART, ll, best_logL);
    if (CALCZ) vmsg("Z=%g (approx %g)", calcZ(), Z);
    for (u32 iter = 0; iter < NITER; iter++) {
      for (u64 di = 0; di < NTUPLE; di++) {
	update_tuple(&val(data, di * NTOK, sym_t));
      }
      double ll0 = ll;
      ll = logL();
      vmsg("Iteration %u/%u logL=%g", 1+iter, NITER, ll);
      if (ll - ll0 <= THRESHOLD) break;
    }
    if (start == 0 || ll > best_logL) {
      vmsg("Updating best_vec with logL=%g", ll);
      best_logL = ll;
      copy_best_vec();
    }
    vmsg("Restart %u/%u logL1=%g best=%g", 1+start, RESTART, ll, best_logL);
    if (CALCZ) vmsg("Z=%g (approx %g)", calcZ(), Z);
  }
  for (u32 t = 0; t < NTOK; t++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (best_vec[t][q] == NULL) continue;
      printf("%u:%s\t%zu\t", t, sym2str(q), cnt[t][q]);
      svec_print(best_vec[t][q]);
      putchar('\n');
    }
  }
  fflush(stdout);
  free_data();
  free_rng();
  if (WEIGHT) free_weight();
  symtable_free();
  dfreeall();
  fprintf(stderr, "%f\n", best_logL);
  vmsg("bye");
}

double logL() {
  double l = 0;
  for (u64 i = 0; i < NTUPLE; i++) {
    sym_t *t = &val(data, i * NTOK, sym_t);
    sym_t x = t[0];
    sym_t y = t[1];
    float px = frq(0, x);
    float py = frq(1, y);
    svec vx = vec[0][x];
    svec vy = vec[1][y];
    float xy = svec_sqdist(vx, vy);
    l += log(px * py) - xy;
  }
  return (l / NTUPLE - log(Z));
}

double calcZ() {
  double z = 0;
  for (sym_t x = 1; x <= qmax; x++) {
    if (VERBOSE && (x % 1000 == 0)) fputc('.', stderr);
    if (cnt[0][x] == 0) continue;
    float px = frq(0, x);
    svec vx = vec[0][x];
    for (sym_t y = 1; y <= qmax; y++) {
      if (cnt[1][y] == 0) continue;
      float py = frq(1, y);
      svec vy = vec[1][y];
      float xy = svec_sqdist(vx, vy);
      z += px * py * exp(-xy);
    }
  }
  if (VERBOSE) fputc('\n', stderr);
  return z;
}

void update_tuple(sym_t *t) {
  /*weighted update*/
  static svec *u = NULL;
  static svec *v = NULL;
  static svec dx = NULL;
  if (u == NULL) u = _d_malloc(NTOK * sizeof(svec));
  if (v == NULL) v = _d_malloc(NTOK * sizeof(svec));
  if (dx == NULL) dx = svec_alloc(NDIM);
  for (u32 i = 0; i < NTOK; i++) u[i] = vec[i][t[i]];
  for (u32 i = 0; i < NTOK; i++) {
    /* Sampling values from the marginal distributions. */
    /* Can this be done once, or do we have to resample for every x? */
    if(i > 0 && t[i] == NULLFEATID) continue;
    for (u32 j = 0; j < NTOK; j++) {
      if (j==i) { v[j] = u[i]; continue;}
      u64 r = gsl_rng_get(rng_R);
      r = (r << 32) | gsl_rng_get(rng_R);
      r = r % NTUPLE;
      sym_t y = val(data, r * NTOK + j, sym_t);
      v[j] = vec[j][y];
      if(i > 0) break;
    }
    /* Compute the move for u[i] */
    svec_set_zero(dx);
    double ww;
    for (u32 j = 0; j < NTOK; j++) {
      if (j == i) continue;
      ww = weight == NULL ? 1 : (i > 0 ? weight[i] : weight[j]);
      double push = 0, pull = 0;
      if (v[j] == NULL) v[j] = dummy_vec;
      else push = exp(-svec_sqdist(u[i], v[j])) / Z;
      if(u[j] == NULL)  u[j] = dummy_vec;
      else pull = 1;
      if(push != 0 || pull != 0){
	for (u32 d = 0; d < NDIM; d++) {
	  float dxd = svec_get(dx, d);
	  float x = svec_get(u[i], d);
	  float y = svec_get(u[j], d);
	  float z = svec_get(v[j], d);
	  svec_set(dx, d, dxd + ww * ( pull * (y - x) + push * (x - z)));
	}
      }
      /*restore the vectors to original forms*/
      if(push == 0) v[j] = NULL;
      if(pull == 0) u[j] = NULL;
      if(i > 0) break;
      }
    /* Apply the move scaled by learning parameter */
    u64 cx = update_cnt[i][t[i]]++;
    float nx = ETA0 * (PHI0 / (PHI0 + cx));
    svec_scale(dx, nx);
    svec_add(u[i], dx);
    svec_normalize(u[i]);
  }
}

u32 init_weight(){
  u32 size = 100, i = 0;
  weight = _d_malloc(size * sizeof(double));
  forline (buf, NULL) {
    fortok (tok, buf) {
      weight[i] = atof(tok);
      assert(weight[i++] >= 0);
      if(i >= 100) {
	size *= 2;
	weight = _d_realloc(weight, size);
      }
    }
    assert(i > 0);
    break;
  }
  return i;
}

void free_weight() {
  if (weight != NULL) _d_free(weight);
}

u64 init_data() {
  qmax = 0;
  data = darr(0, sym_t);
  forline (buf, NULL) {
    u32 ntok = 0;
    fortok (tok, buf) {
      sym_t q = str2sym(tok, true);
      if (q > qmax) qmax = q;
      size_t lendata = len(data);
      val(data, lendata, sym_t) = q;
      if(strcmp(tok, NULLFEATMARKER) == 0) NULLFEATID = q;
      ntok++;
    }
    if(NTOK == 0) NTOK = ntok;
    assert(ntok == NTOK); //Each line has equal number of tokens
  }
  assert(NTOK > 0);
  update_cnt = _d_malloc(NTOK * sizeof(ptr_t));
  cnt = _d_malloc(NTOK * sizeof(ptr_t));
  vec = _d_malloc(NTOK * sizeof(ptr_t));
  best_vec = _d_malloc(NTOK * sizeof(ptr_t));
  dummy_vec = svec_alloc(NDIM);
  svec_zero(dummy_vec);
  uweight = _d_calloc(NTOK, sizeof(double));
  for (u32 i = 0; i < NTOK; i++) {
    update_cnt[i] = _d_calloc(qmax+1, sizeof(u64));
    cnt[i] = _d_calloc(qmax+1, sizeof(u64));
    vec[i] = _d_calloc(qmax+1, sizeof(svec));
    best_vec[i] = _d_calloc(qmax+1, sizeof(svec));
  }
  u64 N = len(data) / NTOK;
  for (u64 i = 0; i < N; i++) {
    sym_t *p = &val(data, i * NTOK, sym_t);
    for (u32 j = 0; j < NTOK; j++) {
      sym_t k = p[j];
      assert(k <= qmax);
      cnt[j][k]++;
      if(k == NULLFEATID){
	vec[j][k] = best_vec[j][k] = NULL;
      }
      else if (vec[j][k] == NULL) {
	vec[j][k] = svec_alloc(NDIM);
	best_vec[j][k] = svec_alloc(NDIM);
      }
    }
  }
  return N;
}

void free_data() {
  for (u32 i = 0; i < NTOK; i++) {
    for (sym_t j = 0; j <= qmax; j++) {
      if (vec[i][j] != NULL) {
	svec_free(vec[i][j]);
	svec_free(best_vec[i][j]);
      }
    }
    _d_free(best_vec[i]);
    _d_free(vec[i]);
    _d_free(cnt[i]);
    _d_free(update_cnt[i]);
  }
  _d_free(uweight);
  svec_free(dummy_vec);
  _d_free(best_vec);
  _d_free(vec);
  _d_free(cnt);
  _d_free(update_cnt);
  darr_free(data);
}

void randomize_vectors() {
  for (u32 j = 0; j < NTOK; j++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (vec[j][q] != NULL) {
	svec_randomize(vec[j][q]);
	update_cnt[j][q] = 0;
      }
    }
  }
}

void copy_best_vec() {
  for (u32 j = 0; j < NTOK; j++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (vec[j][q] != NULL) {
	svec_memcpy(best_vec[j][q], vec[j][q]);
      }
    }
  }
}

void init_rng() {
  gsl_rng_env_setup();
  rng_T = gsl_rng_mt19937;
  rng_R = gsl_rng_alloc(rng_T);
}

void free_rng() {
  gsl_rng_free(rng_R);
}


#ifndef GSL_WRAPPERS_H
#define GSL_WRAPPERS_H

// #include <gsl/gsl_check_range.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h>

#define outlog(format, args...) \
    fprintf(stderr, format, args); \
    fprintf(stderr, "\n");

double safe_log(double);
double log_sum(double, double);

static inline double vget(const gsl_vector* v, int i)
{ return(gsl_vector_get(v, i)); };

static inline void vset(gsl_vector* v, int i, double x)
{ gsl_vector_set(v, i, x); };

// Increment a vector element by a double.
void vinc(gsl_vector*, int, double);

static inline double mget(const gsl_matrix* m, int i, int j)
{ return(gsl_matrix_get(m, i, j)); };

static inline void mset(gsl_matrix* m, int i, int j, double x)
{ gsl_matrix_set(m, i, j, x); };

void msetcol(gsl_matrix* m, int r, const gsl_vector* val);

// Increment a matrix element by a double.
void minc(gsl_matrix*, int, int, double);
void msetrow(gsl_matrix*, int, const gsl_vector*);

void col_sum(gsl_matrix*, gsl_vector*);

void vct_printf(const gsl_vector* v);
void mtx_printf(const gsl_matrix* m);
void vct_fscanf(const char*, gsl_vector* v);
void mtx_fscanf(const char*, gsl_matrix* m);
void vct_fprintf(const char* filename, gsl_vector* v);
void mtx_fprintf(const char* filename, const gsl_matrix* m);

double log_det(gsl_matrix*);

void matrix_inverse(gsl_matrix*, gsl_matrix*);

void sym_eigen(gsl_matrix*, gsl_vector*, gsl_matrix*);

double sum(const gsl_vector* v);

double norm(gsl_vector * v);

void vct_log(gsl_vector* v);
void vct_exp(gsl_vector* x);

void choose_k_from_n(int k, int n, int* result);

void log_normalize(gsl_vector* x);
void normalize(gsl_vector* x);

void optimize(int dim,
              gsl_vector* x,
              void* params,
              void (*fdf)(const gsl_vector*, void*, double*, gsl_vector*),
              void (*df)(const gsl_vector*, void*, gsl_vector*),
              double (*f)(const gsl_vector*, void*));

void optimize_fdf(int dim,
                  gsl_vector* x,
                  void* params,
                  void (*fdf)(const gsl_vector*, void*, double*, gsl_vector*),
                  void (*df)(const gsl_vector*, void*, gsl_vector*),
                  double (*f)(const gsl_vector*, void*),
                  double* f_val,
                  double* conv_val,
                  int* niter);

void log_write(FILE* f, char* string);
int directory_exist(const char *dname);
void make_directory(char* name);

gsl_rng* new_random_number_generator();

#endif


#include <assert.h>
#include <cblas.h>
#include <complex.h>
#include <errno.h>
#include <fftw3.h>
#include <getopt.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <math.h>
#include <omp.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#define C 299792458
#define PI 3.141592654
#define ANSI_COLOR_RED     "\x1b[31m"
#define ANSI_COLOR_GREEN   "\x1b[32m"
#define ANSI_COLOR_YELLOW  "\x1b[33m"
#define ANSI_COLOR_BLUE    "\x1b[34m"
#define ANSI_COLOR_MAGENTA "\x1b[35m"
#define ANSI_COLOR_CYAN    "\x1b[36m"
#define ANSI_COLOR_RESET   "\x1b[0m"
#define _MODEL_DEBUG 0
#define MAX_TX 1
#define MAX_RX 1
#define MAX_RIBBON_SIZE 20
#define MAX_SURFACES 20
// if max values exceeded, may cause memory bugs

struct simulation {
        struct environment *env;
        struct file_reader *fr;
};

struct spatial_motion_model {
        double velocity[3];
        double position[3];
};

struct transmission_model {
        double power_in_dBm;
};

struct propagation_model {
        double distance;
};

struct general_node {
        struct spatial_motion_model *smm;
        struct transmission_model *tm;
        int id;
};

struct transmitter {
        struct general_node *gn;
        double complex baseband_signal;
};

struct receiver {
        struct general_node *gn;
        double recv_noise_power;
        double complex rx_signal;
        struct receiver_ray_ribbon_ll_node *rlln;
};

struct perfect_reflector {
        double unit_normal[3];
        double unit_length_normal[3];
        double unit_width_normal[3];
        double center_point[3];
        double length, width;
};

struct environment {
        struct receiver **receivers_array;
        struct transmitter **transmitters_array;
        struct perfect_reflector **prarray;
        struct general_node **node_array;
        struct ray_ribbon_array **env_paths;
        struct ray_ribbon_array **tx_paths;

        double recv_unit_normal[3];
        double time;
        double complex *unit_power_gaussian_noise;
        double frequency;
        double wavelength;
        double delta_time;
        double end_time;
        double max_limit;
        double min_limit;
	int refresh_time;
	int awgn_supplied;
        int time_index;

        int num_transmitters;
        int num_receivers;
        int num_reflectors;

        int sz_array_tx;
        int sz_array_rx;
        int sz_array_gn;
        int sz_array_pr;

        // flags
        bool read_in_nodes;
        /* should be true by the time TX or RX is read in */
        bool node_memory_allocated;
        bool tx_paths_updated;
        bool tx_paths_updated_rx_paths_updated;
};

struct file_reader {
        FILE *infile;
        FILE *outfile;

        // filenames longer than 999 will be truncated
        char input_filename[1000];
        char output_filename[1000];
};

int id();
struct simulation *init_simulation();
struct spatial_motion_model *init_spatial_motion_model();
struct transmission_model *init_transmission_model();
struct propagation_model *init_propagation_model();
struct general_node *init_general_node();
struct transmitter *init_transmitter();
struct receiver *init_receiver();
struct perfect_reflector *init_perfect_reflector(const double *normal,
                                         const double *center_point,
                                         const double *length_normal,
                                         double length, double width);
struct perfect_reflector **init_perfect_reflectorarray(int number);
struct environment *init_environment();
int malloc_environment(struct environment *env);

void print_vector(const double *db);
void print_spatial_motion_model(const struct spatial_motion_model *smm);
void print_transmission_model(const struct transmission_model *tm);
void print_propagation_model(const struct propagation_model *pm);
void print_general_node(const struct general_node *gn);
void print_transmitter(const struct transmitter *tx);
void print_receiver(const struct receiver *rx);
void print_perfect_reflectors(const struct perfect_reflector *pr);
void print_environment(const struct environment *env);
void print_env_paths(const struct environment *env);
void print_tx_paths(const struct environment *env);

void destroy_spatial_motion_model(struct spatial_motion_model *smm);
void destroy_simulation(struct simulation *sim);
void destroy_transmission_model(struct transmission_model *tm);
void destroy_propagation_model(struct propagation_model *pm);
void destroy_transmitter(struct transmitter *tx);
void destroy_receiver(struct receiver *tx);
void destroy_environment(struct environment *env);
void destroy_file_reader(struct file_reader *fr);
void destroy_perfect_reflector(struct perfect_reflector *pr);
void destroy_perfect_reflectorarray(struct perfect_reflector **pr_begin);
struct file_reader *init_file_reader(int argc, char *argv[]);
void cross_product(const double *v1, const double *v2, double *v3);
double normalize_unit_vector(double *v1);
void diff(const double *v1, const double *v2, double *v3);
int find_len(void **ptr);

void add_receiver_patch(struct environment *env, int length);
void destroy_last_reflector(struct environment *env);
double distance(const struct general_node *gn1,
                const struct general_node *gn2);

bool update_environment_from_file_sim(struct simulation *sim);
bool handle_request(struct environment *env, FILE *fp, const char *req_type);
bool custom_fscanf(FILE *fp, const char *str, void *ptr);

// Deprecated
bool update_environment_from_file(struct environment *env,
                                FILE *fp);


# ifndef CELL_CYCLE_H
# define CELL_CYCLE_H

#include <vector>
#include <deque>
#include <string>
#include <memory>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <math.h>

using namespace std;

#define CONSTANT_PHASE 0
#define EXPONENTIAL_PHASE 1

#endif

//////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
class cell_cycle{//Class containing behaviour related to cell_cycle.
////////////
//////////

		private:

		int cell_state; // current state of the cycle
		double time_state; // current time in the state of the cycle
		double time_cycle; // total time in this cell_cycle
		double currentPhaseDuration; // time for the next phase
		vector<double> state_durations; // array with the cell states durations
		vector<int> state_types; // array with the cell phases types (exponential, uniform, ... )
		int divisions; // number of divisions so far
		int num_states; // total number of cell phases included
		bool divide_material; // flag to see if the species in the cell are dividied after cell division
		gsl_rng * rng; // allocator for the rng generator

		public:

		cell_cycle(){ // default constructor is a constructor with empty info
			time_state = 0;
			time_cycle = 0;
			divisions = 0;
			cell_state = 0;
			num_states = 0;
			rng= gsl_rng_alloc (gsl_rng_mt19937);
			gsl_rng_set (rng,::time(NULL)*getpid());
		}

		void addCellPhase(double duration, int type_phase){
			cout<<"Adding Cell Phase of duration"<< duration<<" \n";
			state_durations.push_back(duration);
			state_types.push_back(type_phase);
			num_states++;
			if (num_states == 1){// if this is the first state set, also use it for the current cell cycle time
				getPhaseDuration();
			}
			cout<<"Cell Phase added\n";
		}

		void ResetCellCycle(){
			time_state = 0;
			time_cycle = 0;
			cell_state = 0;
			getPhaseDuration();
			divisions = 0;
			divide_material = false;
		}

		// cell_cycle(int nstates,float duration, int type){
		// 	// The default constructor generates n cycle phases with same duration of certain type
		// 	time_state = 0;
		// 	time_cycle = 0;
		// 	divisions = 0;
		// 	cell_state = 0;
		// 	num_states = nstates;
		// 	state_durations = new double[num_states];
		// 	state_types = new int[num_states];
		// 	for(int i=0;i<nstates;i++){
		// 		state_durations[i] = duration;
		// 		state_types[i] = type;
		// 	}
		// }

		void setPhaseDuration(int phase, double duration, int type_phase){
			state_durations[phase] = duration;
			state_types[phase] = type_phase;
		}

		double getPhaseDuration(){
		// compute the value of the time for the next phase and store it in NextPhaseTime to avoid recomputing
			if (state_types[cell_state] == CONSTANT_PHASE){
				currentPhaseDuration = state_durations[cell_state];
			}
			else if (state_types[cell_state] == EXPONENTIAL_PHASE){
				currentPhaseDuration = state_durations[cell_state] * log(1.0/gsl_rng_uniform_pos(rng));
			}

			// cout<<"Next Phase duration: "<<currentPhaseDuration<<'\n';
			return currentPhaseDuration;
		}

		double getTimeForNextCellPhase(){
			return currentPhaseDuration - time_state;
		}

		int AdvanceNextPhase(){
			cell_state++;
			if (cell_state==num_states){// if the cell reaches the final state
				cell_state = 0;
				time_cycle = 0;
			}
			time_state = 0;
			getPhaseDuration();
			if (cell_state == 0){ // if we are dividing
				divisions += 1;
				divide_material = true;
			}
			else{
				divide_material = false;
			}
			return cell_state;
		}

		bool MaterialIsDivided(){
			return divide_material;
		}

		void updateCelltime(double time){
			time_cycle += time;
			time_state += time;
		}

		void setCellCyclePositionTime(int phase, double time){
			cell_state = phase;
			time_state = time;
			time_cycle = time;
		}

		void printCellProperties(){
			cout<<"total states "<<num_states<<'\n';
			cout<<"of duration ";
			for (auto d: state_durations){ cout << d << ' ';};
			cout<<"\n of type";
			for (auto d: state_types){ cout << d << ' ';};
			cout<<'\n';
		}

		int getCellPhase(){
			return cell_state;
		}

		double getPhaseTime(){
			return time_state;
		}

		void PrintState(){
			cout<< "cell state: "<<cell_state<< " time_state: "<<time_state<<'\n';
			cout<< "currentPhaseDuration: "<<currentPhaseDuration<<'\n';
			printCellProperties();
			cout<< "time cell has divided: "<<divisions<<'\n';
			cout<< "cell is ready for division: "<< divide_material<<'\n';
		}

};