/*
 * A set of functions for fittign an impulse function
 * (C) Gal Chechik 2004
 */

#include <iomanip>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <time.h>
#include "impulse.h"
#include "io.h"
#include "main.h"
#include "nrutil.h"
#include "nr.h"
using namespace std;

/*
 *
 * Amodel - 2 sigmoids sharing the same beta   (h0,h1,h2,t1,t2,b)
 * Bmodel - a single sigmoid                   (h0,h1,t1,b)
 * Cmodel - two sigmoids, two different betas  (h0,h1,h2,t1,t2,b1,b2)
 * Dmodel - two impulses                       (h0,h1,h2,h3,h4,t1,t2,t3,t4,b);
 * Emodel - sum 2 sigs (same slope)            (h0,h1,h2,t1,t2,b)
 * Fmodel - sum 2 sigs (diff slope)            (h0,h1,h2,t1,t2,b1,b2)
 * Gmodel - same as Amodel with extra zeros in data
 * Hmodel - same as Cmodel with extra zeros in data
 * Imodel - same as Gmodel with Prior on slopes 
 * Jmodel - same as Bmodel with extra zeros in data
 *
 */


float *global_xs;
float *global_ys;
int    global_n_points;
long   global_seed=1;
float  global_fraction_noise=0.1;
float  global_additive_noise=0.05;
int    global_stability_version = 3;
int    N_MODEL_PARMS;      // No of parameters in the model chosen

// For hand tuned priors
float global_hand_prior_weight=0.05;
int   thetas_do_hand_priors;

// For empirical priors
int     thetas_do_emp_priors;
float ** thetas_means;
float ** thetas_covar;
float *  thetas_emp_prior;
#define N_CLUSTERS (10)
#define EMP_PRIOR_WEIGHT (1.0)
#define GAUSSIAN_MODE (1)
#define PI (3.141592654)


/*
  Hand tuned priors on beta shuld be a decaying exponential:
  p(b) = exp(-abs(b))   ;     - log(p(b)) = abs(b) 
  d (-logp(b)) / d(b) = sign(beta)   
*/

int set_n_model_parms(char model) { 
  switch(model){ 
  case 'A': N_MODEL_PARMS = N_AMODEL_PARMS;break;
  case 'B': N_MODEL_PARMS = N_BMODEL_PARMS;break;
  case 'C': N_MODEL_PARMS = N_CMODEL_PARMS;break;
  case 'D': N_MODEL_PARMS = N_DMODEL_PARMS;break;
  case 'G': N_MODEL_PARMS = N_AMODEL_PARMS;break;
  case 'H': N_MODEL_PARMS = N_CMODEL_PARMS;break;
  case 'I': N_MODEL_PARMS = N_AMODEL_PARMS;break;    
  case 'J': N_MODEL_PARMS = N_BMODEL_PARMS;break;    
  default: cerr << "impulse.cpp: Illegal model = '" << model << "'\n";  exit(1);
  }
  cout << "N_MODEL_PARMS = " << N_MODEL_PARMS << "\n";
  return N_MODEL_PARMS;
}
// -------------------------------------------------
string set_fitfilename(char *segnum, string gene_name, char model)
{
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/Fit/seg"
    + segnum + "/seg" + segnum + "_" + gene_name + "_fit.m";
  return outfilename;
}
// -------------------------------------------------
string set_stabfilename(char *segnum, string gene_name, char model, string stability_version){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/Stability"
    + stability_version + '0' + "/seg" + segnum
    + "/seg" + segnum + "_" + gene_name + "_stab.m";
  return outfilename;
}
// -------------------------------------------------
string set_pvalfilename(char *segnum, string gene_name, char model){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/Pvals/seg" + segnum + "/seg" + segnum
    + "_" + gene_name + "_pval.m";
  return outfilename;
}
// -------------------------------------------------
string set_cvfilename(char *segnum, string gene_name, char model){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/FillMiss/seg" + segnum + "/seg" + segnum
    + "_" + gene_name + "_cvfil.m";
  return outfilename;
}
// -------------------------------------------------
string set_theta_mean_filename(char *segnum, char model){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
   + model + "model/ThetaStats/seg" + segnum + "_means.txt";
  return outfilename;
}
// -------------------------------------------------
string set_theta_covar_filename(char *segnum, char model){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/ThetaStats/seg" + segnum + "_covars.txt";
  return outfilename;
}
// -------------------------------------------------
string set_theta_prior_filename(char *segnum, char model){
  string outfilename = homedir()+BASE_DIR + "SingleGenesFit/"
    + model + "model/ThetaStats/seg" + segnum + "_priors.txt";
  return outfilename;
}

// -------------------------------------------------
// gaussian random number
//
float randn(long *idum)
{
  float ran1(long *idum);
  static int iset=0;
  static float gset;
  float fac,rsq,v1,v2;
  
  if (*idum < 0) iset=0;
  if  (iset == 0) {
    do {
      v1=2.0*ran1(idum)-1.0;
      v2=2.0*ran1(idum)-1.0;
      rsq=v1*v1+v2*v2;
    } while (rsq >= 1.0 || rsq == 0.0);
    fac=sqrt(-2.0*log(rsq)/rsq);
    gset=v1*fac;
    iset=1;
    return v2*fac;
  } else {
    iset=0;
    return gset;
  }
}

// -------------------------------------------------
// randperm: create a random permutation
void randperm (int *perm, int n) {
  int to,tmp,from;
  for (int i=0;i<n;i++){
    from =i;
    to  = (int)floor((double)rand()/RAND_MAX*n);
    tmp       = perm[to];
    perm[to]  = perm[from];
    perm[from]= tmp;    
  }  
  return;
}
// -------------------------------------------------
// print_theta
// -------------------------------------------------
void print_theta(float*theta,char model) {
  switch (model){
  case 'A':
  case 'G':
  case 'E':    
    cout << "theta=["
	 << " h0="  << theta[0]	 << " h1="  << theta[1]
	 << " h2="  << theta[2]	 << " t1="  << theta[3]
	 << " t2="  << theta[4]	 << " beta="<< theta[5]	 << "]\n";
    break;
      case 'B':
    cout << "theta=["
	 << " h0="  << theta[0]	 << " h1="  << theta[1]
	 << " t1="  << theta[2]	 << " beta="<< theta[3]	 << "]\n";
    break;
  case 'C':
  case 'H':
  case 'F':	
    cout << "theta=["
	 << " h0="  << theta[0]	 << " h1="  << theta[1]
	 << " h2="  << theta[2]	 << " t1="  << theta[3]
	 << " t2="  << theta[4]	 << " b1="  << theta[5]
	 << " b2="  << theta[6]	 << "]\n";      
    break;
  case 'D':
    cout << "theta=["
	 << " h0="  << theta[0]	 << " h1="  << theta[1]
	 << " h2="  << theta[2]	 << " h3="  << theta[3]
	 << " h4="  << theta[4]	 << " t1="  << theta[5]
	 << " t2="  << theta[6]	 << " t3="  << theta[7]
	 << " t4="  << theta[8]	 << " b2="<< theta[9]	 << "]\n";
    break;
  }
  return;
}

// -------------------------------------------------
// sigmoid: Calculate the y's of a given sigmoid
// -------------------------------------------------
void sigmoid (float *local_xs, float *local_ys, int n,	      
	      float x_shift, float slope, float y_scale, float y_shift) {
  for (int i=0;i<n;i++){
    local_ys[i]= y_shift + y_scale/(1+exp(-slope*(local_xs[i]-x_shift)));
  }

  //cout << y_scale<< "+" << y_scale <<"/"<< exp(-slope*(local_xs[0]-x_shift))<<endl;
  return;
}
    
// -------------------------------------------------
// calc_ys_Amodel:
// calculate the y's according to given model parameters theta
// -------------------------------------------------
void calc_ys_Amodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float t1=theta[3];
  float t2=theta[4];
  float beta=theta[5];  
  int n = local_n_points;

  // Sigmoids
  float *sig1 = new float[n];  sigmoid(local_xs,sig1,n,t1,+beta,h1-h0,h0);
  float *sig2 = new float[n];  sigmoid(local_xs,sig2,n,t2,-beta,h1-h2,h2);
  
  // Product
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i]*sig2[i]/h1;
  }

  delete [] sig1;
  delete [] sig2;  
  
  return; 
}

