//
// 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
{
	vec empty_vec = vec(0);
	
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;}
	}
}

//! Storage of randun() internals
class RandunStorage
{
		const int A;
		const int M;
		static double seed;
		static int counter;
	public:
		RandunStorage() : A (16807), M (2147483647) {};
		//!set seed of the randun() generator
		void set_seed (double seed0) {seed = seed0;}
		//! generate randun() sample
		double get() {
			long long tmp = A * seed;
			tmp = tmp % 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;};

ivec unique (const ivec &in)
{
	ivec uniq (0);
	int j = 0;
	bool found = false;
	for (int i = 0;i < in.length(); i++) {
		found = false;
		j = 0;
		while ( (!found) && (j < uniq.length())) {
			if (in (i) == uniq (j)) found = true;
			j++;
		}
		if (!found) uniq = concat (uniq, in (i));
	}
	return uniq;
}

ivec unique_complement (const ivec &in, const ivec &base)
{
	// almost a copy of unique
	ivec uniq (0);
	int j = 0;
	bool found = false;
	for (int i = 0;i < in.length(); i++) {
		found = false;
		j = 0;
		while ( (!found) && (j < uniq.length())) {
			if (in (i) == uniq (j)) found = true;
			j++;
		}
		j=0;
		while ( (!found) && (j < base.length())) {
			if (in (i) == base (j)) found = true;
			j++;
		}
		if (!found) uniq = concat (uniq, in (i));
	}
	return uniq;
}

}