/*
 *  utility.c
 *
 *  Created by wei sun on Thu Sep 20 2007.
 *
 */


#include "utility.h"
#include <R.h>
#include <Rmath.h>

double log_exp_add(double x, double y){
	if(x==R_PosInf || y==R_PosInf){
		error("sorry, do not expect +Inf in log_exp_add\n");
	}
	if(x==R_NegInf){
		if(y==R_NegInf){
		  return -INFINITY;
		}else{
			return y;
		}
	}else{
		if(y==R_NegInf){
		  return x;
		}else{
			if(x > y){
				return x + log(1+exp(y-x));
			}else{
				return y + log(1+exp(x-y));
			}
		}
	}
}

double log_exp_sub(double x, double y){
	char estr[255];
	if(x==R_PosInf || y==R_PosInf){
		error("sorry, do not expect +Inf in log_exp_sub\n");
	}
	if(x==R_NegInf){
		if(y==R_NegInf){
		  return -INFINITY;
		}else{
			error("log_exp_sub(x,y): x = -Inf, so y has to be -Inf too\n");
		}
	}else{
		if(y==R_NegInf){
		  return x;
		}else{
			if(x < y){
				sprintf(estr, "log_exp_sub(x,y): x cannot be smaller than y");
				sprintf(estr, "%s now x=%f, y=%f\n", estr, x, y);
				error(estr);
			}else if(x==y){
				return -INFINITY;
			}else{
				return x + log(1-exp(y-x));
			}
		}
	}
}

double d_max(double *v, int n){
	int i;
	double m = v[0];
	if(n <= 1){
		return(m);
	}else{
	  for(i=1; i<n; i++){
		  if(m < v[i]){ m = v[i]; }
	  }
  }
  return(m);
}

double d_min(double *v, int n){
	int i;
	double m = v[0];
	if(n <= 1){
		return(m);
	}else{
	  for(i=1; i<n; i++){
		  if(m > v[i]){ m = v[i]; }
	  }
  }
  return(m);
}

/**********************************************************************
 * 
 * reorg
 *
 * Reorganize a vector to a matrix of given size. 
 *
 * Allocation done by R_alloc, so that R does the cleanup.
 *
 **********************************************************************/

void reorg(double *v, double ***m, int nrow, int ncol)
{
    int i;
    
    *m = (double **)R_alloc(nrow, sizeof(double*));
    
    (*m)[0] = v;
    if(nrow>1){
        for(i=1; i<nrow; i++){
            (*m)[i] = (*m)[i-1] + ncol;
        }
    }
}

void reorg_int(int *v, int ***m, int nrow, int ncol)
{
    int i;
    
    *m = (int **)R_alloc(nrow, sizeof(int*));
    
    (*m)[0] = v;
    if(nrow>1){
        for(i=1; i<nrow; i++){
            (*m)[i] = (*m)[i-1] + ncol;
        }
    }
}

/**********************************************************************
 * RANDOME NUMBER GENERATER
 
	On balance, we recommend ran1 for general use. It is portable, based on 
	Parkand Miller's Minimal Standard generator with an additional shuffle, 
	and has no known flaws other than period exhaustion. 
	
	If you are generating more than 100,000,000 random numbers in a single 
	calculation(that is, more than about 5% of ran1's period), we recommend 
	the use of ran2, with its much longer period. 
 **********************************************************************/

/** ---------------------------------------------------------------------
 * Function:	float ran1(long*)
 *				    float ran2(long*)
 * ---------------------------------------------------------------------- 
	Minimal random number generator of Parkand Miller with Bays-Durham shuffle 
	and added safe guards. Returns a uniform random deviate between 0.0 and 1.0 
	(exclusive of the end point values). Call with idum a negative integer to 
	initialize; thereafter, donot alter idum between successive deviates in a 
	sequence. RNMX should approximate the largest floating value that is less 
	than 1. 
*/

#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)

float ran1(long *idum)
{
	int j;
	long k;
	static long iy=0;
	static long iv[NTAB];
	float temp;

	if (*idum <= 0 || !iy) {
		if (-(*idum) < 1) *idum=1;
		else *idum = -(*idum);
		for (j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ;
			*idum=IA*(*idum-k*IQ)-IR*k;
			if (*idum < 0) *idum += IM;
			if (j < NTAB) iv[j] = *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ;
	*idum=IA*(*idum-k*IQ)-IR*k;
	if (*idum < 0) *idum += IM;
	j=iy/NDIV;
	iy=iv[j];
	iv[j] = *idum;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}
#undef IA
#undef IM
#undef AM
#undef IQ
#undef IR
#undef NTAB
#undef NDIV
#undef EPS
#undef RNMX


#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)

float ran2(long *idum)
{
	int j;
	long k;
	static long idum2=123456789;
	static long iy=0;
	static long iv[NTAB];
	float temp;

	if (*idum <= 0) {
		if (-(*idum) < 1) *idum=1;
		else *idum = -(*idum);
		idum2=(*idum);
		for (j=NTAB+7;j>=0;j--) {
			k=(*idum)/IQ1;
			*idum=IA1*(*idum-k*IQ1)-k*IR1;
			if (*idum < 0) *idum += IM1;
			if (j < NTAB) iv[j] = *idum;
		}
		iy=iv[0];
	}
	k=(*idum)/IQ1;
	*idum=IA1*(*idum-k*IQ1)-k*IR1;
	if (*idum < 0) *idum += IM1;
	k=idum2/IQ2;
	idum2=IA2*(idum2-k*IQ2)-k*IR2;
	if (idum2 < 0) idum2 += IM2;
	j=iy/NDIV;
	iy=iv[j]-idum2;
	iv[j] = *idum;
	if (iy < 1) iy += IMM1;
	if ((temp=AM*iy) > RNMX) return RNMX;
	else return temp;
}
#undef IM1
#undef IM2
#undef AM
#undef IMM1
#undef IA1
#undef IA2
#undef IQ1
#undef IQ2
#undef IR1
#undef IR2
#undef NTAB
#undef NDIV
#undef EPS
#undef RNMX


/**********************************************************************
 * 
 * getPermute
 *
 * generate a permutaion from 0 to n-1 using Knuth shuffle
 * http://en.wikipedia.org/wiki/Knuth_shuffle 
 *
 **********************************************************************/

int getPermute(int* per, int n, long *idum) {
  int i, j, v;
  
  for (i=0; i<n; i++) {
    per[i] = i;
  }
 
  for (i=n-1; i>0; i--) {
    j = floor(ran1(idum)*(i+1)); /* a integer from 0 to i */
    v = per[i];
    per[i] = per[j];
    per[j] = v;
  }
  
  return 1;
}

/**********************************************************************
 *
 * permute a (double) matrix and the corresponding index matrix   
 * columnwise according to a permutation vector 
 *
 **********************************************************************/

void permute(double **dat, double **p_dat, int **dati, int **p_dati, int *per, int nrow, int ncol)
{
	int i, j;
	for(i=0; i<nrow; i++){
		for(j=0; j<ncol; j++){
			p_dat[i][j]  = dat[i][per[j]];
			p_dati[i][j] = dati[i][per[j]];
		}
	}
}
