//
// C++ Implementation: itpp_ext
//
// Description:
//
//
// Author: smidl <smidl@utia.cas.cz>, (C) 2008
//
// Copyright: See COPYING file that comes with this distribution
//
//

#include "itpp_ext.h"

#ifndef M_PI
#define M_PI        3.14159265358979323846
#endif
// from  algebra/lapack.h

extern "C" {  /* QR factorization of a general matrix A  */
	void dgeqrf_ ( int *m, int *n, double *a, int *lda, double *tau, double *work,
	int *lwork, int *info );
};

namespace itpp {
Array<int> to_Arr ( const ivec &indices ) {
	Array<int> a ( indices.size() );
	for ( int i = 0; i < a.size(); i++ ) {
		a ( i ) = indices ( i );
	}
	return a;
}

ivec linspace ( int from, int to ) {
	int n = to - from + 1;
	int i;
	it_assert_debug ( n > 0, "wrong linspace" );
	ivec iv ( n );
	for ( i = 0; i < n; i++ ) iv ( i ) = from + i;
	return iv;
};

void set_subvector ( vec &ov, const ivec &iv, const vec &v ) {
	it_assert_debug ( ( iv.length() <= v.length() ),
	                  "Vec<>::set_subvector(ivec, vec<Num_T>): Indexing out "
	                  "of range of v" );
	for ( int i = 0; i < iv.length(); i++ ) {
		it_assert_debug ( iv ( i ) < ov.length(),
		                  "Vec<>::set_subvector(ivec, vec<Num_T>): Indexing out "
		                  "of range of v" );
		ov ( iv ( i ) ) = v ( i );
	}
}

vec get_vec ( const vec &v, const ivec &indexlist ) {
	int size = indexlist.size();
	vec temp ( size );
	for ( int i = 0; i < size; ++i ) {
		temp ( i ) = v._data() [indexlist ( i ) ];
	}
	return temp;
}

// Gamma
#define log std::log
#define exp std::exp
#define sqrt std::sqrt
#define R_FINITE std::isfinite

bvec operator> ( const vec &t1, const vec &t2 ) {
	it_assert_debug ( t1.length() == t2.length(), "Vec<>::operator>(): different size of vectors" );
	bvec temp ( t1.length() );
	for ( int i = 0; i < t1.length(); i++ )
		temp ( i ) = ( t1[i] > t2[i] );
	return temp;
}

bvec operator< ( const vec &t1, const vec &t2 ) {
	it_assert_debug ( t1.length() == t2.length(), "Vec<>::operator>(): different size of vectors" );
	bvec temp ( t1.length() );
	for ( int i = 0; i < t1.length(); i++ )
		temp ( i ) = ( t1[i] < t2[i] );
	return temp;
}


bvec operator& ( const bvec &a, const bvec &b ) {
	it_assert_debug ( b.size() == a.size(), "operator&(): Vectors of different lengths" );

	bvec temp ( a.size() );
	for ( int i = 0; i < a.size(); i++ ) {
		temp ( i ) = a ( i ) & b ( i );
	}
	return temp;
}

bvec operator| ( const bvec &a, const bvec &b ) {
	it_assert_debug ( b.size() != a.size(), "operator&(): Vectors of different lengths" );

	bvec temp ( a.size() );
	for ( int i = 0; i < a.size(); i++ ) {
		temp ( i ) = a ( i ) | b ( i );
	}
	return temp;
}

//#if 0
Gamma_RNG::Gamma_RNG ( double a, double b ) {
	setup ( a, b );
}
double  Gamma_RNG::sample() {
	//A copy of rgamma code from the R package!!
	//

	/* Constants : */
	const static double sqrt32 = 5.656854;
	const static double exp_m1 = 0.36787944117144232159;/* exp(-1) = 1/e */

	/* Coefficients q[k] - for q0 = sum(q[k]*a^(-k))
	 * Coefficients a[k] - for q = q0+(t*t/2)*sum(a[k]*v^k)
	 * Coefficients e[k] - for exp(q)-1 = sum(e[k]*q^k)
	 */
	const static double q1 = 0.04166669;
	const static double q2 = 0.02083148;
	const static double q3 = 0.00801191;
	const static double q4 = 0.00144121;
	const static double q5 = -7.388e-5;
	const static double q6 = 2.4511e-4;
	const static double q7 = 2.424e-4;

	const static double a1 = 0.3333333;
	const static double a2 = -0.250003;
	const static double a3 = 0.2000062;
	const static double a4 = -0.1662921;
	const static double a5 = 0.1423657;
	const static double a6 = -0.1367177;
	const static double a7 = 0.1233795;

	/* State variables [FIXME for threading!] :*/
	static double aa = 0.;
	static double aaa = 0.;
	static double s, s2, d;    /* no. 1 (step 1) */
	static double q0, b, si, c;/* no. 2 (step 4) */

	double e, p, q, r, t, u, v, w, x, ret_val;
	double a = alpha;
	double scale = 1.0 / beta;

	if ( !R_FINITE ( a ) || !R_FINITE ( scale ) || a < 0.0 || scale <= 0.0 ) {
		it_error ( "Gamma_RNG wrong parameters" );
	}

	if ( a < 1. ) { /* GS algorithm for parameters a < 1 */
		if ( a == 0 )
			return 0.;
		e = 1.0 + exp_m1 * a;
		for ( ;; ) {  //VS repeat
			p = e * unif_rand();
			if ( p >= 1.0 ) {
				x = -log ( ( e - p ) / a );
				if ( exp_rand() >= ( 1.0 - a ) * log ( x ) )
					break;
			} else {
				x = exp ( log ( p ) / a );
				if ( exp_rand() >= x )
					break;
			}
		}
		return scale * x;
	}

	/* --- a >= 1 : GD algorithm --- */

	/* Step 1: Recalculations of s2, s, d if a has changed */
	if ( a != aa ) {
		aa = a;
		s2 = a - 0.5;
		s = sqrt ( s2 );
		d = sqrt32 - s * 12.0;
	}
	/* Step 2: t = standard normal deviate,
	           x = (s,1/2) -normal deviate. */

	/* immediate acceptance (i) */
	t = norm_rand();
	x = s + 0.5 * t;
	ret_val = x * x;
	if ( t >= 0.0 )
		return scale * ret_val;

	/* Step 3: u = 0,1 - uniform sample. squeeze acceptance (s) */
	u = unif_rand();
	if ( ( d * u ) <= ( t * t * t ) )
		return scale * ret_val;

	/* Step 4: recalculations of q0, b, si, c if necessary */

	if ( a != aaa ) {
		aaa = a;
		r = 1.0 / a;
		q0 = ( ( ( ( ( ( q7 * r + q6 ) * r + q5 ) * r + q4 ) * r + q3 ) * r
		         + q2 ) * r + q1 ) * r;

		/* Approximation depending on size of parameter a */
		/* The constants in the expressions for b, si and c */
		/* were established by numerical experiments */

		if ( a <= 3.686 ) {
			b = 0.463 + s + 0.178 * s2;
			si = 1.235;
			c = 0.195 / s - 0.079 + 0.16 * s;
		} else if ( a <= 13.022 ) {
			b = 1.654 + 0.0076 * s2;
			si = 1.68 / s + 0.275;
			c = 0.062 / s + 0.024;
		} else {
			b = 1.77;
			si = 0.75;
			c = 0.1515 / s;
		}
	}
	/* Step 5: no quotient test if x not positive */

	if ( x > 0.0 ) {
		/* Step 6: calculation of v and quotient q */
		v = t / ( s + s );
		if ( fabs ( v ) <= 0.25 )
			q = q0 + 0.5 * t * t * ( ( ( ( ( ( a7 * v + a6 ) * v + a5 ) * v + a4 ) * v
			                             + a3 ) * v + a2 ) * v + a1 ) * v;
		else
			q = q0 - s * t + 0.25 * t * t + ( s2 + s2 ) * log ( 1.0 + v );


		/* Step 7: quotient acceptance (q) */
		if ( log ( 1.0 - u ) <= q )
			return scale * ret_val;
	}

	for ( ;; ) { //VS repeat
		/* Step 8: e = standard exponential deviate
		 *	u =  0,1 -uniform deviate
		 *	t = (b,si)-double exponential (laplace) sample */
		e = exp_rand();
		u = unif_rand();
		u = u + u - 1.0;
		if ( u < 0.0 )
			t = b - si * e;
		else
			t = b + si * e;
		/* Step	 9:  rejection if t < tau(1) = -0.71874483771719 */
		if ( t >= -0.71874483771719 ) {
			/* Step 10:	 calculation of v and quotient q */
			v = t / ( s + s );
			if ( fabs ( v ) <= 0.25 )
				q = q0 + 0.5 * t * t *
				    ( ( ( ( ( ( a7 * v + a6 ) * v + a5 ) * v + a4 ) * v + a3 ) * v
				        + a2 ) * v + a1 ) * v;
			else
				q = q0 - s * t + 0.25 * t * t + ( s2 + s2 ) * log ( 1.0 + v );
			/* Step 11:	 hat acceptance (h) */
			/* (if q not positive go to step 8) */
			if ( q > 0.0 ) {
				// TODO: w = expm1(q);
				w = exp ( q ) - 1;
				/*  ^^^^^ original code had approximation with rel.err < 2e-7 */
				/* if t is rejected sample again at step 8 */
				if ( ( c * fabs ( u ) ) <= ( w * exp ( e - 0.5 * t * t ) ) )
					break;
			}
		}
	} /* repeat .. until  `t' is accepted */
	x = s + 0.5 * t;
	return scale * x * x;
}


bool qr ( const mat &A, mat &R ) {
	int info;
	int m = A.rows();
	int n = A.cols();
	int lwork = n;
	int k = std::min ( m, n );
	vec tau ( k );
	vec work ( lwork );

	R = A;

	// perform workspace query for optimum lwork value
	int lwork_tmp = -1;
	dgeqrf_ ( &m, &n, R._data(), &m, tau._data(), work._data(), &lwork_tmp,
	          &info );
	if ( info == 0 ) {
		lwork = static_cast<int> ( work ( 0 ) );
		work.set_size ( lwork, false );
	}
	dgeqrf_ ( &m, &n, R._data(), &m, tau._data(), work._data(), &lwork, &info );

	// construct R
	for ( int i = 0; i < m; i++ )
		for ( int j = 0; j < std::min ( i, n ); j++ )
			R ( i, j ) = 0;

	return ( info == 0 );
}

//#endif
std::string num2str ( double d ) {
	char tmp[20];//that should do
	sprintf ( tmp, "%f", d );
	return std::string ( tmp );
};
std::string num2str ( int i ) {
	char tmp[10];//that should do
	sprintf ( tmp, "%d", i );
	return std::string ( tmp );
};

// digamma
// copied from C. Bonds' source
#include <math.h>
#define el 0.5772156649015329

double psi ( double x ) {
	double s, ps, xa, x2;
	int n, k;
	static double a[] = {
		-0.8333333333333e-01,
		0.83333333333333333e-02,
		-0.39682539682539683e-02,
		0.41666666666666667e-02,
		-0.75757575757575758e-02,
		0.21092796092796093e-01,
		-0.83333333333333333e-01,
		0.4432598039215686
	};

	xa = fabs ( x );
	s = 0.0;
	if ( ( x == ( int ) x ) && ( x <= 0.0 ) ) {
		ps = 1e308;
		return ps;
	}
	if ( xa == ( int ) xa ) {
		n = xa;
		for ( k = 1; k < n; k++ ) {
			s += 1.0 / k;
		}
		ps =  s - el;
	} else if ( ( xa + 0.5 ) == ( ( int ) ( xa + 0.5 ) ) ) {
		n = xa - 0.5;
		for ( k = 1; k <= n; k++ ) {
			s += 1.0 / ( 2.0 * k - 1.0 );
		}
		ps = 2.0 * s - el - 1.386294361119891;
	} else {
		if ( xa < 10.0 ) {
			n = 10 - ( int ) xa;
			for ( k = 0; k < n; k++ ) {
				s += 1.0 / ( xa + k );
			}
			xa += n;
		}
		x2 = 1.0 / ( xa * xa );
		ps = log ( xa ) - 0.5 / xa + x2 * ( ( ( ( ( ( ( a[7] * x2 + a[6] ) * x2 + a[5] ) * x2 +
		                                        a[4] ) * x2 + a[3] ) * x2 + a[2] ) * x2 + a[1] ) * x2 + a[0] );
		ps -= s;
	}
	if ( x < 0.0 )
		ps = ps - M_PI * std::cos ( M_PI * x ) / std::sin ( M_PI * x ) - 1.0 / x;
	return ps;
}

void triu(mat &A){
	for(int i=1;i<A.rows();i++) { // row cycle
		for (int j=0; j<i; j++) {A(i,j)=0;}
	}
}

class RandunStorage{
	const int A;
	const int M;
	static double seed;
	static int counter;
	public:
		RandunStorage(): A(16807), M(2147483647) {};
		void set_seed(double seed0){seed=seed0;}
		double get(){
			int tmp=mod(A*seed,M); 
			seed =tmp;
			counter++; 
			return seed/M;}
};
static RandunStorage randun_global_storage;
double RandunStorage::seed=1111111;
int RandunStorage::counter=0;
double randun(){return randun_global_storage.get();};
vec randun(int n){vec res(n); for(int i=0;i<n;i++){res(i)=randun();}; return res;};
mat randun(int n, int m){mat res(n,m); for(int i=0;i<n*m;i++){res(i)=randun();}; return res;};

}