// -------------------------------------------------
// calc_ys_Bmodel:
// calculate the y's according to given model parameters theta
// -------------------------------------------------
void calc_ys_Bmodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float t1=theta[2];
  float beta=theta[3];  

  int n = local_n_points;
  if(0){print_theta(theta,'B'); }
		   
  float *sig1 = new float[n];
  sigmoid(local_xs,sig1,n,t1,+beta,h1-h0,h0);
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i];
  }

  delete [] sig1;
  return; 
}
// -------------------------------------------------
// calc_ys_Cmodel: calculate the y's for given theta
// -------------------------------------------------
void calc_ys_Cmodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float t1=theta[3];
  float t2=theta[4];
  float b1=theta[5];
  float b2=theta[6];
  int n = local_n_points;

  // Sigmoids
  float *sig1 = new float[n];  sigmoid(local_xs,sig1,n,t1,b1,h1-h0,h0);  
  float *sig2 = new float[n];  sigmoid(local_xs,sig2,n,t2,b2,h1-h2,h2);
  
  // Product
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i]*sig2[i]/h1;
  }

  delete [] sig1;  delete [] sig2;
  return; 
}
// -------------------------------------------------
// calc_ys_Dmodel: calculate the y's for given theta
// -------------------------------------------------
void calc_ys_Dmodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float h3=theta[3];
  float h4=theta[4];  
  float t1=theta[5];
  float t2=theta[6];
  float t3=theta[7];
  float t4=theta[8];  
  float b =theta[9];
  int n = local_n_points;

  // Sigmoids
  float *sig1=new float[n]; sigmoid(local_xs,sig1,n,t1,+b,h1-h0,h0);  
  float *sig2=new float[n]; sigmoid(local_xs,sig2,n,t2,-b,h1-h2,h2);
  float *sig3=new float[n]; sigmoid(local_xs,sig3,n,t3,+b,h3-h2,h0);  
  float *sig4=new float[n]; sigmoid(local_xs,sig4,n,t4,-b,h3-h4,h4);
    
  // Sum of Products
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i]*sig2[i]/h1 + sig3[i]*sig4[i]/h3;
  }

  delete [] sig1;
  delete [] sig2;  
  return; 
}

// -------------------------------------------------
// calc_ys_Emodel:
// calculate the y's according to given model parameters theta
// -------------------------------------------------
void calc_ys_Emodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float t1=theta[3];
  float t2=theta[4];
  float beta=theta[5];  

  int n = local_n_points;

  // Sigmoids
  float *sig1 = new float[n];  sigmoid(local_xs,sig1,n,t1,+beta,h1-h0,h0);
  float *sig2 = new float[n];  sigmoid(local_xs,sig2,n,t2,-beta,h1-h2,h2);
  
  // Sum
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i]+sig2[i];
  }
  delete [] sig1;  delete [] sig2;  
  return; 
}

// -------------------------------------------------
// calc_ys_Fmodel: calculate the y's for given theta
// -------------------------------------------------
void calc_ys_Fmodel(float *local_xs, float* local_ys, float *theta, int local_n_points)
{
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float t1=theta[3];
  float t2=theta[4];
  float b1=theta[5];
  float b2=theta[6];
  int n = local_n_points;

  // Sigmoids
  float *sig1 = new float[n];  sigmoid(local_xs,sig1,n,t1,b1,h1-h0,h0);  
  float *sig2 = new float[n];  sigmoid(local_xs,sig2,n,t2,b2,h1-h2,h2);
  
  // Product
  for(int i=0;i<n;i++){
    local_ys[i] = sig1[i]+sig2[i];
  }

  delete [] sig1;
  delete [] sig2;
  return; 
}

// -------------------------------------------------
// derivative of gauss pdf (dp/dx)
// -------------------------------------------------
float deriv_pdf_gauss(float *x, int i_x, float *means,float *covars,
		      int n,int mode) {
  // i_x is the index of the x (x_i), at which we differentiate.
  float df=0;
  switch(mode){
    case 0: // Spheric
    cout << " Spheric mode Not implemented yet" ;
    break;
    case 1: // Diagonal
      // In this model  
      // dp/dx_i = N(x,mu,std) * (x_i-mu_i)/std_i
      ///cout << "LOG=" << log_pdf_gauss(x,means,covars,n,mode) << endl;
      df = exp(log_pdf_gauss(x,means,covars,n,mode));
      df *= -(x[i_x]-means[i_x])/(2*covars[i_x]);
    break;
   case 2: // General
    cout << " General mode Not implemented yet" ;
    break;
  default:
    cerr << "Illegal mode = "<< mode << ".\n";
    break;
  }
  return df;
}

// -------------------------------------------------
// pdf gauss
// -------------------------------------------------
float log_pdf_gauss(float *x, float *means,float *covars,int n,int mode) {
  float logf=0;
  switch(mode){
    case '0': // Spheric
    cout << " Spheric mode Not implemented yet" ;
    break;
    case '1': // Diagonal
      // Normalizing factor
      logf = - n/2.0 * log(2.0*PI);
      for(int i=0;i<n;i++){
	logf += - log(covars[i])/2.0;
      }
      // Main factor
      for(int i=0;i<n;i++){
	logf += - (x[i]-means[i])/covars[i]*(x[i]-means[i])/2.0;
      }      
    break;
    case '2': // General
    cout << " General mode Not implemented yet" ;
    break;  
  }
  return logf;
}

// -------------------------------------------------
// pdf gauss
// -------------------------------------------------
float pdf_gauss(float *x, float *means,float *covars,int n,int mode) {
  return exp(log_pdf_gauss(x,means,covars,n,mode));
}

// -------------------------------------------------
// calc_emp_prior
// calculate probability of a parm vector x, according to a given prior distribution
// -------------------------------------------------
float calc_emp_prior(float *x, float **means, float **covar, float *weights,
		     int n_clusters, int dim, int mode) {
  float f=0;
  for(int i_clust=0;i_clust<n_clusters;i_clust++){
    f += weights[i_clust] * 
      pdf_gauss(x, means[i_clust],covar[i_clust],dim,mode);
  }
  return f;
}

// -------------------------------------------------
// calc_hand_prior
// calculate probability of a parm vector x, according to a given
// prior distribution
// -------------------------------------------------
float calc_hand_prior(float *x, int n)
{
  float beta = x[n-1];
  // float p = exp(-fabs(beta));
  float minus_logp = fabs(beta);  
  //cout << "\t\tcalc_hand_prior\n";  
  return minus_logp;
}

// -------------------------------------------------
// calc_deriv_hand_prior
// -------------------------------------------------
float calc_deriv_hand_prior(float *x, int n)
{
  float beta = x[n-1];
  // float p = exp(-fabs(beta));
  // float minus_logp = fabs(beta);  
  // float d_minuslogp_d_beta = sign(beta);    
  return ((beta>0) ? + 1 : -1);
}


// -------------------------------------------------
// calc_deriv_model_log_emp_prior
// -------------------------------------------------
float calc_deriv_model_log_emp_prior(float *x, int i_x, float **means,
				     float **covar, float *weights, int n_clusters,
				     int dim, int mode)
{
  // d log(prior) / dtheta = 
  //  1/prior \sum_i p(c_i) gauss(x,mu_i,std_i) * (x-mu_i)/ 2 std_i  
  float f=0;

  for(int i_clust=0;i_clust<n_clusters;i_clust++){
    f += weights[i_clust] * 
      deriv_pdf_gauss(x, i_x, means[i_clust], covar[i_clust], dim, mode);
  }

  float emp_prior =	calc_emp_prior(x, thetas_means, thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_AMODEL_PARMS, mode);  
  return f/emp_prior;
  
}

