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

#include <itpp/itbase.h>
#include "itpp_ext.h"

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

//Gamma

  Gamma_RNG::Gamma_RNG(double a, double b)
  {
    setup(a,b);
  }

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

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

}