// -------------------------------------------------
// calc_deriv_Amodel
// -------------------------------------------------
void calc_deriv_Amodel(float *theta, float *d_thetas){
  // Init 
  float h0=theta[0];
  float h1=theta[1];
  float h2=theta[2];
  float t1=theta[3];
  float t2=theta[4];
  float b1=theta[5];  
  int n = global_n_points;
  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,+b1,1,0);
  float *S2 = new float[n]; sigmoid(xs,S2,n,t2,-b1,1,0);
 
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,+b1,h1-h0,h0);
  float *s2   = new float[n]; sigmoid(xs,s2,n,t2,-b1,h1-h2,h2);

  vector<float> f(n);
  float dh0 = 0.0, dh1 = 0.0, dh2 = 0.0;
  float dt1 = 0.0, dt2 = 0.0, db1 = 0.0;
  float invh1 = 1.0/h1;
  float e;
  
  for(int i=0;i<n;i++){
    f[i] = s1[i]*s2[i]*invh1;
    e    = (ys[i]-f[i])*invh1;
    
    dh0 += e * (-1 * (1-S1[i])*s2[i]);
    dh1 += e * (-1 * s1[i]*s2[i]*invh1 + S1[i]*s2[i] + S2[i]*s1[i]);
    dh2 += e * (-1 * (1-S2[i])*s1[i]);
    dt1 += e * (+1 * (+b1)*(h1-h0)*S1[i]*(1-S1[i])*s2[i]);
    dt2 += e * (+1 *( +b1)*(h1-h2)*S2[i]*(1-S2[i])*s1[i]);
    db1 += e * (+1 * s2[i]*(t1-xs[i])*(h1-h0)*S1[i]*(1-S1[i]));
    db1 += e * (+1 * s1[i]*(t2-xs[i])*(h1-h2)*S2[i]*(1-S2[i]));
  }
  d_thetas[0] = dh0;
  d_thetas[1] = dh1;  
  d_thetas[2] = dh2;  
  d_thetas[3] = dt1;  
  d_thetas[4] = dt2;    
  d_thetas[5] = db1;  

  // Add ampirical priors on thetas   
  if(thetas_do_emp_priors){
    for(int i_theta=0;i_theta<N_AMODEL_PARMS;i_theta++){
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta, thetas_means,
				       thetas_covar,
				       thetas_emp_prior, N_CLUSTERS,
				       N_AMODEL_PARMS, GAUSSIAN_MODE);
    }
  }

  // Add prior's derivative
  if(thetas_do_hand_priors){
    d_thetas[5] += 
      global_hand_prior_weight * calc_deriv_hand_prior(theta, N_MODEL_PARMS);
  }
  delete [] S1;  delete [] S2;
  delete [] s1;  delete [] s2;
  return; 
}

// -------------------------------------------------
// calc_deriv_Bmodel
// -------------------------------------------------
void calc_deriv_Bmodel(float *theta, float *d_thetas) {
  // Init 
  float h0=theta[0], h1=theta[1];
  float t1=theta[2], b1=theta[3];  
  int n = global_n_points;
  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,+b1,1,0);
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,+b1,h1-h0,h0);

  vector<float> f(n);
  float dh0 = 0.0, dh1 = 0.0;
  float dt1 = 0.0, db1 = 0.0;
  float invh1 = 1.0/h1;
  for(int i=0;i<n;i++){
    f[i] = s1[i];
    dh0 += (f[i]-ys[i]) * (1-S1[i]);
    dh1 += (f[i]-ys[i]) * S1[i];
    dt1 += (f[i]-ys[i]) * +b1 * (h1-h0) * S1[i]*(1-S1[i]);
    db1 += (f[i]-ys[i]) * (t1-xs[i])*(h1-h0)*S1[i]*(1-S1[i]);
  }

  d_thetas[0] = dh0;  d_thetas[1] = dh1;  
  d_thetas[2] = dt1;  d_thetas[3] = db1;  
  
  if(thetas_do_emp_priors){
    // Add priors on thetas 
    for(int i_theta=0;i_theta<N_BMODEL_PARMS;i_theta++){
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta, thetas_means, thetas_covar,
			     thetas_emp_prior, N_CLUSTERS, N_BMODEL_PARMS, GAUSSIAN_MODE);
    }
  }
  delete [] S1;  delete [] s1;
  return; 
}
// -------------------------------------------------
// calc_deriv_Cmodel
// -------------------------------------------------
void calc_deriv_Cmodel(float *theta, float *d_thetas) {
  // Init 
  float h0=theta[0], h1=theta[1], h2=theta[2];
  float t1=theta[3], t2=theta[4];
  float b1=theta[5], b2=theta[6];
  
  int n = global_n_points;
  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,b1,1,0);
  float *S2 = new float[n]; sigmoid(xs,S2,n,t2,b2,1,0);
  
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,b1,h1-h0,h0);
  float *s2   = new float[n]; sigmoid(xs,s2,n,t2,b2,h1-h2,h2);

  // Init
  vector<float> f(n);  
  float dh0 = 0.0;
  float dh1 = 0.0;
  float dh2 = 0.0;
  float dt1 = 0.0;
  float dt2 = 0.0;
  float db1 = 0.0;
  float db2 = 0.0;
  float invh1 = 1.0/h1;
  float e;
  
  for(int i=0;i<n;i++){
    f[i] = s1[i]*s2[i] * invh1;
    e = (ys[i]-f[i]) * invh1;    
    dh0 += e * ( -1 * (1-S1[i]) * s2[i]);
    dh1 += e * ( -1 * invh1*s1[i]*s2[i] + S1[i]*s2[i] + S2[i]*s1[i]);
    dh2 += e * ( -1 * (1-S2[i]) * s1[i]);
    dt1 += e * ( +1 * (+b1)*(h1-h0)*S1[i]*(1-S1[i])*s2[i]);  
    dt2 += e * ( +1 * (+b2)*(h1-h2)*S2[i]*(1-S2[i])*s1[i]);
    db1 += e * ( +1 * s2[i]*(t1-xs[i])*(h1-h0)*S1[i]*(1-S1[i]));
    db2 += e * ( +1 * s1[i]*(t2-xs[i])*(h1-h2)*S2[i]*(1-S2[i]));
  }

  d_thetas[0] = dh0;  d_thetas[1] = dh1;    d_thetas[2] = dh2;  
  d_thetas[3] = dt1;  d_thetas[4] = dt2;    
  d_thetas[5] = db1;  d_thetas[6] = db2;  
  
  if(thetas_do_emp_priors){
    // Add priors on thetas 
    for(int i_theta=0;i_theta<N_AMODEL_PARMS;i_theta++){
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta,
				       thetas_means, thetas_covar,
				       thetas_emp_prior, N_CLUSTERS,
				       N_AMODEL_PARMS, GAUSSIAN_MODE);
    }
  }
  // Add prior's derivative  
  if(thetas_do_hand_priors){
    d_thetas[5] += 
      global_hand_prior_weight * calc_deriv_hand_prior(theta, N_MODEL_PARMS);
    d_thetas[6] += 
      global_hand_prior_weight * calc_deriv_hand_prior(theta, N_MODEL_PARMS);
  }
  delete [] S1;  delete [] S2;
  delete [] s1;  delete [] s2;
  return; 
}


// -------------------------------------------------
// calc_deriv_Dmodel: 
// -------------------------------------------------
void calc_deriv_Dmodel(float *theta, float *d_thetas)
{
  // Init
  float h0=theta[0], h1=theta[1], h2=theta[2];
  float h3=theta[3], h4=theta[4];  
  float t1=theta[5], t2=theta[6];
  float t3=theta[7], t4=theta[8];  
  float b1=theta[9], b2=-b1;
  int n = global_n_points;  

  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,b1,1,0);
  float *S2 = new float[n]; sigmoid(xs,S2,n,t2,b2,1,0);
  float *S3 = new float[n]; sigmoid(xs,S3,n,t3,b1,1,0);
  float *S4 = new float[n]; sigmoid(xs,S4,n,t4,b2,1,0);
  
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,b1,h1-h0,h0);
  float *s2   = new float[n]; sigmoid(xs,s2,n,t2,b2,h1-h2,h2);
  float *s3   = new float[n]; sigmoid(xs,s1,n,t3,b1,h3-h2,h2);
  float *s4   = new float[n]; sigmoid(xs,s2,n,t4,b2,h3-h4,h4);  

  vector<float> f(n);
  float dh0 = 0.0, dh1 = 0.0, dh2 = 0.0, dh3 = 0.0, dh4 = 0.0;
  float dt1 = 0.0, dt2 = 0.0, dt3 = 0.0, dt4 = 0.0;
  float db1 = 0.0;  
  float invh1 = 1.0/h1;
  float invh3 = 1.0/h3;
  float e1, e3;
  
  for(int i=0;i<n;i++){
    f[i] = s1[i]*s2[i]*invh1 + s3[i]*s4[i]*invh3;
    e1 = (ys[i]-f[i])*invh1;
    e3 = (ys[i]-f[i])*invh3;
    dh0 += e1 * (- (1-S1[i])*s2[i]);
    dh1 += e1 * (- s1[i] * s2[i]*invh1 + S1[i]*s2[i] + S2[i]*s1[i]);
    dh2 += e1 * (- (1-S2[i])*s1[i]);
    dh2 += e3 * (- (1-S3[i])*s4[i]);
    dh3 += e3 * (- s3[i] * s4[i]*invh1 + S3[i]*s4[i] + S4[i]*s3[i]);
    dh4 += e3 * (- (1-S4[i])*s3[i]);
    dt1 += e1 * (+ b1*(h1-h0)*S1[i]*(1-S1[i])*s2[i]);
    dt2 += e1 * (+ b2*(h1-h2)*S2[i]*(1-S2[i])*s1[i]);
    dt3 += e3 * (+ b1*(h3-h2)*S3[i]*(1-S3[i])*s4[i]);
    dt4 += e3 * (+ b2*(h3-h4)*S4[i]*(1-S4[i])*s3[i]);
    
    db1 += e1 * (+ s2[i]*(t1-xs[i])*(h1-h0)*S1[i]*(1-S1[i]));
    db1 += e1 * (+ s1[i]*(t2-xs[i])*(h1-h2)*S2[i]*(1-S2[i]));    
    db1 += e3 * (+ s4[i]*(t3-xs[i])*(h3-h2)*S3[i]*(1-S3[i]));
    db1 += e3 * (+ s3[i]*(t4-xs[i])*(h3-h4)*S4[i]*(1-S4[i]));        
  }  

  d_thetas[0] = dh0;  d_thetas[1] = dh1;  d_thetas[2] = dh2;
  d_thetas[3] = dh3;  d_thetas[4] = dh4;
  d_thetas[5] = dt1;  d_thetas[6] = dt2;
  d_thetas[7] = dt3;  d_thetas[8] = dt4;
  d_thetas[9] = db1;  

  if(thetas_do_emp_priors){
    // Add priors on thetas 
    for(int i_theta=0;i_theta<N_AMODEL_PARMS;i_theta++){
      ///cout << "\nbefore: " << d_thetas[i_theta];      
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta, thetas_means, thetas_covar,
			     thetas_emp_prior, N_CLUSTERS, N_AMODEL_PARMS, GAUSSIAN_MODE);
    }
  }
  delete [] S1; delete [] S2;
  delete [] S3; delete [] S4;
  delete [] s1; delete [] s2;  
  delete [] s3; delete [] s4;
  return; 
}


// -------------------------------------------------
// calc_deriv_Emodel
// -------------------------------------------------
void calc_deriv_Emodel(float *theta, float *d_thetas){
  // Init 
  float h0=theta[0], h1=theta[1], h2=theta[2];
  float t1=theta[3], t2=theta[4];
  float b1=theta[5];  
  int n = global_n_points;
  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,+b1,1,0);
  float *S2 = new float[n]; sigmoid(xs,S2,n,t2,-b1,1,0); 
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,+b1,h1-h0,h0);
  float *s2   = new float[n]; sigmoid(xs,s2,n,t2,-b1,h1-h2,h2);

  vector<float> f(n);
  float dh0 = 0.0, dh1 = 0.0, dh2 = 0.0;
  float dt1 = 0.0, dt2 = 0.0;
  float db1 = 0.0;
  float invh1 = 1.0/h1;
  float e;
  
  for(int i=0;i<n;i++){
    f[i] = s1[i]+s2[i];
    e    = (ys[i]-f[i]);
    
    dh0 += e * (1-S1[i]);
    dh1 += e * (S1[i]+S2[i]);
    dh2 += e * (1-S2[i]);
    dt1 += e * ( (-b1)*(h1-h0)*S1[i]*(1-S1[i]));
    dt2 += e * ( (-b1)*(h2-h0)*S2[i]*(1-S2[i]));    
    db1 += e * (-h0 * (t1-xs[i])*S1[i]*(1-S1[i]));
    db1 += e * (-h2 * (t2-xs[i])*S2[i]*(1-S2[i]));
  }
  d_thetas[0] = dh0;
  d_thetas[1] = dh1;  
  d_thetas[2] = dh2;  
  d_thetas[3] = dt1;  
  d_thetas[4] = dt2;    
  d_thetas[5] = db1;  

  if(thetas_do_emp_priors){
    // Add priors on thetas 
    for(int i_theta=0;i_theta<N_AMODEL_PARMS;i_theta++){
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta, thetas_means, thetas_covar,
			     thetas_emp_prior, N_CLUSTERS, N_EMODEL_PARMS,
			     GAUSSIAN_MODE);
    }
  }
  
  delete [] S1;
  delete [] S2;
  delete [] s1;
  delete [] s2;
  
  return; 
}

// -------------------------------------------------
// calc_deriv_Fmodel
// -------------------------------------------------
void calc_deriv_Fmodel(float *theta, float *d_thetas){
  // Init 
  float h0=theta[0], h1=theta[1], h2=theta[2];
  float t1=theta[3], t2=theta[4];
  float b1=theta[5], b2=theta[5];    
  int n = global_n_points;
  float *xs = global_xs;
  float *ys = global_ys;

  // Prepare some vectors;
  float *S1 = new float[n]; sigmoid(xs,S1,n,t1,+b1,1,0);
  float *S2 = new float[n]; sigmoid(xs,S2,n,t2,-b1,1,0);
 
  float *s1   = new float[n]; sigmoid(xs,s1,n,t1,+b1,h1-h0,h0);
  float *s2   = new float[n]; sigmoid(xs,s2,n,t2,-b1,h1-h2,h2);

  vector<float> f(n);
  float dh0 = 0.0, dh1 = 0.0, dh2 = 0.0;
  float dt1 = 0.0, dt2 = 0.0, db1 = 0.0, db2 = 0.0;  
  float invh1 = 1.0/h1;
  float e;
  
  for(int i=0;i<n;i++){
    f[i] = s1[i]+s2[i];
    e    = (ys[i]-f[i]);
    
    dh0 += e * (1-S1[i]);
    dh1 += e * (S1[i]+S2[i]);
    dh2 += e * (1-S2[i]);
    dt1 += e * ( (-b1)*(h1-h0)*S1[i]*(1-S1[i]));
    dt2 += e * ( (-b1)*(h2-h0)*S2[i]*(1-S2[i]));    
    db1 += e * (-h0 * (t1-xs[i])*S1[i]*(1-S1[i]));
    db2 += e * (-h2 * (t2-xs[i])*S2[i]*(1-S2[i]));
  }
  d_thetas[0] = dh0;
  d_thetas[1] = dh1;  
  d_thetas[2] = dh2;  
  d_thetas[3] = dt1;  
  d_thetas[4] = dt2;    
  d_thetas[5] = db1;
  d_thetas[5] = db2;

  if(thetas_do_emp_priors){
    // Add priors on thetas 
    for(int i_theta=0;i_theta<N_AMODEL_PARMS;i_theta++){
      d_thetas[i_theta] += EMP_PRIOR_WEIGHT * 
	calc_deriv_model_log_emp_prior(theta, i_theta, thetas_means, thetas_covar,
			     thetas_emp_prior, N_CLUSTERS, N_FMODEL_PARMS, GAUSSIAN_MODE);
    }
  }  
  delete [] S1;  delete [] S2;
  delete [] s1;  delete [] s2;
  return; 
}

// -------------------------------------------------
// calc_error_Amodel
// -------------------------------------------------
float calc_error_Amodel(float*theta)
{

  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'A'); }
  calc_ys_Amodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    //cout << "d= " << ys_model[i] << " " << global_ys[i] << "\n";
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar,
				     thetas_emp_prior,
				     N_CLUSTERS, N_AMODEL_PARMS,
				     GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  // add hand-tuned prior over thetas
  if(thetas_do_hand_priors){
    float log_hand_prior = calc_hand_prior(theta, N_AMODEL_PARMS);    
    err += global_hand_prior_weight*log_hand_prior;
  }

  // cout << "theta[5] =" <<  theta[5] << endl;
  
  delete [] ys_model;  
  return err;
}

// -------------------------------------------------
// calc_error_Bmodel
// -------------------------------------------------
float calc_error_Bmodel(float*theta)
{

  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'B'); }
  calc_ys_Bmodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_BMODEL_PARMS, GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  delete [] ys_model;  
  return err;
}


// -------------------------------------------------
// calc_error_Cmodel
// -------------------------------------------------
float calc_error_Cmodel(float*theta)
{
  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'C'); }      
  calc_ys_Cmodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_CMODEL_PARMS, GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  
  delete [] ys_model;  
  return err;
}

// -------------------------------------------------
// calc_error_Dmodel
// -------------------------------------------------
float calc_error_Dmodel(float*theta)
{
  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'D'); }      
  calc_ys_Cmodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_CMODEL_PARMS, GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  
  delete [] ys_model;  
  return err;
}


// -------------------------------------------------
// calc_error_Emodel
// -------------------------------------------------
float calc_error_Emodel(float*theta)
{

  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'E'); }
  calc_ys_Emodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    //cout << "d= " << ys_model[i] << " " << global_ys[i] << "\n";
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_EMODEL_PARMS, GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  
  delete [] ys_model;  
  return err;
}

// -------------------------------------------------
// calc_error_Fmodel
// -------------------------------------------------
float calc_error_Fmodel(float*theta)
{
  // Predict ys
  int n = global_n_points;
  float * ys_model = new float [n];

  if(0){print_theta(theta,'F'); }
  calc_ys_Fmodel(global_xs, ys_model, theta, n);

  float err=0.0;
  for(int i=0; i<n; i++){
    //cout << "d= " << ys_model[i] << " " << global_ys[i] << "\n";
    err+= (ys_model[i]-global_ys[i])*(ys_model[i]-global_ys[i]);
  }

  // add prior over thetas ("empirial bayes")
  if(thetas_do_emp_priors){
    float emp_prior = calc_emp_prior(theta,thetas_means,thetas_covar, thetas_emp_prior,
			   N_CLUSTERS, N_FMODEL_PARMS, GAUSSIAN_MODE);    
    err += EMP_PRIOR_WEIGHT*log(emp_prior);
  }

  delete [] ys_model;  
  return err;
}

// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Gmodel(float *best_theta,
			       int zero_transform,
			       int n_repeats)
{

  void frprmn(float best_theta[],int n,
              float ftol, int *iter, float *fret,
              float (*func)(float [] ),
              void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_AMODEL_PARMS];
  float *theta_base = new float[N_AMODEL_PARMS];

  // Init
  int firstPt = MODEL_GH_NUM_EXTRA_ZEROS + zero_transform; // index of first real pt
  int numPts = global_n_points - MODEL_GH_NUM_EXTRA_ZEROS - zero_transform;
  int totalPts = global_n_points;
  
  theta_base[0]= global_ys[firstPt];
  theta_base[1]= global_ys[firstPt + (int)floor(0.33*numPts)];
  theta_base[2]= (global_ys[totalPts-1]+global_ys[totalPts-2])/2;
  theta_base[3]= global_xs[firstPt + (int)floor(0.33*numPts)];
  theta_base[4]= global_xs[firstPt + (int)floor(0.66*numPts)];
  theta_base[5]= 0.1;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //cout << " "<<i_repeat;
    for(int i=0;i<N_AMODEL_PARMS;i++){
      float r = +randn(&global_seed)*1.25;
      theta[i] = theta_base[i]*(1+r);
    }

    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_AMODEL_PARMS,
           ftol, &iter, &err, calc_error_Amodel, calc_deriv_Amodel);

    // Save good results
    if(err<best_err){
      for(int i=0;i<N_AMODEL_PARMS;i++){
        best_theta[i] = theta[i];
        best_err = err;
      }
    }
  }
  //cout << "finished loop on n_repeats\n";
    
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}

// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Hmodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
              float ftol, int *iter, float *fret,
              float (*func)(float [] ),
              void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_CMODEL_PARMS];
  float *theta_base = new float[N_CMODEL_PARMS];

  // Init
  int firstPt = MODEL_GH_NUM_EXTRA_ZEROS + zero_transform; // index of first real pt
  int numPts = global_n_points - MODEL_GH_NUM_EXTRA_ZEROS - zero_transform;
  int totalPts = global_n_points;

  theta_base[0]= global_ys[firstPt];
  theta_base[1]= global_ys[firstPt + (int)floor(0.33*numPts)];
  theta_base[2]= (global_ys[totalPts-1]+global_ys[totalPts-2])/2;
  theta_base[3]= global_xs[firstPt + (int)floor(0.33*numPts)];
  theta_base[4]= global_xs[firstPt + (int)floor(0.66*numPts)];
  theta_base[5]= 0.1;
  theta_base[6]= 0.1;

  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_CMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }

    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_CMODEL_PARMS,
           ftol, &iter, &err, calc_error_Cmodel, calc_deriv_Cmodel);

    // Save good results
    if(err<best_err){
      for(int i=0;i<N_CMODEL_PARMS;i++){
        best_theta[i] = theta[i];
        best_err = err;
      }
    }

  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}



// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Amodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_AMODEL_PARMS];
  float *theta_base = new float[N_AMODEL_PARMS];  
  
  // Init
  int n = global_n_points - zero_transform;
  theta_base[0]= global_ys[0]                      ;
  theta_base[1]= global_ys[(int)floor(0.33*n)]     ;
  theta_base[2]= (global_ys[n-1]+global_ys[n-2])/2 ;
  theta_base[3]= global_xs[(int)floor(0.33*n)]     ;
  theta_base[4]= global_xs[(int)floor(0.66*n)]     ;
  theta_base[5]= 0.1                               ;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  // cout << "n_repeats ="<<n_repeats << endl;
  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    // cout << " "<<i_repeat;    
    for(int i=0;i<N_AMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_AMODEL_PARMS,
	   ftol, &iter, &err, calc_error_Amodel, calc_deriv_Amodel);

    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_AMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}


// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Bmodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
 
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_BMODEL_PARMS];
  float *theta_base = new float[N_BMODEL_PARMS]; 
  
  // Init
  int n = global_n_points - zero_transform;
  theta_base[0]= global_ys[0]                      ;
  theta_base[1]= global_ys[(int)floor(0.33*n)]     ;
  theta_base[2]= global_xs[(int)floor(0.33*n)]     ;
  theta_base[3]= 0.1                               ;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_BMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_BMODEL_PARMS,
	   ftol, &iter, &err, calc_error_Bmodel, calc_deriv_Bmodel);

    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_BMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}


// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Cmodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_CMODEL_PARMS];
  float *theta_base = new float[N_CMODEL_PARMS];  
  
  // Init
  int n = global_n_points - zero_transform;
  theta_base[0]= global_ys[0]                      ;
  theta_base[1]= global_ys[(int)floor(0.33*n)]     ;
  theta_base[2]= (global_ys[n-1]+global_ys[n-2])/2 ;
  theta_base[3]= global_xs[(int)floor(0.33*n)]     ;
  theta_base[4]= global_xs[(int)floor(0.66*n)]     ;
  theta_base[5]= 0.1                               ;
  theta_base[6]= 0.1                               ;  


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_CMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_CMODEL_PARMS,
	   ftol, &iter, &err, calc_error_Cmodel, calc_deriv_Cmodel);

    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_CMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}



// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Dmodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_DMODEL_PARMS];
  float *theta_base = new float[N_DMODEL_PARMS];  
  
  // Init
  int n = global_n_points - zero_transform;
  theta_base[0]= global_ys[0]                      ;
  theta_base[1]= global_ys[(int)floor(0.20*n)]     ;
  theta_base[3]= global_ys[(int)floor(0.40*n)]     ;
  theta_base[3]= global_ys[(int)floor(0.60*n)]     ;
  theta_base[4]= (global_ys[n-1]+global_ys[n-2])/2 ;
  theta_base[5]= global_xs[(int)floor(0.20*n)]     ;
  theta_base[6]= global_xs[(int)floor(0.40*n)]     ;
  theta_base[7]= global_xs[(int)floor(0.60*n)]     ;
  theta_base[8]= global_xs[(int)floor(0.80*n)]     ;
  theta_base[9]= 0.1                               ;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_DMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_DMODEL_PARMS,
     	   ftol, &iter, &err, calc_error_Dmodel, calc_deriv_Dmodel);
    
    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_DMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}


// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Emodel(float *best_theta,int zero_transform,int n_repeats)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_EMODEL_PARMS];
  float *theta_base = new float[N_EMODEL_PARMS];  
  
  // Init
  int n = global_n_points - zero_transform;
  theta_base[0]= global_ys[0]                      ;
  theta_base[1]= global_ys[(int)floor(0.33*n)]     ;
  theta_base[2]= (global_ys[n-1]+global_ys[n-2])/2 ;
  theta_base[3]= global_xs[(int)floor(0.33*n)]     ;
  theta_base[4]= global_xs[(int)floor(0.66*n)]     ;
  theta_base[5]= 0.1                               ;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_EMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_EMODEL_PARMS,
	   ftol, &iter, &err, calc_error_Emodel, calc_deriv_Emodel);

    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_EMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}


// -------------------------------------------------
// cg_fit use conjugate gradient for fitting the parameters
// -------------------------------------------------
double cg_fit_single_nr_Bmodel(float *best_theta,int zero_transform,int n_repeats,int n_extra_zeros)
{

  void frprmn(float best_theta[],int n,
	      float ftol, int *iter, float *fret,
	      float (*func)(float [] ),
	      void (*dfunc)(float [], float []));
 
  int iter;
  float err;
  float best_err=999.9;
  float *theta = new float[N_BMODEL_PARMS];
  float *theta_base = new float[N_BMODEL_PARMS]; 


  // Init
  int firstPt = n_extra_zeros + zero_transform; // index of first real pt
  int numPts = global_n_points - n_extra_zeros - zero_transform;
  int totalPts = global_n_points;
  theta_base[0]= global_ys[firstPt]                      ;
  theta_base[1]= global_ys[firstPt + (int)floor(0.33*numPts)]     ;
  theta_base[2]= global_xs[firstPt + (int)floor(0.33*numPts)]     ;
  theta_base[3]= 0.1                               ;


  if(theta_base[1]<0.01 & theta_base[1]>-0.01){
    if(theta_base[1]>0){
      theta_base[1] = 0.01;
    }else{
      theta_base[1] = -0.01;
    }
  }

  for(int i_repeat=0;i_repeat<n_repeats;i_repeat++){
    //    cout << "init theta: ";
    for(int i=0;i<N_BMODEL_PARMS;i++){
      float r = +randn(&global_seed)*0.75;
      theta[i] = theta_base[i]*(1+r);
    }
    
    // Optimize using NR conj-grad
    float ftol = 0.0001*global_n_points;
    frprmn(theta, N_BMODEL_PARMS,
	   ftol, &iter, &err, calc_error_Bmodel, calc_deriv_Bmodel);

    // Save good results 
    if(err<best_err){
      for(int i=0;i<N_BMODEL_PARMS;i++){
	best_theta[i] = theta[i];
	best_err = err;
      }
    }
    
  }
  delete [] theta;
  delete [] theta_base;
  return (best_err);
}

// -------------------------------------------------
// cg_fit_single_nr
// -------------------------------------------------
double cg_fit_single_nr(char model, float *theta,
			int zero_transform,
			int n_repeats)
{
  double err;
  int z = zero_transform;
  switch(model){
  case 'A': err = cg_fit_single_nr_Amodel(theta, z, n_repeats);break;
  case 'B': err = cg_fit_single_nr_Bmodel(theta, z, n_repeats,0);break;
  case 'C': err = cg_fit_single_nr_Cmodel(theta, z, n_repeats);break;
  case 'D': err = cg_fit_single_nr_Dmodel(theta, z, n_repeats);break;      
  case 'G': err = cg_fit_single_nr_Gmodel(theta, z, n_repeats);break;
  case 'H': err = cg_fit_single_nr_Hmodel(theta, z, n_repeats);break;
  case 'I': err = cg_fit_single_nr_Gmodel(theta, z, n_repeats);break;
  case 'J': err = cg_fit_single_nr_Bmodel(theta, z, n_repeats, MODEL_GH_NUM_EXTRA_ZEROS);break;    
  default:
    cerr << "im::cg_fit_single_nr: Illegal model = " << model << "\n";  exit(1);
  }

  return err;
}


// -------------------------------------------------
// count_valid
// -------------------------------------------------
int count_valid(bool ** mask, float ** express, int i_gene, vector<int> segs_inds) {
  int n_valid=0;
  
  //cout << "count_valid: \n";
  // count valid
  unsigned int n_zeros=0;
  for(unsigned int i=0;i<segs_inds.size();i++){
    //cout << "  " << express[i_gene][segs_inds[i]-1];
    if(fabs(express[i_gene][segs_inds[i]-1])<0.0000001){
      n_zeros++;
    }
  }
  if(n_zeros==segs_inds.size()){
    n_valid=0;
    return n_valid;
  }
  
  // count valid
  for(unsigned int i=0;i<segs_inds.size();i++){
    if(mask[i_gene][segs_inds[i]-1]==1){
      n_valid++;
    }
  }

  return n_valid;

}

// -------------------------------------------------
// alloc_xs_ys
// -------------------------------------------------
void alloc_xs_ys(int size)  {
  global_ys = new float[size];
  global_xs = new float[size];
  return;
}


// -------------------------------------------------
// set_xs_ys
// -------------------------------------------------
void set_xs_ys(float ** express, bool  ** mask, int i_gene,
	       vector<float> segs_lags, vector<int> segs_inds,
	       int zero_transform, int num_extra_zeros)  {
  // Set Xs and Ys
  global_n_points=0;
  
  // Add extra zeros
  for (int i= num_extra_zeros; i>0; i--) {
    global_xs[global_n_points] = -10 * i;
    global_ys[global_n_points] = 0;
    global_n_points++;
  }
  if(zero_transform){
    global_xs[global_n_points] = 0;
    global_ys[global_n_points] = 0;
    global_n_points++;
  }
  for(unsigned int i=0;i<segs_inds.size();i++){
    if(mask[i_gene][segs_inds[i]-1]==1){
      global_xs[global_n_points] = segs_lags[i];
      global_ys[global_n_points] = express[i_gene][segs_inds[i]-1];	
      global_n_points++;
    }
  }
  return;
}

// -------------------------------------------------
// estimate_model
// -------------------------------------------------
double estimate_model(float * theta,
		      float** express,
		      bool  ** mask,
		      int i_gene,
		      vector<float> segs_lags,		      
		      vector<int> segs_inds,
		      ofstream& outstream,
		      int zero_transform,
		      char model) {
  
  string segs_inds_name = "segs_inds";
  write_vec_matlab(segs_inds_name,segs_inds);
  string segs_lags_name = "segs_lags";  
  write_vec_matlab(segs_lags_name,segs_lags);        

  //
  int n_valid = count_valid(mask, express, i_gene, segs_inds);
  cout << "n valid "<< n_valid << "\n";

  double err=0.0;
  if(n_valid>2){  
    
    // Number of extra zeros to add
    int num_extra_zeros = 0;
    if (model == 'G' || model == 'H' || model == 'I' || model =='J'){
      num_extra_zeros = MODEL_GH_NUM_EXTRA_ZEROS;
      // model G pushes for df(t)/dt=0 at t=0
    }

    // Set Xs and Ys
    set_xs_ys(express, mask, i_gene, segs_lags, segs_inds,
	      zero_transform, num_extra_zeros);
    
    string gxs_name = "global_xs";
    write_vec_matlab(gxs_name,global_xs,global_n_points);
    write_vec_matlab(outstream,gxs_name,global_xs,global_n_points);    
    string gys_name = "global_ys";
    write_vec_matlab(gys_name,global_ys,global_n_points);
    write_vec_matlab(outstream,gys_name,global_ys,global_n_points);

    // Fit the model    
    err = cg_fit_single_nr(model,theta,zero_transform,N_REPEATS) ;
        
  }else{
    
    // Skip modeling this gene 
    cout << "SKIP: Only " << n_valid << " valid measurments.\n";
    err=0;
    for(int i=0;i<N_MODEL_PARMS;i++){theta[i]=0.0;}

  }

  // Output to screen 
  cout<< "\n  err=" << setw (6) <<err << "\n";
  cout<< " theta=[";for(int i=0;i<N_MODEL_PARMS;i++){
    cout<<fixed<<theta[i]<<" ";
  }cout << "]";
  cout<< setw(6)<<clock()/CLOCKS_PER_SEC << " secs;\n";

  // Output to file   
  outstream << "err("<<i_gene+1<<") = " << err << ";\n";
  outstream << "smooth("<<i_gene+1<<") = " << global_hand_prior_weight <<";\n";
  outstream << "n_valid("<<i_gene+1<<") = " << n_valid <<";\n";
  outstream << "theta{"<<i_gene+1<<"} = [";
  for(int i=0;i<N_MODEL_PARMS;i++){ outstream<<theta[i]<<" "; }outstream << "];\n";

  return(err);
}

// -------------------------------------------------
// estimate_pvals
// -------------------------------------------------
void estimate_pvals(float *  theta,
		    float ** express,
		    bool  ** mask,
		    vector<float> segs_lags,
		    vector<int> segs_inds,
		    int      i_gene,
		    int      n_shuffles,
		    int      n_valid,		    
		    double   err,
		    ofstream &outstream,
		    int zero_transform,
		    char model)
{

  // Headers
  outstream<<"n_shuffles="<< setw (4) <<n_shuffles << ";\n";
  outstream<<"n_valid="<<setfill(' ')<<setw(4)<<n_valid <<";\n";
  outstream<<"rerr=[";
  
  // When not enough measures  
  if(n_valid<=2){ 
    outstream << "];\n";  
    outstream << "pval=NaN;\n";
    outstream << "n_better_random=NaN;\n";
    outstream << "ok=1;\n";
    return; 
  }

  // Number of extra zeros to add
  int num_extra_zeros = 0;
  if (model == 'G' || model == 'H' || model == 'I' || model == 'J'){
    num_extra_zeros = MODEL_GH_NUM_EXTRA_ZEROS;
    // model G enforces df(t)/dt=0 at t=0
  }  

  
  // Set Xs and Ys
  set_xs_ys(express, mask, i_gene, segs_lags, segs_inds,
	    zero_transform, num_extra_zeros);
  
  float *orig_xs=new float[global_n_points];  
  for (int i=0;i<global_n_points;i++){orig_xs[i]=global_xs[i];}
  
  int * perm=new int[global_n_points];
  for (int i=0;i<global_n_points;i++){perm[i]=i;}
  
  int n_better_random=0;
  for(int i_shuffle=0;i_shuffle<n_shuffles; i_shuffle++){
    // Shuffle 
    randperm(perm,global_n_points);
    for (int i=0;i<global_n_points;i++){global_xs[i]=orig_xs[perm[i]];}
    
    //Optimize for shuffled data
    double rerr;
    rerr = cg_fit_single_nr(model,theta,zero_transform,N_REPEATS) ;    

    // Current p-value
    if(rerr<err){n_better_random++;}
    double tmppval = (1.0+n_better_random)/(1.0+i_shuffle);
    
    // Output to screen
    cout << setfill (' ') << setw (4) << i_shuffle << ") "; cout.precision(6);
    cout << setw(6) << fixed <<rerr << " "; cout.precision(6);
    cout << "rthetas=[";for(int i=0;i<N_MODEL_PARMS;i++){
      cout << setw(6)<<fixed <<theta[i]<<" ";
    }cout << "];";      
    cout << "tmppval="<<tmppval<<";";    
    cout << setw(6)<<clock()/CLOCKS_PER_SEC << " secs;\n";      

    // Output to file
    outstream << setprecision (4) <<fixed << rerr << ";";
  }
  
  cout << "n_better_random="<<n_better_random << "\n";
  double pval = (1.0+n_better_random)/(1.0+n_shuffles);
  cout << "pval="<<pval<<"\n";
  
  // Output summary
  outstream << "];\n";  
  outstream << "pval=" << pval << ";\n";
  outstream << "n_better_random=" << n_better_random <<";\n";
  outstream << "ok=1;\n";
  
  return;
}


// -------------------------------------------------
// estimate_stability
// -------------------------------------------------
void estimate_stability(float *  theta,
			float ** express,
			bool  ** mask,
			vector<float> segs_lags,
			vector<int> segs_inds,
			int      i_gene,
			int      n_shuffles,
			int      n_valid,		    
			double   err,
			ofstream &outstream,
			int zero_transform,
			char model)
{

  // Headers
  outstream << "n_shuffles="  << setw(4)     << n_shuffles << ";\n";
  outstream << "n_valid="     << setfill(' ')<< setw(4) <<n_valid <<";\n";
  outstream << "rthetas=zeros("<< n_shuffles  << ","<<N_MODEL_PARMS << ");\n";
  outstream << "rerr=zeros("  << n_shuffles  << ",1);\n";  
  
  // When not enough measures  
  if(n_valid<=2){ return; }

  // Number of extra zeros to add
  int num_extra_zeros = 0;
  if (model == 'G' || model == 'H' || model == 'I' || model == 'J'){    
    // enforce df(t)/dt=0 at t=0    
    num_extra_zeros = MODEL_GH_NUM_EXTRA_ZEROS;
  }
  
  // Set Xs and Ys
  set_xs_ys(express, mask, i_gene, segs_lags, segs_inds,
	    zero_transform, num_extra_zeros);
  
  float *orig_ys=new float[global_n_points];  
  for (int i=0;i<global_n_points;i++){orig_ys[i]=global_ys[i];}
    
  for(int i_shuffle=0;i_shuffle<n_shuffles; i_shuffle++){
    
    // Add noise 
    for (int i=0;i<global_n_points;i++){
      // multiplicative = 0.1, additive noise = 0.05;	      
      switch(global_stability_version){
	float r, r_add;	
      case 1:
	// old noise version
	r = +randn(&global_seed)  *
	  (global_fraction_noise*orig_ys[i]+global_additive_noise);
	global_ys[i] = orig_ys[i] +r;
	break;
	
      case 2: // Changed on March 13, 2006
	r_add  = +randn(&global_seed)*global_additive_noise;
	global_ys[i] = orig_ys[i] +r_add;
	
      case 3: // Added on Apr 21, 2006
	r_add  = +randn(&global_seed)*global_additive_noise*2;
	global_ys[i] = orig_ys[i] +r_add;
	
      case 4: // Added on Apr 21, 2006
	r_add  = +randn(&global_seed)*global_additive_noise*4;
	global_ys[i] = orig_ys[i] +r_add;	

      case 5: // Added on Nov 19, 2007
	r_add  = +randn(&global_seed)*global_additive_noise*6;
	global_ys[i] = orig_ys[i] +r_add;	

      case 6: // Added on Nov 19, 2007
	r_add  = +randn(&global_seed)*global_additive_noise/2;
	global_ys[i] = orig_ys[i] +r_add;

      case 7: // Added on Nov 19, 2007
	r_add  = +randn(&global_seed)*global_additive_noise/5;
	global_ys[i] = orig_ys[i] +r_add;	
      }            
      
    }
    
    //Optimize for noised data
    double rerr;
    rerr = cg_fit_single_nr(model,theta,zero_transform,N_REPEATS) ;    

    // Output to file
    outstream << "rerr("<<i_shuffle+1 << ") = ";    
    outstream << setprecision (4) <<fixed << rerr << ";";
    outstream << "rthetas("<<i_shuffle+1 << ",:)=[";
    for(int i=0;i<N_MODEL_PARMS;i++){
      outstream <<setw(6)<<fixed<<theta[i]<<" ";
    }outstream << "];\n";

    // Output to screen
    cout << setfill (' ')<<setw(4)<<i_shuffle+1<<") ";cout.precision(4);
    cout << setw(6) << fixed <<rerr << " "; cout.precision(4);
    cout << "rthetas("<<i_shuffle+1 << ",:)=[";
    for(int i=0;i<N_MODEL_PARMS;i++){
      cout << setw(6)<<fixed <<theta[i]<<" ";
    }cout << "];%";
    cout << setw(6)<<clock()/CLOCKS_PER_SEC << " secs;\n";
    
  }

  // Output summary
  outstream << "ok=1;\n";  
  
  return;
}



// -------------------------------------------------
// estimate_cvall
// -------------------------------------------------
void estimate_cvall(float * theta,
		    float** express,
		    bool  ** mask,
		    int i_gene,
		    vector<float> segs_lags,
		    vector<int> segs_inds,
		    int n_valid,
		    ofstream &outstream,
		    int zero_transform,
		    char model)
{

  // Headers  
  outstream<<"cvtheta=zeros("<<global_n_points<<","<<N_MODEL_PARMS<<");\n";
  outstream<<"n_valid="<<setfill(' ')<<n_valid<<";\n";
  
  // When not enough measures
  if(n_valid<=2){ return; }
  
  // Save full data aside (only valid points)

  // Number of extra zeros to add
  int num_extra_zeros = 0;
  if (model == 'G' || model == 'H' || model == 'I' || model == 'J'){
    num_extra_zeros = MODEL_GH_NUM_EXTRA_ZEROS;
    // model G enforces df(t)/dt=0 at t=0
  }

  // Set Xs and Ys
  set_xs_ys(express, mask, i_gene, segs_lags, segs_inds, zero_transform, num_extra_zeros);

  cout << global_n_points << endl;
  int full_n_points = global_n_points;
  float *full_xs=new float[full_n_points];
  float *full_ys=new float[full_n_points];
  for(int i=0;i<full_n_points;i++){
    full_xs[i]=global_xs[i];
    full_ys[i]=global_ys[i];
  }
  write_vec_matlab("full_xs",full_xs,full_n_points);
  write_vec_matlab("full_ys",full_ys,full_n_points);
  write_vec_matlab(outstream,"full_xs",full_xs,full_n_points);
  write_vec_matlab(outstream,"full_ys",full_ys,full_n_points);

    
  // Loop over which measure to leave out
  int n_cv = global_n_points;
  double * cv_errs = new double [n_cv];
  for(int i_cv=0;i_cv<n_cv; i_cv++){
    
    // Leave one out
    global_n_points = full_n_points-1;
    for(int i=0;i<i_cv;i++){
      global_ys[i]=full_ys[i];
      global_xs[i]=full_xs[i];    
    }
    for(int i=i_cv+1;i<n_cv;i++){
      global_ys[i-1]=full_ys[i];
      global_xs[i-1]=full_xs[i];    
    }
    //write_vec_matlab("  % global_xs",global_xs,global_n_points);
    //write_vec_matlab("  % global_ys",global_ys,global_n_points);    
      
      
    //Optimize for cv data
    //Optimize for shuffled data
    cv_errs[i_cv] = cg_fit_single_nr(model,theta,zero_transform,N_REPEATS) ;


    // Output to screen
    cout << setfill (' ') << setw (4) << i_cv << ") ";
    cout.precision(6);      
    cout << setw(6) << fixed <<cv_errs[i_cv] << " ";
    cout.precision(6);
    cout << "cvtheta=[";
    for(int i=0;i<N_MODEL_PARMS;i++){
      if(theta[i]<0.01 & theta[i] > -0.01){
	cout << setprecision(10)<<fixed <<theta[i]<<" ";
      }else{
	cout << setprecision(4)<<fixed <<theta[i]<<" ";
      }
    }cout << "];";      
    
    cout << setw(6)<<clock()/CLOCKS_PER_SEC << " secs;\n";      
    
    // Output to file
    outstream<<"cverr("
	     << setfill (' ') << setw(2) <<i_cv+1 <<")="
	     << setw (6) <<fixed << cv_errs[i_cv] << ";";
    outstream<<"cvtheta("
	     << setfill (' ') << setw (4) <<i_cv+1 <<",:)=[";
    for(int i=0;i<N_MODEL_PARMS;i++){outstream << setw(6)<<fixed<<theta[i]<<" ";}
    outstream << "];\n";
  }
    
  return;
}

// -------------------------------------------------
// concat_to_float_mat
// -------------------------------------------------
void concat_to_float_mat(float **&mat,float **mat2,int n_lines,
			int n_fields, int n_fields2)
{
  // Append mat2 to mat1 as additional columns
  
  // Allocate contiguous n_rows by n_columns matrix
  float** out_mat;
  float * tmp_for_alloc_float;
  tmp_for_alloc_float=new float[n_lines*(n_fields+n_fields2)]; 
  out_mat=new float*[n_lines];
  for(int i=0;i<n_lines;i++){ 
    out_mat[i]=&(tmp_for_alloc_float[i*(n_fields+n_fields2)]); 
  }

  cout << "     concat n_fields2=" << n_fields2;
  cout << "to n_fields="<< n_fields << endl;

  // Copy matrices
  for(int i=0; i<n_lines; i++){ 
    for(int ifld=0; ifld<n_fields; ifld++){
      out_mat[i][ifld] = mat[i][ifld];
    }
    for(int ifld=0; ifld<n_fields2; ifld++){
      out_mat[i][ifld+n_fields] = mat2[i][ifld];
    }    
  }

  // Free memeory of mat
  if(n_fields>0){ delete [] mat;  }
  mat = out_mat;

  return;
}

// -------------------------------------------------
// concat_to_bool_mat
// -------------------------------------------------
void concat_to_bool_mat(bool **&mat,bool **mat2,int n_lines,
			int n_fields, int n_fields2) {
  // Append mat2 to mat1 as additional columns
  
  // Allocate contiguous n_rows by n_columns matrix
  bool** out_mat;
  bool * tmp_for_alloc_bool;
  tmp_for_alloc_bool=new bool[n_lines*(n_fields+n_fields2)]; 
  out_mat=new bool*[n_lines];
  for(int i=0;i<n_lines;i++){ 
    out_mat[i]=&(tmp_for_alloc_bool[i*(n_fields+n_fields2)]); 
  }

  // Copy matrices
  for(int i=0; i<n_lines; i++){
    for(int ifld=0; ifld<n_fields; ifld++){
      out_mat[i][ifld] = mat[i][ifld];
    }
    for(int ifld=0; ifld<n_fields2; ifld++){
      out_mat[i][ifld+n_fields] = mat2[i][ifld];
    }    
  }

  // Free memeory of mat
  if(n_fields>0){ delete [] mat; }
  mat = out_mat;
  
  return;
}

/* ============================================== */
void recalc_data(float ** & express,
		 bool **  & mask,
		 int &    n_genes,
		 int &    n_fields)
{


  int      n_skip=1;
  int      n_headers=1;
  
  cout << "Read list of papers\n";  
  vector<string> paper_names;
  string papers_filename= homedir() + BASE_DIR + "papers.lst";
  read_string_vector(papers_filename, paper_names);
  

  // Read from Compendium
  float ** express1; bool  ** mask1; int n_fields1;  
  string expression_filename = homedir() + EXPRESS_DIR  + "Compendium/data.tab";  
  cout << "\nRead Expression from " << expression_filename<<"\n";  
  read_expression_mat(expression_filename, n_skip, n_headers, express1,
		      mask1, n_genes, n_fields1);
  cout << endl;
  cout << "     Read "<< n_genes+1 << " genes.\n";
  cout << "     Read "<< n_fields1 << " fields1.\n";
  
  concat_to_bool_mat (mask, mask1, n_genes, 0, n_fields1);
  concat_to_float_mat(express, express1, n_genes, 0, n_fields1);
  n_fields = n_fields1;
  cout << "     Total "<< n_fields << " fields.\n";        

  // Read from individual papers  
  for(unsigned int i=0; i<paper_names.size();i++){
    string paper_name = paper_names[i];

    string filename = homedir() + EXPRESS_DIR + paper_name + "padded.tab";
    cout << "\n\nRead Expression from " << filename<<"\n";  
    read_expression_mat(filename, n_skip, n_headers, express1,
			mask1, n_genes, n_fields1);
    cout << endl;
    cout << "     Read "<< n_genes+1 << " genes.\n";
    cout << "     Read "<< n_fields1 << " new fields1.\n";    
    concat_to_bool_mat (mask, mask1, n_genes, n_fields, n_fields1);
    concat_to_float_mat(express, express1, n_genes, n_fields, n_fields1);
    n_fields += n_fields1;
    cout << "     Total after concatenation: "<< n_fields << " fields.\n";
  }
}