#include "ccv.h"
#include "ccv_internal.h"
#ifdef HAVE_CBLAS
#include <cblas.h>
#elif HAVE_ACCELERATE_FRAMEWORK
#include <Accelerate/Accelerate.h>
#endif

double ccv_trace(ccv_matrix_t* mat)
{
	return 0;
}

double ccv_norm(ccv_matrix_t* mat, int type)
{
	return 0;
}

double ccv_normalize(ccv_matrix_t* a, ccv_matrix_t** b, int btype, int flag)
{
	ccv_dense_matrix_t* da = ccv_get_dense_matrix(a);
	assert(CCV_GET_CHANNEL(da->type) == CCV_C1);
	ccv_declare_derived_signature(sig, da->sig != 0, ccv_sign_with_format(20, "ccv_normalize(%d)", flag), da->sig, CCV_EOF_SIGN);
	btype = (btype == 0) ? CCV_GET_DATA_TYPE(da->type) | CCV_C1 : CCV_GET_DATA_TYPE(btype) | CCV_C1;
	ccv_dense_matrix_t* db = *b = ccv_dense_matrix_renew(*b, da->rows, da->cols, CCV_ALL_DATA_TYPE | CCV_C1, btype, sig);
	assert(db);
	ccv_object_return_if_cached(db->tag.f64, db);
	double sum = 0, inv;
	int i, j;
	unsigned char* a_ptr = da->data.u8;
	unsigned char* b_ptr = db->data.u8;
	switch (flag)
	{
		case CCV_L1_NORM:
#define for_block(_for_set, _for_get) \
			for (i = 0; i < da->rows; i++) \
			{ \
				for (j = 0; j < da->cols; j++) \
					sum += _for_get(a_ptr, j, 0); \
				a_ptr += da->step; \
			} \
			inv = 1.0 / sum; \
			a_ptr = da->data.u8; \
			for (i = 0; i < da->rows; i++) \
			{ \
				for (j = 0; j < da->cols; j++) \
					_for_set(b_ptr, j, _for_get(a_ptr, j, 0) * inv, 0); \
				a_ptr += da->step; \
				b_ptr += db->step; \
			}
			ccv_matrix_setter(db->type, ccv_matrix_getter, da->type, for_block);
#undef for_block
			break;
		case CCV_L2_NORM:
#define for_block(_for_set, _for_get) \
			for (i = 0; i < da->rows; i++) \
			{ \
				for (j = 0; j < da->cols; j++) \
					sum += _for_get(a_ptr, j, 0) * _for_get(a_ptr, j, 0); \
				a_ptr += da->step; \
			} \
			sum = sqrt(sum); \
			inv = 1.0 / sum; \
			a_ptr = da->data.u8; \
			for (i = 0; i < da->rows; i++) \
			{ \
				for (j = 0; j < da->cols; j++) \
					_for_set(b_ptr, j, _for_get(a_ptr, j, 0) * inv, 0); \
				a_ptr += da->step; \
				b_ptr += db->step; \
			}
			ccv_matrix_setter(db->type, ccv_matrix_getter, da->type, for_block);
#undef for_block
			break;
	}
	return db->tag.f64 = sum;
}

void ccv_sat(ccv_dense_matrix_t* a, ccv_dense_matrix_t** b, int type, int padding_pattern)
{
	ccv_declare_derived_signature(sig, a->sig != 0, ccv_sign_with_format(20, "ccv_sat(%d)", padding_pattern), a->sig, CCV_EOF_SIGN);
	int safe_type = (a->type & CCV_8U) ? ((a->rows * a->cols >= 0x808080) ? CCV_64S : CCV_32S) : ((a->type & CCV_32S) ? CCV_64S : a->type);
	type = (type == 0) ? CCV_GET_DATA_TYPE(safe_type) | CCV_GET_CHANNEL(a->type) : CCV_GET_DATA_TYPE(type) | CCV_GET_CHANNEL(a->type);
	int ch = CCV_GET_CHANNEL(a->type);
	int i, j;
	unsigned char* a_ptr = a->data.u8;
	ccv_dense_matrix_t* db;
	unsigned char* b_ptr;
	switch (padding_pattern)
	{
		case CCV_NO_PADDING:
			db = *b = ccv_dense_matrix_renew(*b, a->rows, a->cols, CCV_ALL_DATA_TYPE | CCV_GET_CHANNEL(a->type), type, sig);
			ccv_object_return_if_cached(, db);
			b_ptr = db->data.u8;
#define for_block(_for_set_b, _for_get_b, _for_get) \
			for (j = 0; j < ch; j++) \
				_for_set_b(b_ptr, j, _for_get(a_ptr, j, 0), 0); \
			for (j = ch; j < a->cols * ch; j++) \
				_for_set_b(b_ptr, j, _for_get_b(b_ptr, j - ch, 0) + _for_get(a_ptr, j, 0), 0); \
			a_ptr += a->step; \
			b_ptr += db->step; \
			for (i = 1; i < a->rows; i++) \
			{ \
				for (j = 0; j < ch; j++) \
					_for_set_b(b_ptr, j, _for_get_b(b_ptr - db->step, j, 0) + _for_get(a_ptr, j, 0), 0); \
				for (j = ch; j < a->cols * ch; j++) \
					_for_set_b(b_ptr, j, _for_get_b(b_ptr, j - ch, 0) - _for_get_b(b_ptr - db->step, j - ch, 0) + _for_get_b(b_ptr - db->step, j, 0) + _for_get(a_ptr, j, 0), 0); \
				a_ptr += a->step; \
				b_ptr += db->step; \
			}
			ccv_matrix_setter_getter(db->type, ccv_matrix_getter, a->type, for_block);
#undef for_block
			break;
		case CCV_PADDING_ZERO:
			db = *b = ccv_dense_matrix_renew(*b, a->rows + 1, a->cols + 1, CCV_ALL_DATA_TYPE | CCV_GET_CHANNEL(a->type), type, sig);
			ccv_object_return_if_cached(, db);
			b_ptr = db->data.u8;
#define for_block(_for_set_b, _for_get_b, _for_get) \
			for (j = 0; j < db->cols * ch; j++) \
				_for_set_b(b_ptr, j, 0, 0); \
			b_ptr += db->step; \
			for (i = 0; i < a->rows; i++) \
			{ \
				for (j = 0; j < ch; j++) \
					_for_set_b(b_ptr, j, 0, 0); \
				for (j = ch; j < db->cols * ch; j++) \
					_for_set_b(b_ptr, j, _for_get_b(b_ptr, j - ch, 0) - _for_get_b(b_ptr - db->step, j - ch, 0) + _for_get_b(b_ptr - db->step, j, 0) + _for_get(a_ptr, j - ch, 0), 0); \
				a_ptr += a->step; \
				b_ptr += db->step; \
			}
			ccv_matrix_setter_getter(db->type, ccv_matrix_getter, a->type, for_block);
#undef for_block
			break;
	}
}

double ccv_sum(ccv_matrix_t* mat, int flag)
{
	ccv_dense_matrix_t* dmt = ccv_get_dense_matrix(mat);
	double sum = 0;
	unsigned char* m_ptr = dmt->data.u8;
	int i, j, ch = CCV_GET_CHANNEL(dmt->type);
#define for_block(_, _for_get) \
	switch (flag) \
	{ \
		case CCV_UNSIGNED: \
			for (i = 0; i < dmt->rows; i++) \
			{ \
				for (j = 0; j < dmt->cols * ch; j++) \
					sum += fabs(_for_get(m_ptr, j, 0)); \
				m_ptr += dmt->step; \
			} \
			break; \
		case CCV_SIGNED: \
		default: \
			for (i = 0; i < dmt->rows; i++) \
			{ \
				for (j = 0; j < dmt->cols * ch; j++) \
					sum += _for_get(m_ptr, j, 0); \
				m_ptr += dmt->step; \
			} \
	}
	ccv_matrix_getter(dmt->type, for_block);
#undef for_block
	return sum;
}

double ccv_variance(ccv_matrix_t* mat)
{
	ccv_dense_matrix_t* dmt = ccv_get_dense_matrix(mat);
	double mean = 0, variance = 0;
	unsigned char* m_ptr = dmt->data.u8;
	int i, j, ch = CCV_GET_CHANNEL(dmt->type);
#define for_block(_, _for_get) \
	for (i = 0; i < dmt->rows; i++) \
	{ \
		for (j = 0; j < dmt->cols * ch; j++) \
		{ \
			mean += _for_get(m_ptr, j, 0); \
			variance += _for_get(m_ptr, j, 0) * _for_get(m_ptr, j, 0); \
		} \
		m_ptr += dmt->step; \
	}
	ccv_matrix_getter(dmt->type, for_block);
#undef for_block
	mean = mean / (dmt->rows * dmt->cols * ch);
	variance = variance / (dmt->rows * dmt->cols * ch);
	return variance - mean * mean;
}

void ccv_multiply(ccv_matrix_t* a, ccv_matrix_t* b, ccv_matrix_t** c, int type)
{
	ccv_dense_matrix_t* da = ccv_get_dense_matrix(a);
	ccv_dense_matrix_t* db = ccv_get_dense_matrix(b);
	assert(da->rows == db->rows && da->cols == db->cols && CCV_GET_DATA_TYPE(da->type) == CCV_GET_DATA_TYPE(db->type) && CCV_GET_CHANNEL(da->type) == CCV_GET_CHANNEL(db->type));
	ccv_declare_derived_signature(sig, da->sig != 0 && db->sig != 0, ccv_sign_with_literal("ccv_multiply"), da->sig, db->sig, CCV_EOF_SIGN);
	int no_8u_type = (da->type & CCV_8U) ? CCV_32S : da->type;
	type = (type == 0) ? CCV_GET_DATA_TYPE(no_8u_type) | CCV_GET_CHANNEL(da->type) : CCV_GET_DATA_TYPE(type) | CCV_GET_CHANNEL(da->type);
	ccv_dense_matrix_t* dc = *c = ccv_dense_matrix_renew(*c, da->rows, da->cols, CCV_ALL_DATA_TYPE | CCV_GET_CHANNEL(da->type), type, sig);
	ccv_object_return_if_cached(, dc);
	int i, j, ch = CCV_GET_CHANNEL(da->type);
	unsigned char* aptr = da->data.u8;
	unsigned char* bptr = db->data.u8;
	unsigned char* cptr = dc->data.u8;
#define for_block(_for_get, _for_set) \
	for (i = 0; i < da->rows; i++) \
	{ \
		for (j = 0; j < da->cols * ch; j++) \
			_for_set(cptr, j, _for_get(aptr, j, 0) * _for_get(bptr, j, 0), 0); \
		aptr += da->step; \
		bptr += db->step; \
		cptr += dc->step; \
	}
	ccv_matrix_getter(da->type, ccv_matrix_setter, dc->type, for_block);
#undef for_block
}

void ccv_add(ccv_matrix_t* a, ccv_matrix_t* b, ccv_matrix_t** c, int type)
{
	ccv_dense_matrix_t* da = ccv_get_dense_matrix(a);
	ccv_dense_matrix_t* db = ccv_get_dense_matrix(b);
	assert(da->rows == db->rows && da->cols == db->cols && CCV_GET_CHANNEL(da->type) == CCV_GET_CHANNEL(db->type));
	ccv_declare_derived_signature(sig, da->sig != 0 && db->sig != 0, ccv_sign_with_literal("ccv_add"), da->sig, db->sig, CCV_EOF_SIGN);
	int no_8u_type = (da->type & CCV_8U) ? CCV_32S : da->type;
	type = (type == 0) ? CCV_GET_DATA_TYPE(no_8u_type) | CCV_GET_CHANNEL(da->type) : CCV_GET_DATA_TYPE(type) | CCV_GET_CHANNEL(da->type);
	ccv_dense_matrix_t* dc = *c = ccv_dense_matrix_renew(*c, da->rows, da->cols, CCV_ALL_DATA_TYPE | CCV_GET_CHANNEL(da->type), type, sig);
	ccv_object_return_if_cached(, dc);
	int i, j, ch = CCV_GET_CHANNEL(da->type);
	unsigned char* aptr = da->data.u8;
	unsigned char* bptr = db->data.u8;
	unsigned char* cptr = dc->data.u8;
#define for_block(_for_get_a, _for_get_b, _for_set) \
	for (i = 0; i < da->rows; i++) \
	{ \
		for (j = 0; j < da->cols * ch; j++) \
			_for_set(cptr, j, _for_get_a(aptr, j, 0) + _for_get_b(bptr, j, 0), 0); \
		aptr += da->step; \
		bptr += db->step; \
		cptr += dc->step; \
	}
	ccv_matrix_getter_a(da->type, ccv_matrix_getter_b, db->type, ccv_matrix_setter, dc->type, for_block);
#undef for_block
}

void ccv_subtract(ccv_matrix_t* a, ccv_matrix_t* b, ccv_matrix_t** c, int type)
{
	ccv_dense_matrix_t* da = ccv_get_dense_matrix(a);
	ccv_dense_matrix_t* db = ccv_get_dense_matrix(b);
	assert(da->rows == db->rows && da->cols == db->cols && CCV_GET_DATA_TYPE(da->type) == CCV_GET_DATA_TYPE(db->type) && CCV_GET_CHANNEL(da->type) == CCV_GET_CHANNEL(db->type));
	ccv_declare_derived_signature(sig, da->sig != 0 && db->sig != 0, ccv_sign_with_literal("ccv_subtract"), da->sig, db->sig, CCV_EOF_SIGN);
	int no_8u_type = (da->type & CCV_8U) ? CCV_32S : da->type;
	type = (type == 0) ? CCV_GET_DATA_TYPE(no_8u_type) | CCV_GET_CHANNEL(da->type) : CCV_GET_DATA_TYPE(type) | CCV_GET_CHANNEL(da->type);
	ccv_dense_matrix_t* dc = *c = ccv_dense_matrix_renew(*c, da->rows, da->cols, CCV_ALL_DATA_TYPE | CCV_GET_CHANNEL(da->type), type, sig);
	ccv_object_return_if_cached(, dc);
	int i, j, ch = CCV_GET_CHANNEL(da->type);
	unsigned char* aptr = da->data.u8;
	unsigned char* bptr = db->data.u8;
	unsigned char* cptr = dc->data.u8;
#define for_block(_for_get, _for_set) \
	for (i = 0; i < da->rows; i++) \
	{ \
		for (j = 0; j < da->cols * ch; j++) \
			_for_set(cptr, j, _for_get(aptr, j, 0) - _for_get(bptr, j, 0), 0); \
		aptr += da->step; \
		bptr += db->step; \
		cptr += dc->step; \
	}
	ccv_matrix_getter(da->type, ccv_matrix_setter, dc->type, for_block);
#undef for_block
}

void ccv_gemm(ccv_matrix_t* a, ccv_matrix_t* b, double alpha, ccv_matrix_t* c, double beta, int transpose, ccv_matrix_t** d, int type)
{
	ccv_dense_matrix_t* da = ccv_get_dense_matrix(a);
	ccv_dense_matrix_t* db = ccv_get_dense_matrix(b);
	ccv_dense_matrix_t* dc = (c == 0) ? 0 : ccv_get_dense_matrix(c);

	assert(CCV_GET_DATA_TYPE(da->type) == CCV_GET_DATA_TYPE(db->type) && CCV_GET_CHANNEL(da->type) == 1 && CCV_GET_CHANNEL(db->type) == 1 && ((transpose & CCV_A_TRANSPOSE) ? da->rows : da->cols) == ((transpose & CCV_B_TRANSPOSE) ? db->cols : db->rows));

	if (dc != 0)
		assert(CCV_GET_DATA_TYPE(dc->type) == CCV_GET_DATA_TYPE(da->type) && CCV_GET_CHANNEL(dc->type) == 1 && ((transpose & CCV_A_TRANSPOSE) ? da->cols : da->rows) == dc->rows && ((transpose & CCV_B_TRANSPOSE) ? db->rows : db->cols) == dc->cols);

	ccv_declare_derived_signature_case(sig, ccv_sign_with_format(20, "ccv_gemm(%d)", transpose), ccv_sign_if(dc == 0 && da->sig != 0 && db->sig != 0, da->sig, db->sig, CCV_EOF_SIGN), ccv_sign_if(dc != 0 && da->sig != 0 && db->sig != 0 && dc->sig != 0, da->sig, db->sig, dc->sig, CCV_EOF_SIGN));
	type = CCV_GET_DATA_TYPE(da->type) | CCV_GET_CHANNEL(da->type);
	ccv_dense_matrix_t* dd = *d = ccv_dense_matrix_renew(*d, (transpose & CCV_A_TRANSPOSE) ? da->cols : da->rows, (transpose & CCV_B_TRANSPOSE) ? db->rows : db->cols, type, type, sig);
	ccv_object_return_if_cached(, dd);

	if (dd != dc && dc != 0)
		memcpy(dd->data.u8, dc->data.u8, dc->step * dc->rows);
	else if (dc == 0) // clean up dd if dc is not provided
		memset(dd->data.u8, 0, dd->step * dd->rows);

#if (defined HAVE_CBLAS || defined HAVE_ACCELERATE_FRAMEWORK)
	switch (CCV_GET_DATA_TYPE(dd->type))
	{
		case CCV_32F:
			cblas_sgemm(CblasRowMajor, (transpose & CCV_A_TRANSPOSE) ? CblasTrans : CblasNoTrans, (transpose & CCV_B_TRANSPOSE) ? CblasTrans : CblasNoTrans, dd->rows, dd->cols, (transpose & CCV_A_TRANSPOSE) ? da->rows : da->cols, alpha, da->data.f32, da->cols, db->data.f32, db->cols, beta, dd->data.f32, dd->cols);
			break;
		case CCV_64F:
			cblas_dgemm(CblasRowMajor, (transpose & CCV_A_TRANSPOSE) ? CblasTrans : CblasNoTrans, (transpose & CCV_B_TRANSPOSE) ? CblasTrans : CblasNoTrans, dd->rows, dd->cols, (transpose & CCV_A_TRANSPOSE) ? da->rows : da->cols, alpha, da->data.f64, da->cols, db->data.f64, db->cols, beta, dd->data.f64, dd->cols);
			break;
	}
#else
	assert(0 && "You need a BLAS compatible library for this function, e.g. libatlas.");
#endif
}


#include <gsl/gsl_ntuple.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

struct data
{
  double x;
  double y;
  double z;
};

int
main (void)
{
  const gsl_rng_type * T;
  gsl_rng * r;

  struct data ntuple_row;
  int i;

  gsl_ntuple *ntuple
    = gsl_ntuple_create ("test.dat", &ntuple_row,
                         sizeof (ntuple_row));

  gsl_rng_env_setup ();

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  for (i = 0; i < 10000; i++)
    {
      ntuple_row.x = gsl_ran_ugaussian (r);
      ntuple_row.y = gsl_ran_ugaussian (r);
      ntuple_row.z = gsl_ran_ugaussian (r);

      gsl_ntuple_write (ntuple);
    }

  gsl_ntuple_close (ntuple);
  gsl_rng_free (r);

  return 0;
}


/* randist/mvgauss.c
 *
 * Copyright (C) 2016 Timothée Flutre, Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_statistics.h>

static int multivar_vcov (const double data[], size_t d, size_t tda, size_t n,
                          double vcov[], size_t tda2);

/* Generate a random vector from a multivariate Gaussian distribution using
 * the Cholesky decomposition of the variance-covariance matrix, following
 * "Computational Statistics" from Gentle (2009), section 7.4.
 *
 * mu      mean vector (dimension d)
 * L       matrix resulting from the Cholesky decomposition of
 *         variance-covariance matrix Sigma = L L^T (dimension d x d)
 * result  output vector (dimension d)
 */
int
gsl_ran_multivariate_gaussian (const gsl_rng * r,
                               const gsl_vector * mu,
                               const gsl_matrix * L,
                               gsl_vector * result)
{
  const size_t M = L->size1;
  const size_t N = L->size2;

  if (M != N)
    {
      GSL_ERROR("requires square matrix", GSL_ENOTSQR);
    }
  else if (mu->size != M)
    {
      GSL_ERROR("incompatible dimension of mean vector with variance-covariance matrix", GSL_EBADLEN);
    }
  else if (result->size != M)
    {
      GSL_ERROR("incompatible dimension of result vector", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      for (i = 0; i < M; ++i)
        gsl_vector_set(result, i, gsl_ran_ugaussian(r));

      gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, L, result);
      gsl_vector_add(result, mu);

      return GSL_SUCCESS;
    }
}

/* Compute the log of the probability density function at a given quantile
 * vector for a multivariate Gaussian distribution using the Cholesky
 * decomposition of the variance-covariance matrix.
 *
 * x       vector of quantiles (dimension d)
 * mu      mean vector (dimension d)
 * L       matrix resulting from the Cholesky decomposition of
 *         variance-covariance matrix Sigma = L L^T (dimension d x d)
 * result  output of the density (dimension 1)
 * work    vector used for intermediate computations (dimension d)
 */
int
gsl_ran_multivariate_gaussian_log_pdf (const gsl_vector * x,
                                       const gsl_vector * mu,
                                       const gsl_matrix * L,
                                       double * result,
                                       gsl_vector * work)
{
  const size_t M = L->size1;
  const size_t N = L->size2;

  if (M != N)
    {
      GSL_ERROR("requires square matrix", GSL_ENOTSQR);
    }
  else if (mu->size != M)
    {
      GSL_ERROR("incompatible dimension of mean vector with variance-covariance matrix", GSL_EBADLEN);
    }
  else if (x->size != M)
    {
      GSL_ERROR("incompatible dimension of quantile vector", GSL_EBADLEN);
    }
  else if (work->size != M)
    {
      GSL_ERROR("incompatible dimension of work vector", GSL_EBADLEN);
    }
  else
    {
      size_t i;
      double quadForm;        /* (x - mu)' Sigma^{-1} (x - mu) */
      double logSqrtDetSigma; /* log [ sqrt(|Sigma|) ] */

      /* compute: work = x - mu */
      for (i = 0; i < M; ++i)
        {
          double xi = gsl_vector_get(x, i);
          double mui = gsl_vector_get(mu, i);
          gsl_vector_set(work, i, xi - mui);
        }

      /* compute: work = L^{-1} * (x - mu) */
      gsl_blas_dtrsv(CblasLower, CblasNoTrans, CblasNonUnit, L, work);

      /* compute: quadForm = (x - mu)' Sigma^{-1} (x - mu) */
      gsl_blas_ddot(work, work, &quadForm);

      /* compute: log [ sqrt(|Sigma|) ] = sum_i log L_{ii} */
      logSqrtDetSigma = 0.0;
      for (i = 0; i < M; ++i)
        {
          double Lii = gsl_matrix_get(L, i, i);
          logSqrtDetSigma += log(Lii);
        }

      *result = -0.5*quadForm - logSqrtDetSigma - 0.5*M*log(2.0*M_PI);

      return GSL_SUCCESS;
    }
}

int
gsl_ran_multivariate_gaussian_pdf (const gsl_vector * x,
                                   const gsl_vector * mu,
                                   const gsl_matrix * L,
                                   double * result,
                                   gsl_vector * work)
{
  double logpdf;
  int status = gsl_ran_multivariate_gaussian_log_pdf(x, mu, L, &logpdf, work);

  if (status == GSL_SUCCESS)
    *result = exp(logpdf);

  return status;
}

/* Compute the maximum-likelihood estimate of the mean vector of samples
 * from a multivariate Gaussian distribution.
 *
 * Example from R (GPL): http://www.r-project.org/
 * (samples <- matrix(c(4.348817, 2.995049, -3.793431, 4.711934, 1.190864, -1.357363), nrow=3, ncol=2))
 * colMeans(samples) # 1.183478 1.515145
 */
int
gsl_ran_multivariate_gaussian_mean (const gsl_matrix * X, gsl_vector * mu_hat)
{
  const size_t M = X->size1;
  const size_t N = X->size2;

  if (N != mu_hat->size)
    {
      GSL_ERROR("mu_hat vector has wrong size", GSL_EBADLEN);
    }
  else
    {
      size_t j;

      for (j = 0; j < N; ++j)
        {
          gsl_vector_const_view c = gsl_matrix_const_column(X, j);
          double mean = gsl_stats_mean(c.vector.data, c.vector.stride, M);
          gsl_vector_set(mu_hat, j, mean);
        }

      return GSL_SUCCESS;
    }
}

/* Compute the maximum-likelihood estimate of the variance-covariance matrix
 * of samples from a multivariate Gaussian distribution.
 */
int
gsl_ran_multivariate_gaussian_vcov (const gsl_matrix * X, gsl_matrix * sigma_hat)
{
  const size_t M = X->size1;
  const size_t N = X->size2;

  if (sigma_hat->size1 != sigma_hat->size2)
    {
      GSL_ERROR("sigma_hat must be a square matrix", GSL_ENOTSQR);
    }
  else if (N != sigma_hat->size1)
    {
      GSL_ERROR("sigma_hat does not match X matrix dimensions", GSL_EBADLEN);
    }
  else
    {
      return multivar_vcov (X->data, N, X->tda, M, sigma_hat->data, sigma_hat->tda);
    }
}

/* Example from R (GPL): http://www.r-project.org/
 * (samples <- matrix(c(4.348817, 2.995049, -3.793431, 4.711934, 1.190864, -1.357363), nrow=3, ncol=2))
 * cov(samples) # 19.03539 11.91384 \n 11.91384  9.28796
 */
static int
multivar_vcov (const double data[], size_t d, size_t tda, size_t n,
               double vcov[], size_t tda2)
{
  size_t j1 = 0, j2 = 0;

  for (j1 = 0; j1 < d; ++j1)
    {
      vcov[j1 * tda2 + j1] = gsl_stats_variance(&(data[j1]), tda, n);
      for (j2 = j1 + 1; j2 < d; ++j2)
        {
          vcov[j1 * tda2 + j2] = gsl_stats_covariance(&(data[j1]), tda,
                                                      &(data[j2]), tda, n);
          vcov[j2 * tda2 + j1] = vcov[j1 * tda2 + j2];
        }
      }

  return GSL_SUCCESS;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

void read_matrix(int** index, int** matrix, double scaling, int N_kw, char* input_fileName)
{
    FILE *fp = fopen(input_fileName, "r");
    fscanf(fp, "%*[^\n]\n");

    for (int ii = 0; ii < N_kw; ii++)
        fscanf(fp, "%d,", &((*index)[ii]));
    fscanf(fp, "%*[^\n]\n");

    int tmp;
    for (int ii = 0; ii < N_kw; ii++)
    {
        for (int jj = 0; jj < N_kw; jj++)
        {
            fscanf(fp, "%d,", &tmp);
            (*matrix)[ii*N_kw + jj] = (int) (scaling * tmp);
        }
        fscanf(fp, "%*[^\n]\n");
    }
    fclose(fp);
}

void pad_matrix(int** matrix_padded, int** matrix, int N_kw, int N_doc, int freq_max)
{
    // Initialising RNG
    const gsl_rng_type * T;
    gsl_rng * r;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc(T);

    // perform padding on the keywords
    int ii, jj;
    #pragma omp parallel for private(ii)
    for (int ii = 0; ii < N_kw; ii++)
        (*matrix_padded)[ii*N_kw + ii] = 2*freq_max;

    // perform padding
    #pragma omp parallel for private(ii, jj)
    for (ii = 0; ii < N_kw; ii++)
    {
        for (jj = 0; jj < N_kw; jj++)
        {
            if (ii > jj)
            {
                int n1 = 2*freq_max - (*matrix)[ii*N_kw + ii];
                int n2 = 2*freq_max - (*matrix)[jj*N_kw + jj];
                int x1 = gsl_ran_hypergeometric(r, n1, 2*N_doc-n1, (*matrix_padded)[jj*N_kw + jj]);
                int x2 = gsl_ran_hypergeometric(r, n2, 2*N_doc-n2, (*matrix_padded)[ii*N_kw + ii]);

                (*matrix_padded)[ii*N_kw + jj] = (*matrix)[ii*N_kw + jj] + x1 + x2;
                (*matrix_padded)[jj*N_kw + ii] = (*matrix_padded)[ii*N_kw + jj];
            }
        }
    }
    gsl_rng_free(r);
}

void observe_matrix(gsl_matrix* matrix_obs, int** matrix_padded, int N_kw)
{
    // perform observed count generation
    for (int ii = 0; ii < N_kw; ii++)
        for (int jj = 0; jj < N_kw; jj++)
            gsl_matrix_set(matrix_obs, ii, jj, (double) ((*matrix_padded)[ii*N_kw + jj]));
}

void permutation_generation(int* idx1, int* idx2, int** permutation_tmp, int** permutation, int** permutation_inv, int N_kw, int N_obs)
{
    *idx1 = rand() % N_obs;
    *idx2 = -1;
    int idx_old = (*permutation)[*idx1];
    int idx_new = rand() % N_kw;

    (*permutation_tmp)[*idx1] = idx_new;

    if ((*permutation_inv)[idx_new] >= 0)
    {
        *idx2 = (*permutation_inv)[idx_new];
        (*permutation_tmp)[*idx2] = idx_old;
    }
}


#include <math.h>
#include <gsl/gsl_ntuple.h>
#include <gsl/gsl_histogram.h>

struct data
{
  double x;
  double y;
  double z;
};

int sel_func (void *ntuple_data, void *params);
double val_func (void *ntuple_data, void *params);

int
main (void)
{
  struct data ntuple_row;

  gsl_ntuple *ntuple
    = gsl_ntuple_open ("test.dat", &ntuple_row,
                       sizeof (ntuple_row));
  double lower = 1.5;

  gsl_ntuple_select_fn S;
  gsl_ntuple_value_fn V;

  gsl_histogram *h = gsl_histogram_alloc (100);
  gsl_histogram_set_ranges_uniform(h, 0.0, 10.0);

  S.function = &sel_func;
  S.params = &lower;

  V.function = &val_func;
  V.params = 0;

  gsl_ntuple_project (h, ntuple, &V, &S);
  gsl_histogram_fprintf (stdout, h, "%f", "%f");
  gsl_histogram_free (h);
  gsl_ntuple_close (ntuple);

  return 0;
}

int
sel_func (void *ntuple_data, void *params)
{
  struct data * data = (struct data *) ntuple_data;
  double x, y, z, E2, scale;
  scale = *(double *) params;

  x = data->x;
  y = data->y;
  z = data->z;

  E2 = x * x + y * y + z * z;

  return E2 > scale;
}

double
val_func (void *ntuple_data, void *params)
{
  struct data * data = (struct data *) ntuple_data;
  double x, y, z;

  x = data->x;
  y = data->y;
  z = data->z;

  return x * x + y * y + z * z;
}


#pragma once
#include <thrust/host_vector.h>
#include <sstream>
#include "IsingLattice2D.h"
#include "class_mc_io.h"
#include "Matrix.h"
#include "MemTimeTester.h"
extern "C" {
#include "random.h"
}
#include <gsl/gsl_sf_zeta.h>
#include <cmath>
#include "obs_calc_fast.cuh"

double real_trigamma_cmplxarg(double x, double y, int m, int l);

class GeneralLRW{
	//This is a more lightweight version of the long range wolff class, designed to be
	//more general - hopefully to admit the use of user-defined "test_spins" functions
	//so that it can be used for many different MC simulations.  It does not hold the
	//lattice in order to keep the lattice easily available for GPU computations
	std::vector<std::vector<double>> interactions;//interaction matrix
	std::vector<std::vector<double>> interaction_sum;//values from interaction sum integral
	std::vector<double> site_sums;//absolute sum of interactions at each site
	double mag;//current magnetization
	double h; //applied field
	std::vector<spin> buffer;//holds the indices of spins to check
	std::vector<std::vector<int>> cluster;//0 at indices that are not in cluster, 1 at indices that are
	int cluster_size;//size of the cluster during a given MC step
	int cluster_mag;//total magnetization of the cluster
	void set_spin_boson_model_wc_exp(class_mc_params);//version for finite cutoff frequency with exponential tail
	void set_spin_boson_model_wc_hard(class_mc_params);//version for finite cutoff frequency with sharp cutoff
	void set_spin_boson_model(class_mc_params);//create interaction matrix for the spin boson model with infinite cutoff frequency
	void set_interaction_sum();
	void set_site_sums();
	MemTimeTester timer;

	inline double kernel_integrand(double alpha, int Nt, double rbv, double taub, double u){
		//evaluate the integrand of the kernel of the spin-boson effective interactions
		return 2.0 * alpha / Nt / Nt * u * cos(rbv*u)*(exp(u*(-taub)) + exp(u*(taub - 1.0)))/(1 - exp(-u));
	}

	double simpson_3_8(double alpha, int Nt, double bomc, double rbv, double taub, int N_points){
		//perform an integration of the spin-boson kernel with a hard, finite cutoff bomc.  Evaluated at
		//site distance rbv = r/(beta*v), taub = tau/beta, with number of time slices Nt and number of
		//integration slices N_points (this method will multiply N_points by 3 and use simpson's 3/8 rule)
		double du = (bomc)/(3.0*N_points);
		double result = 3.0*du*(4.0*alpha / Nt / Nt + kernel_integrand(alpha, Nt, rbv, taub, bomc))/8.0;
		for (int i = 1; i < 3*N_points; ++i){
			if(3*(i/3) == i){
				result += 0.75*du*kernel_integrand(alpha, Nt, rbv, taub, i*du);
			}
			else{
				result += 9.0*du*kernel_integrand(alpha, Nt, rbv, taub, i*du)/8.0;
			}
		}
		return result;
	}

public:
	GeneralLRW(class_mc_params);

	void print_timers(){ timer.print_timers(); }

	void step(IsingLattice2D& lat);

    bool step_one_site(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref);

	bool step_one_site_prealloc(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref,
			cufftHandle *full_forward_plan, cufftHandle *full_backward_plan, cufftHandle *onesite_forward_plan, cufftHandle *onesite_backward_plan,
			cudaError cuda_status, cufftDoubleReal *full_state_rs, cufftDoubleComplex *full_state_ft, cufftDoubleReal *onesite_state_rs, cufftDoubleComplex *onesite_state_ft);

	bool step_one_site_prealloc_lowmem(IsingLattice2D& lat, double *prev_action, thrust::host_vector<double>& corr_ref,
			thrust::host_vector<double>& ,thrust::host_vector<double>& ,thrust::host_vector<double>& ,thrust::host_vector<double>& ,
			thrust::host_vector<double>& ,thrust::host_vector<double>& ,
			cufftHandle *full_forward_plan, cufftHandle *full_backward_plan, cufftHandle *onesite_forward_plan, cufftHandle *onesite_backward_plan,
			cudaError cuda_status, cufftDoubleReal *full_state_rs, cufftDoubleComplex *full_state_ft, cufftDoubleReal *onesite_state_rs, cufftDoubleComplex *onesite_state_ft);

	bool metropolis_step(IsingLattice2D& lat, double *action);

	bool fast_metropolis_step(IsingLattice2D& lat, double *action);

	void test_spins(spin, IsingLattice2D& lat);

    void test_spins_one_site(spin, IsingLattice2D& lat);

	void set_mag(IsingLattice2D& lat){
		mag = 0;
		for (int i = 0; i < lat.get_Lx(); ++i){
			for (int j = 0; j < lat.get_Ly(); ++j){
				mag += lat.get_spin(i, j);
			}
		}
		mag = ((double) mag) / lat.get_N();
	}

	double get_mag(){
		return mag;
	}

	double get_cluster_size() { return (double)cluster_size; }

    void get_thrust_interactions(thrust::host_vector<double>& result){
		if(result.size() != interactions.size()*interactions[0].size()){
			result.resize(interactions.size()*interactions[0].size());
		}
		for (int i = 0; i < interactions.size(); ++i){
			for (int j = 0; j < interactions[i].size(); ++j){
				result[i*interactions[i].size() + j] = interactions[i][j];
			}
		}
    }

	std::vector<double> get_interaction_vector(){
		std::vector<double> result(interactions.size()*interactions[0].size());
		int Ly = interactions[0].size();
		for (int x = 0; x < interactions.size(); ++x){
			for (int y = 0; y < Ly; ++y){
				result[x*Ly + y] = interactions[x][y];
			}
		}
		return result;
	}

	std::vector<std::vector<double>> get_interactions(){
		return interactions;
	}

    double calc_mqt0_abs(IsingLattice2D&, double);

    double calc_mqt0_2_abs(IsingLattice2D&, double);

    double calc_mqt0_4_abs(IsingLattice2D&, double);

    double calc_mqt0_gpu(thrust::host_vector<double>&, double, int);

    double calc_locw1(IsingLattice2D&, double);

	double calc_sx(IsingLattice2D&);

	double calc_space_kinks(IsingLattice2D& lat);

	double calc_sz_stagger(IsingLattice2D&);

	double calc_xmag(IsingLattice2D&);

	double calc_xmag2(IsingLattice2D&);

	double calc_xmag4(IsingLattice2D&);

    double calc_mag(IsingLattice2D&);

	double calc_loc(IsingLattice2D&);

	double calc_loc2(IsingLattice2D&);

	double calc_loc4(IsingLattice2D&);

	double calc_s1s2(IsingLattice2D&);

	double calc_action_slow(IsingLattice2D&);

	double calc_point_action_slow(IsingLattice2D&,int, int);

	double calc_point_action_fast(IsingLattice2D&, int, int);

	void print_site_sums(){
		std::cout << "Site sums:\n" << vec2str(site_sums) << "\n";
	}

	void print_interactions() {
		std::stringstream outstring;
		outstring << "Interactions:\n";
		for (int i = 0; i < interactions.size(); ++i){
			outstring << vec2str(interactions[i]) << "\n";
		}
		std::cout << outstring.str() << "\n";
	}

	void print_interaction_sum() {
		std::stringstream outstring;
		outstring << "Interaction sum:\n";
		for (int i = 0; i < interaction_sum.size(); ++i){
			outstring << vec2str(interaction_sum[i]) << "\n";
		}
		std::cout << outstring.str() << "\n";
	}

	std::string get_int_string() {
		std::stringstream ss;
		ss << "Interactions:\n" ;
		for (int i = 0; i < interactions.size(); ++i){
			ss << vec2str(interactions[i]) << "\n";
		}
		return ss.str();
	}

};


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <malloc.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>

#define L 1 //1 2 3 4
#define LL 2 //2^(1 2 3 4)
#define LLL 4 //2^(2 3 4 5)

#define N 8

#define M 1 //# of trials
#define R 1 //the number of different initial conditions
#define S 100 //the number of different rules
#define T1 100 //transient
#define T2 1000 //time window used
int **mat;
int component[N],cnum;

void findcomponent(int node)
{
	int i;

	for (i=0;i<N;i++){
		if (mat[node][i]==1 && component[i]==0){
			component[i]=cnum;
			findcomponent(i);
		}
	}
}

double dabs(double x)
{
	if (x<0.0)return -x;
	else return x;
}

int main()
{
	FILE *fp,*fq,*fe,*fg,*fh,*fc,*fa;

	int i,j,k,l,t,m,r,s;
	int flag,flag1,flag2,tmp,tmp1,tmp2,tmp3,tmp4,ttmp;
	double dtmp;

	int x[N],xx[N][L+1],y[N];
	int in[N];
	int **nb,**rule;

	int check[N],sum,check_sum;

	double p1[N][LL],p2[N][LLL];
	double ****p3,****p4;
	int num1,num2,num3;

	double indeg[N],outdeg[N];

	double p=0.5;

	double q;
	int kav=2;
	int **adj,**adjflag;

	double *llap;
	double **lap,**edge,**grad,**circ;
	double div[N],sol[N];
	double diag[N];
	double **uu,**vv,**lap_inv;

	double el2_av,gl2_av,cl2_av,cratio_av;
	double el2,gl2,cl2,cratio;

	double *curl,*vp;
	int trinum;
	int **tri;

	double *ccurcur;
	double **curcur;
	double *diag2;
	double **uu2,**vv2,**curcur_inv;

	double **lcirc,**gcirc;

	double hl2_av,lcl2_av,gcratio_av,lcratio_av;
	double hl2,lcl2,gcratio,lcratio;

	double et,gt,ht,ct;
	double et_av,gt_av,ht_av,ct_av;

	char filename[100];

	gsl_matrix *v_gsl=gsl_matrix_alloc(N,N);
	gsl_vector *s_gsl=gsl_vector_alloc(N);
	gsl_vector *work=gsl_vector_alloc(N);
	gsl_matrix_view lap_gsl;

	gsl_matrix_view cur_gsl;

	const gsl_rng_type * TYPE;
	gsl_rng * ran;

	gsl_rng_env_setup();
	TYPE=gsl_rng_default;
	ran=gsl_rng_alloc(TYPE);
	gsl_rng_set(ran,2);

	adj=malloc(sizeof(int*)*N);
	adjflag=malloc(sizeof(int*)*N);

	mat=malloc(sizeof(int*)*N);
	lap=malloc(sizeof(double*)*N);
	edge=malloc(sizeof(double*)*N);
	grad=malloc(sizeof(double*)*N);
	circ=malloc(sizeof(double*)*N);
	lcirc=malloc(sizeof(double*)*N);
	gcirc=malloc(sizeof(double*)*N);

	uu=malloc(sizeof(double*)*N);
	vv=malloc(sizeof(double*)*N);
	lap_inv=malloc(sizeof(double*)*N);

	llap=malloc(sizeof(double)*(N*N));

	p3=malloc(sizeof(double***)*N);
	p4=malloc(sizeof(double***)*N);

	for (i=0;i<N;i++){
		adj[i]=malloc(sizeof(int)*N);
		adjflag[i]=malloc(sizeof(int)*N);

		mat[i]=malloc(sizeof(int)*N);

		lap[i]=malloc(sizeof(double)*N);
		edge[i]=malloc(sizeof(double)*N);
		grad[i]=malloc(sizeof(double)*N);
		circ[i]=malloc(sizeof(double)*N);
		lcirc[i]=malloc(sizeof(double)*N);
		gcirc[i]=malloc(sizeof(double)*N);

		uu[i]=malloc(sizeof(double)*N);
		vv[i]=malloc(sizeof(double)*N);
		lap_inv[i]=malloc(sizeof(double)*N);

		p3[i]=malloc(sizeof(double**)*N);
		p4[i]=malloc(sizeof(double**)*N);
		for (j=0;j<N;j++){
			p3[i][j]=malloc(sizeof(double*)*LL);
			p4[i][j]=malloc(sizeof(double*)*LLL);
			for (k=0;k<LL;k++){
				p3[i][j][k]=malloc(sizeof(double)*LL);
			}
			for (k=0;k<LLL;k++){
				p4[i][j][k]=malloc(sizeof(double)*LL);
			}
		}
	}

	sprintf(filename,"rtn_hodge_sw_av_n%d_k%d.txt",N,kav);
	fp=fopen(filename,"w");

	sprintf(filename,"rtn_hodge_sw_trial_n%d_k%d.txt",N,kav);
	fq=fopen(filename,"w");

	for (q=0.1;q<0.11;q=q+0.1){

		el2_av=0.0; gl2_av=0.0; cl2_av=0.0;
		cratio_av=0.0;

		hl2_av=0.0; lcl2_av=0.0;
		gcratio_av=0.0; lcratio_av=0.0;

		et_av=0.0; gt_av=0.0; ht_av=0.0; ct_av=0.0;

		for (m=0;m<M;m++){

			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					adj[i][j]=0;
					adjflag[i][j]=0;
				}
			}
			for (i=0;i<N;i++){
				for (k=-kav;k<kav+1;k++){
					if (k!=0){
						adj[(i+k+N)%N][i]=1;
						adj[i][(i+k+N)%N]=1;
					}
				}
			}

			for (i=0;i<N;i++){
				for (k=-kav;k<kav+1;k++){
					if (k!=0 && gsl_rng_uniform(ran)<q){
						adjflag[(i+k+N)%N][i]=1;
						adjflag[i][(i+k+N)%N]=1;
					}
				}
			}

			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					if (adjflag[i][j]==1){
						if (gsl_rng_uniform(ran)<0.5){
							while (1){
								tmp=(int)((double)N*gsl_rng_uniform(ran));

								if (j!=tmp && adj[tmp][j]==0){
									adj[i][j]=0;
									adj[j][i]=0;
									adj[tmp][j]=1;
									adj[j][tmp]=1;
									break;
								}
							}
						} else{
							while (1){
								tmp=(int)((double)N*gsl_rng_uniform(ran));

								if (i!=tmp && adj[i][tmp]==0){
									adj[i][j]=0;
									adj[j][i]=0;
									adj[tmp][i]=1;
									adj[i][tmp]=1;
									break;
								}
							}
						}
					}
				}
			}

			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					if (adj[i][j]==1){
						if (gsl_rng_uniform(ran)<0.5)adj[i][j]=0;
						else adj[j][i]=0;
					}
				}
			}

			for (i=0;i<N;i++){
				in[i]=0;
				for (j=0;j<N;j++){
					in[i]+=adj[j][i];
				}
			}

			nb=malloc(sizeof(int*)*N);
			rule=malloc(sizeof(int*)*N);
			for (i=0;i<N;i++){
				nb[i]=malloc(sizeof(int)*in[i]);
				rule[i]=malloc(sizeof(int)*in[i]);
			}

			//in-neighbors
			for (i=0;i<N;i++){
				k=0;
				for (j=0;j<N;j++){
					if (adj[j][i]==1){
						nb[i][k]=j;
						k++;
					}
				}
			}
			//

			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					edge[i][j]=0.0;
				}
			}

			for (s=0;s<S;s++){
				//random generation of rules
				for (i=0;i<N;i++){
					for (j=0;j<in[i];j++){
					//	rule[i][j]=gsl_ran_gaussian(ran,sigma);
						if (gsl_rng_uniform(ran)<0.5)rule[i][j]=1;
						else rule[i][j]=-1;
					}
				}

				for (k=0;k<N;k++){
					for (i=0;i<LL;i++){
						p1[k][i]=0.0;
					}

					for (i=0;i<LLL;i++){
						p2[k][i]=0.0;
					}

					for (l=0;l<in[k];l++){
						for (i=0;i<LL;i++){
							for (j=0;j<LL;j++){
								p3[k][nb[k][l]][i][j]=0.0;
							}
						}
						for (i=0;i<LLL;i++){
							for (j=0;j<LL;j++){
								p4[k][nb[k][l]][i][j]=0.0;
							}
						}
					}
				}

				for (r=0;r<R;r++){

					//initial state
					for (i=0;i<N;i++){
						if (gsl_rng_uniform(ran)<p)x[i]=1;
						else x[i]=-1;
					}

					//transient
					for (t=0;t<T1;t++){

						for (i=0;i<N;i++){
							tmp=0;
							for (j=0;j<in[i];j++){
								tmp+=rule[i][j]*x[nb[i][j]];
							}
							if (tmp<0)y[i]=-1;
							else y[i]=1;
						}

						for (i=0;i<N;i++){
							x[i]=y[i];
						}
					}

					for (i=0;i<N;i++){
						for (j=0;j<L;j++){
							xx[i][j]=0;
						}
						xx[i][L]=x[i];
					}

					//time window used to calculation
					for (t=0;t<T2+L-1;t++){
						for (i=0;i<N;i++){
							tmp=0;
							for (j=0;j<in[i];j++){
								tmp+=rule[i][j]*x[nb[i][j]];
							}
							if (tmp<0)y[i]=-1;
							else y[i]=1;

							for (j=0;j<L;j++){
								xx[i][j]=xx[i][j+1];
							}
							xx[i][L]=y[i];
						}

						if (t>=L-1){
							for (i=0;i<N;i++){
								num1=0; num2=0;
								for (k=0;k<L;k++){
									num1+=((xx[i][k]+1)/2)*(int)pow(2.0,k);
									num2+=((xx[i][k]+1)/2)*(int)pow(2.0,k);
								}
								num2+=((xx[i][L]+1)/2)*(int)pow(2.0,L);

								p1[i][num1]+=1.0;
								p2[i][num2]+=1.0;

								for (j=0;j<in[i];j++){
									num3=0;
									for (k=0;k<L;k++){
										num3+=((xx[nb[i][j]][k]+1)/2)*(int)pow(2.0,k);
									}

									p3[i][nb[i][j]][num1][num3]+=1.0;
									p4[i][nb[i][j]][num2][num3]+=1.0;
								}
							}
						}

						for (i=0;i<N;i++){
							x[i]=y[i];
						}
					}
				}//end of r-loop

				for (k=0;k<N;k++){
					for (i=0;i<LL;i++){
						p1[k][i]=p1[k][i]/(double)(T2*R);
					}

					for (i=0;i<LLL;i++){
						p2[k][i]=p2[k][i]/(double)(T2*R);
					}

					for (l=0;l<in[k];l++){
						for (i=0;i<LL;i++){
							for (j=0;j<LL;j++){
								p3[k][nb[k][l]][i][j]=p3[k][nb[k][l]][i][j]/(double)(T2*R);
							}
						}
						for (i=0;i<LLL;i++){
							for (j=0;j<LL;j++){
								p4[k][nb[k][l]][i][j]=p4[k][nb[k][l]][i][j]/(double)(T2*R);
							}
						}
					}
				}

				//construction of edge flow
				for (i=0;i<N;i++){
					for (j=0;j<in[i];j++){
						for (num2=0;num2<LLL;num2++){
							for (num3=0;num3<LL;num3++){
								if (p4[i][nb[i][j]][num2][num3]>0.0)edge[nb[i][j]][i]+=p4[i][nb[i][j]][num2][num3]*(log(p4[i][nb[i][j]][num2][num3])/log(2.0) + log(p1[i][num2%LL])/log(2.0) - log(p2[i][num2])/log(2.0) - log(p3[i][nb[i][j]][num2%LL][num3])/log(2.0));
							}
						}
					}
				}
			}//end of s-loop

			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					edge[i][j]=edge[i][j]/(double)S;
				}
			}

			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					dtmp=edge[i][j]-edge[j][i];
					edge[i][j]=dtmp;
					edge[j][i]=-dtmp;
				}
			}

			//finding the number of components
			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					mat[i][j]=0;
				}
			}

			for (i=0;i<N;i++){
				for (j=0;j<in[i];j++){
					if (i!=nb[i][j]){
						mat[nb[i][j]][i]=1;
						mat[i][nb[i][j]]=1;
					}
				}
			}

			tmp=0;
			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					if (mat[i][j]==1)tmp++;
				}
			}
			et=(double)tmp;

			for (i=0;i<N;i++){
				component[i]=0;
			}

			cnum=1; component[0]=cnum;
			findcomponent(0);
			for (i=1;i<N;i++){
				if (component[i]==0){
					cnum++;
					component[i]=cnum;
					findcomponent(i);
				}
			}

			gt=(double)(N-cnum);

			//graph Laplacian
			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					lap[i][j]=0.0;
				}
			}

			for (i=0;i<N;i++){
				lap[i][i]=0.0;
				for (j=0;j<N;j++){
					lap[i][i]+=(double)mat[i][j];
				}
			}

			for (i=0;i<N;i++){
				for (j=0;j<in[i];j++){
					if (i!=nb[i][j]){
						lap[nb[i][j]][i]=-1.0;
						lap[i][nb[i][j]]=-1.0;
					}
				}
			}

			//generalized inverse of lap
			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					llap[i*N+j]=lap[i][j];
				}
			}

			lap_gsl=gsl_matrix_view_array(llap,N,N);
			gsl_linalg_SV_decomp(&lap_gsl.matrix,v_gsl,s_gsl,work);

			for (i=0;i<N-cnum;i++){
				diag[i]=1.0/gsl_vector_get(s_gsl,i);
			}
			for (i=N-cnum;i<N;i++){
				diag[i]=0.0;
			}

			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					uu[i][j]=gsl_matrix_get(&lap_gsl.matrix,i,j);
					vv[i][j]=gsl_matrix_get(v_gsl,i,j);
				}
			}

			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					lap_inv[i][j]=0.0;
					for (k=0;k<N;k++){
						lap_inv[i][j]+=vv[i][k]*diag[k]*uu[j][k];
					}
				}
			}
			//

			//divergence of edge flow
			for (i=0;i<N;i++){
				div[i]=0.0;
				for (j=0;j<N;j++){
					div[i]+=edge[i][j];
				}
			}

			//solution (negative potential)
			for (i=0;i<N;i++){
				sol[i]=0.0;
				for (j=0;j<N;j++){
					sol[i]-=lap_inv[i][j]*div[j];
				}
			}

			//gradient flow
			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					grad[i][j]=sol[j]-sol[i];
				}
			}

			//circular flow
			for (i=0;i<N;i++){
				for (j=0;j<N;j++){
					circ[i][j]=edge[i][j]-grad[i][j];
				}
			}

			el2=0.0; gl2=0.0; cl2=0.0;
			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					if (lap[i][j]<0.0){
						el2+=edge[i][j]*edge[i][j];
						gl2+=grad[i][j]*grad[i][j];
						cl2+=circ[i][j]*circ[i][j];
					}
				}
			}

			if (el2>0.0)cratio=cl2/el2;
			else cratio=0.0;

			el2_av+=el2/(double)M;
			gl2_av+=gl2/(double)M;
			cl2_av+=cl2/(double)M;
			cratio_av+=cratio/(double)M;

			//decomposing circular flow into harmonic and curl flows
			trinum=0;
			for (i=0;i<N;i++){
				for (j=i+1;j<N;j++){
					for (k=j+1;k<N;k++){
						if (mat[i][j]*mat[j][k]*mat[k][i]!=0)trinum++;
					}
				}
			}

			if (trinum>0){
				curl=malloc(sizeof(double)*trinum);
				tri=malloc(sizeof(int*)*trinum);
				for (i=0;i<trinum;i++){
					tri[i]=malloc(sizeof(int)*3);
				}

				l=0;
				for (i=0;i<N;i++){
					for (j=i+1;j<N;j++){
						for (k=j+1;k<N;k++){
							if (mat[i][j]*mat[j][k]*mat[k][i]!=0){
								tri[l][0]=i; tri[l][1]=j; tri[l][2]=k;
								curl[l]=circ[i][j]+circ[j][k]-circ[i][k];
								l++;
							}
						}
					}
				}

				ccurcur=malloc(sizeof(double)*trinum*trinum);
				diag2=malloc(sizeof(double)*trinum);
				vp=malloc(sizeof(double)*trinum);

				curcur=malloc(sizeof(double*)*trinum);
				uu2=malloc(sizeof(double*)*trinum);
				vv2=malloc(sizeof(double*)*trinum);
				curcur_inv=malloc(sizeof(double*)*trinum);
				for (i=0;i<trinum;i++){
					curcur[i]=malloc(sizeof(double)*trinum);
					uu2[i]=malloc(sizeof(double)*trinum);
					vv2[i]=malloc(sizeof(double)*trinum);
					curcur_inv[i]=malloc(sizeof(double)*trinum);
				}

				for (i=0;i<trinum;i++){
					for (j=0;j<trinum;j++){
						curcur[i][j]=0.0;
						if (tri[i][0]==tri[j][0] && tri[i][1]==tri[j][1])curcur[i][j]+=1.0;
						if (tri[i][0]==tri[j][0] && tri[i][1]==tri[j][2])curcur[i][j]-=1.0;
						if (tri[i][0]==tri[j][1] && tri[i][1]==tri[j][2])curcur[i][j]+=1.0;
						if (tri[i][1]==tri[j][0] && tri[i][2]==tri[j][1])curcur[i][j]+=1.0;
						if (tri[i][1]==tri[j][0] && tri[i][2]==tri[j][2])curcur[i][j]-=1.0;
						if (tri[i][1]==tri[j][1] && tri[i][2]==tri[j][2])curcur[i][j]+=1.0;
						if (tri[i][0]==tri[j][0] && tri[i][2]==tri[j][1])curcur[i][j]-=1.0;
						if (tri[i][0]==tri[j][0] && tri[i][2]==tri[j][2])curcur[i][j]+=1.0;
						if (tri[i][0]==tri[j][1] && tri[i][2]==tri[j][2])curcur[i][j]-=1.0;
					}
				}

				gsl_matrix *v_gsl2=gsl_matrix_alloc(trinum,trinum);
				gsl_vector *s_gsl2=gsl_vector_alloc(trinum);
				gsl_vector *work2=gsl_vector_alloc(trinum);

				//generalized inverse of curcur
				for (i=0;i<trinum;i++){
					for (j=0;j<trinum;j++){
						ccurcur[i*trinum+j]=curcur[i][j];
					}
				}

				cur_gsl=gsl_matrix_view_array(ccurcur,trinum,trinum);
				gsl_linalg_SV_decomp(&cur_gsl.matrix,v_gsl2,s_gsl2,work2);

				tmp=0;
				for (i=0;i<trinum;i++){
					dtmp=gsl_vector_get(s_gsl2,i);
					if (dtmp>1e-8)diag2[i]=1.0/dtmp;
					else {
						diag2[i]=0.0;
						tmp++;
					}
				}

				ct=(double)(trinum-tmp);
				ht=et-gt-ct;

				for (i=0;i<trinum;i++){
					for (j=0;j<trinum;j++){
						uu2[i][j]=gsl_matrix_get(&cur_gsl.matrix,i,j);
						vv2[i][j]=gsl_matrix_get(v_gsl2,i,j);
					}
				}

				for (i=0;i<trinum;i++){
					for (j=0;j<trinum;j++){
						curcur_inv[i][j]=0.0;
						for (k=0;k<trinum;k++){
							curcur_inv[i][j]+=vv2[i][k]*diag2[k]*uu2[j][k];
						}
					}
				}

				for (i=0;i<trinum;i++){
					vp[i]=0.0;
					for (j=0;j<trinum;j++){
						vp[i]+=curcur_inv[i][j]*curl[j];
					}
				}

				for (i=0;i<N;i++){
					lcirc[i][i]=0.0;
					for (j=i+1;j<N;j++){
						lcirc[i][j]=0.0;
						if (mat[i][j]!=0){
							for (k=0;k<trinum;k++){
								if (tri[k][0]==i && tri[k][1]==j)lcirc[i][j]+=vp[k];
								if (tri[k][0]==i && tri[k][2]==j)lcirc[i][j]-=vp[k];
								if (tri[k][1]==i && tri[k][2]==j)lcirc[i][j]+=vp[k];
							}
						}
					}
				}

				for (i=0;i<N;i++){
					for (j=0;j<i;j++){
						lcirc[i][j]=-lcirc[j][i];
					}
				}

				for (i=0;i<N;i++){
					for (j=0;j<N;j++){
						gcirc[i][j]=circ[i][j]-lcirc[i][j];
					}
				}

				hl2=0.0; lcl2=0.0;
				for (i=0;i<N;i++){
					for (j=i+1;j<N;j++){
						if (lap[i][j]<0.0){
							hl2+=gcirc[i][j]*gcirc[i][j];
							lcl2+=lcirc[i][j]*lcirc[i][j];
						}
					}
				}

				fe=fopen("edgeflow.txt","w");
				fg=fopen("gradientflow.txt","w");
				fh=fopen("harmonicflow.txt","w");
				fc=fopen("curlflow.txt","w");
				fa=fopen("adjmatrix.txt","w");
				for (i=0;i<N;i++){
					for (j=0;j<N;j++){
						if (adj[i][j]==1)fprintf(fa,"1 ");
						else fprintf(fa,"0 ");

						if (lap[i][j]<0.0){
							fprintf(fe,"%lf ",edge[i][j]);
							fprintf(fg,"%lf ",grad[i][j]);
							fprintf(fh,"%lf ",gcirc[i][j]);
							fprintf(fc,"%lf ",lcirc[i][j]);
						} else{
							fprintf(fe,"%lf ",0.0);
							fprintf(fg,"%lf ",0.0);
							fprintf(fh,"%lf ",0.0);
							fprintf(fc,"%lf ",0.0);
						}
					}
					fprintf(fe,"\n");
					fprintf(fg,"\n");
					fprintf(fh,"\n");
					fprintf(fc,"\n");
					fprintf(fa,"\n");
				}
				fclose(fe);
				fclose(fg);
				fclose(fh);
				fclose(fc);
				fclose(fa);

				gsl_matrix_free(v_gsl2);
				gsl_vector_free(s_gsl2);
				gsl_vector_free(work2);

				free(ccurcur);
				free(diag2);
				free(vp);

				if (curcur){
					for (i=0;i<trinum;i++){
						free(curcur[i]);
					}
				}
				if (uu2){
					for (i=0;i<trinum;i++){
						free(uu2[i]);
					}
				}
				if (vv2){
					for (i=0;i<trinum;i++){
						free(vv2[i]);
					}
				}
				if (curcur_inv){
					for (i=0;i<trinum;i++){
						free(curcur_inv[i]);
					}
				}

			} else{
				ct=0.0;
				ht=et-gt-ct;

				for (i=0;i<N;i++){
					for (j=0;j<N;j++){
						gcirc[i][j]=circ[i][j];
					}
				}

				hl2=0.0; lcl2=0.0;
				for (i=0;i<N;i++){
					for (j=i+1;j<N;j++){
						if (lap[i][j]<0.0){
							hl2+=gcirc[i][j]*gcirc[i][j];
						}
					}
				}
			}

			if (cl2>0.0)gcratio=hl2/el2;
			else gcratio=0.0;

			hl2_av+=hl2/(double)M;
			lcl2_av+=lcl2/(double)M;
			gcratio_av+=gcratio/(double)M;

			lcratio=cratio-gcratio;
			lcratio_av+=lcratio/(double)M;

			et_av+=et/(double)M;
			gt_av+=gt/(double)M;
			ht_av+=ht/(double)M;
			ct_av+=ct/(double)M;

			if (m%1==0)printf("%lf %d %d %lf %lf %lf %lf %lf %lf\n",q,m,cnum,el2,gl2,hl2,lcl2,gcratio,lcratio);
			fprintf(fq,"%lf %d %d %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf\n",q,m,cnum,el2,gl2,hl2,lcl2,gcratio,lcratio,et,gt,ht,ct);

			if (nb){
				for (i=0;i<N;i++){
					if (nb[i])free(nb[i]);
				}
				free(nb);
			}

			if (rule){
				for (i=0;i<N;i++){
					if (rule[i])free(rule[i]);
				}
				free(rule);
			}
		}//end of m-loop

		fprintf(fp,"%lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf %.15lf\n",q,el2_av,gl2_av,hl2_av,lcl2_av,gcratio_av,lcratio_av,et_av,gt_av,ht_av,ct_av);
	}//end of q-loop

	fclose(fp);
	fclose(fq);

	return 0;
}


/*
 * ParticleFilter.h
 *
 *  Created on: Mar 21, 2012
 *      Author: nair
 */

#ifndef PARTICLEFILTER_H_
#define PARTICLEFILTER_H_

#include <QSettings>
#include <QFileInfo>

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

namespace tracking
{

class ParticleFilterParams
{
public:
    ParticleFilterParams();
    ~ParticleFilterParams();

    void parse(std::string filename);

public:

    int NUMBER_OF_PARTICLES;

    /* standard deviations for gaussian sampling in transition model */
    float TRANS_X_STD;
    float TRANS_Y_STD;
    float TRANS_Z_STD;

    /* autoregressive dynamics parameters for transition model */
    float DYNAMICS_A1;
    float DYNAMICS_A2;
    float DYNAMICS_B0;

    bool DYNAMIC_MODEL_CV;

    float LAMBDA;
};

struct Particle
{
    float x;          /**< current x coordinate */
    float y;          /**< current y coordinate */
    float z;          /**< current z coordinate */
    float xp;         /**< previous x coordinate */
    float yp;         /**< previous y coordinate */
    float zp;         /**< previous z coordinate */
    float x0;         /**< original x coordinate */
    float y0;         /**< original y coordinate */
    float z0;			/**< original z coordinate */
    float x_velocity; /**< current x_velocity coordinate */
    float y_velocity; /**< current y_velocity coordinate */
    float z_velocity; /**< current z_velocity coordinate */

    float w;          /**< weight */
};

class ParticleFilter
{
public:

    ParticleFilter(uint& filter_id);
    ~ParticleFilter();

    void initialize(const cv::Vec3d& initPose);

    void predict();

    void predict_with_simple_model();

    void predict_with_cv_model();

    void correct();

    void resizeParticleSet(int n);

    void setParticleWeightFromDistanceMeasure(int id, float dist);

    void setParticleWeight(int id, float w);

    int nOfParticles()
    {
        return mParams.NUMBER_OF_PARTICLES;
    }

    static int particle_cmp(const void* p1,const void* p2 )
    {
        Particle* _p1 = (Particle*)p1;
        Particle* _p2 = (Particle*)p2;

        if( _p1->w > _p2->w )
            return -1;
        if( _p1->w < _p2->w )
            return 1;
        return 0;
    }

    void getParticlePose(int id, cv::Vec3d& pose);

    void getOutputPose(cv::Vec3d& pose);

    void getOutputVelocity(cv::Vec3d& vel);

    uint getFilterId();

    float getLikelihood();

private:

    void normalize();

    void resample();

private:

    ParticleFilterParams mParams;

    Particle* mParticles;

    cv::Vec3d mOutputPose;

    Eigen::MatrixXd A_Matrix;
    Eigen::MatrixXd G_Matrix;
    Eigen::MatrixXd W_Matrix;

    Eigen::MatrixXd State_Matrix;
    Eigen::MatrixXd State_Prev_Matrix;

    gsl_rng* rng;

    uint filterId;
    float mMaxLikelihood;
};

}
#endif /* PARTICLEFILTER_H_ */


/* AUTORIGHTS
Copyright (C) 2007 Princeton University

This file is part of Ferret Toolkit.

Ferret Toolkit is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2, or (at your option)
any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "LSH.h"

extern int LSH_release (cass_table_t *table);
extern int LSH_dump (cass_table_t *table);

int LSH_init_private (cass_table_t *table, const char *param)

{
	LSH_t *lsh;
	cass_size_t D, M, L, H;
	cass_size_t n;
	const gsl_rng_type *T;
	gsl_rng *r;
	int i, j;
	float w;

	lsh = type_calloc(LSH_t, 1);
	assert(table->parent_cfg != NULL);
	D = lsh->D = table->parent_cfg->vec_dim;
	M = lsh->M = param_get_int(param, "-M", 10);
	assert(M <= sizeof(uint32_t) * 8);
	L = lsh->L = param_get_int(param, "-L", 1);
	H = lsh->H = param_get_int(param, "-H", 1017881);

	lsh->W = type_calloc(float, L);

	w = param_get_float(param, "-w", 1.0);

	for (i = 0; i < L; i++) lsh->W[i] = w;

	if (strstr(param, "-W") && strstr(param, "-recall")) debug("-W and -recall conflicts.\n");

	param_get_float_array(param, "-W", &n, lsh->W);

	lsh->alphas = type_matrix_alloc(float, L * M, D);
	lsh->rnd = type_matrix_alloc(uint32_t, L, M);
	lsh->betas = (float *)malloc(L * M * sizeof(float));

	lsh->tmp = type_matrix_alloc(uint32_t, L, M);
	lsh->tmp2 = type_calloc(uint32_t, L);

	gsl_rng_env_setup();

	T = gsl_rng_default;
	r = gsl_rng_alloc(T);

	for (i = 0; i < L * M; i++)
	{
		for (j = 0; j < D; j++)
		{
			lsh->alphas[i][j] = gsl_ran_gaussian(r, 1.0);
		}
	}

	for (i = 0; i < L * M; i++)
	{
		lsh->betas[i] = gsl_rng_uniform(r) * lsh->W[i / M];
	}
	gsl_rng_free(r);

	srand(0);
	for (i = 0; i < L; i++)
		for (j = 0; j < M; j++)
			lsh->rnd[i][j] = rand();

	lsh->est = NULL;

	if (strstr(param, "-recall"))
	{
		cass_size_t d_step, max_T;
		float d_min, d_max;

		d_step = param_get_int(param, "-d_step", 100);
		max_T = param_get_int(param, "-T_max", 100);
		d_min = param_get_float(param, "-d_min", 0.0);
		d_max = param_get_float(param, "-d_max", 100);
		LSH_recall_init(&lsh->recall, d_step, d_min, d_max, lsh->M, lsh->L, max_T, lsh->W[0]);
	}

	table->__private = lsh;
	return 0;
}

int LSH_free_private (cass_table_t *table)
{
	LSH_t *lsh = table->__private;
	if (table->loaded) LSH_release(table);
	matrix_free(lsh->rnd);
	free(lsh->betas);
	matrix_free(lsh->alphas);
	free(lsh->W);

	matrix_free(lsh->tmp);
	free(lsh->tmp2);

	if (lsh->recall.table != NULL) LSH_recall_cleanup(&lsh->recall);

	free(lsh);

	table->__private = NULL;
	return 0;
}

void LSH_hash2 (LSH_t *lsh, uint32_t **hash, uint32_t *hash2)
{
	int i, j;
	for (i = 0; i < lsh->L; i++)
	{
		hash2[i] = 0;
		for (j = 0; j < lsh->M; j++)
		{
			hash2[i] += lsh->rnd[i][j] * hash[i][j];
		}
		hash2[i] %= lsh->H;
	}
}

void LSH_hash (LSH_t *lsh, const float *pnt, uint32_t **hash)
{
	float s;
	int i, j, k, l;

	l = 0;
	for (i = 0; i < lsh->L; i++)
	{
		for (j = 0; j < lsh->M; j++)
		{
			s = lsh->betas[l];
			for (k = 0; k < lsh->D; k++)
			{
				s += pnt[k] * lsh->alphas[l][k];
			}

			s /= lsh->W[i];
			hash[i][j] = floor(s);
			l++;
		}
	}
}

static inline void LSH_hash_L (LSH_t *lsh, float *pnt, uint32_t **hash, int L)
{
	float s;
	int j, k, l;

	l = L * lsh->M;
	for (j = 0; j < lsh->M; j++)
	{
		s = lsh->betas[l];
		for (k = 0; k < lsh->D; k++)
		{
			s += pnt[k] * lsh->alphas[l][k];
		}

		s /= lsh->W[L];
		hash[L][j] = floor(s);
		l++;
	}
}

static inline void LSH_hash2_L (LSH_t *lsh, uint32_t **hash, uint32_t *hash2, int L)
{
	int j;
	hash2[L] = 0;
	for (j = 0; j < lsh->M; j++)
	{
		hash2[L] += lsh->rnd[L][j] * hash[L][j];
	}
	hash2[L] %= lsh->H;
}

static void LSH_insert (LSH_t *lsh, float *pnt, int tag)
{
	int l;
	LSH_hash(lsh, pnt, lsh->tmp);
	LSH_hash2(lsh, lsh->tmp, lsh->tmp2);

	for (l = 0; l < lsh->L; l++)
	{
		ohash_insert(&lsh->hash[l], lsh->tmp2[l], tag);
	}
	lsh->count++;
}

int LSH_batch_insert(cass_table_t *table, cass_dataset_t *parent, cass_vecset_id_t start, cass_vecset_id_t end)
{
	LSH_t *lsh = table->__private;
	uint32_t i, j, k;
	cass_vec_t *vec;
	for (i = start; i <= end; i++)
	{
		for (j = 0;  j < parent->vecset[i].num_regions; j++)
		{
			k =  j + parent->vecset[i].start_vecid;
			vec = DATASET_VEC(parent, k);
			LSH_insert(lsh, vec->u.float_data, k);
			lsh->count++;
		}
	}
	return 0;
}

/*
void LSH_mass_insert_parallel (LSH_t *lsh, float **pnt, int cnt)
{
	int l;
#pragma omp parallel for schedule(static, 1) shared(lsh, pnt, cnt)
	for (l =  0; l < lsh->L; l++)
	{
		printf("#TH = %d / %d\n", omp_get_thread_num(),
				omp_get_num_threads());
		int i;
		for (i = 0; i < cnt; i++)
		{
			LSH_hash_L(lsh, pnt[i], tmp, l);
			LSH_hash2_L(lsh, tmp, tmp2, l);
			ohash_insert(&lsh->hash[l], tmp2[l], i);
		}
	}
}
*/

int LSH_restore_private (cass_table_t *table, CASS_FILE *fin)
{
	int ret;
	cass_size_t L, M, D;
	int32_t recall;
	LSH_t *lsh;

	lsh = type_calloc(LSH_t, 1);

	ret = cass_read_size(&lsh->D, 1, fin);
	ret += cass_read_size(&lsh->M, 1, fin);
	ret += cass_read_size(&lsh->L, 1, fin);
	ret += cass_read_size(&lsh->H, 1, fin);
	ret += cass_read_size(&lsh->count, 1, fin);

	assert(ret == 5);

	lsh->W = type_calloc(float, lsh->L);
	ret  = cass_read_float(lsh->W, lsh->L, fin);
	assert(ret == lsh->L);

	L = lsh->L;
	M = lsh->M;
	D = lsh->D;

	lsh->alphas = type_matrix_alloc(float, L * M, D);
	lsh->betas = type_calloc(float, L * M);
	lsh->hash = NULL; //(ohash_t *)malloc(L * sizeof(ohash_t));

	lsh->tmp = type_matrix_alloc(uint32_t, L, M);
	lsh->tmp2 = malloc(sizeof(uint32_t) *L );

	ret = cass_read_float(lsh->alphas[0], L * M * D, fin);
	assert(ret == L * M * D);
	ret = cass_read_float(lsh->betas, L * M, fin);
	assert(ret == L * M);

	lsh->rnd = type_matrix_alloc(uint32_t, L, M);

	ret = cass_read_uint32(lsh->rnd[0], L * M, fin);
	assert(ret == L * M);

	ret = cass_read_int32(&recall, 1, fin);
	assert(ret == 1);

	if (recall)
	{
		if (LSH_recall_load(&lsh->recall, fin) != 0) assert(0);
	}

	table->__private = lsh;

	return 0;
}

int LSH_checkpoint_private (cass_table_t *table, CASS_FILE *fout)
{
	LSH_t *lsh = table->__private;
	int ret;
	cass_size_t L, M, D;
	int32_t recall;
	ret = cass_write_size(&lsh->D, 1, fout);
	ret += cass_write_size(&lsh->M, 1, fout);
	ret += cass_write_size(&lsh->L, 1, fout);
	ret += cass_write_size(&lsh->H, 1, fout);
	ret += cass_write_size(&lsh->count, 1, fout);
	assert(ret == 5);
	L = lsh->L;
	M = lsh->M;
	D = lsh->D;

	ret = cass_write_float(lsh->W, L, fout);
	assert(ret == L);

	ret = cass_write_float(lsh->alphas[0], L * M * D, fout);
	assert(ret == L * M * D);
	ret = cass_write_float(lsh->betas, L * M, fout);
	assert(ret == L * M);
	ret = cass_write_uint32(lsh->rnd[0], L * M, fout);
	assert(ret == L * M);

	recall = (lsh->recall.table != NULL);

	ret = cass_write_int32(&recall, 1, fout);
	assert(ret == 1);

	if (recall)
	{
		if (LSH_recall_dump(&lsh->recall, fout) != 0) assert(0);
	}

	if (lsh->hash != NULL && table->dirty) return LSH_dump(table);

	return 0;
}

int LSH_load (cass_table_t *table)
{
	LSH_t *lsh = table->__private;
	CASS_FILE *fin;
	uint32_t i;
	int ret;

	assert(lsh->hash == NULL);

	if (lsh->count == 0)
	{
		lsh->hash = type_calloc(ohash_t, lsh->L);
		for (i = 0; i < lsh->L; i++)
		{
			ohash_init(&(lsh->hash[i]), lsh->H);
		}
		return 0;
	}

	fin = cass_open(table->filename, "r");
	if (fin == NULL) return CASS_ERR_IO;

	lsh->hash = type_calloc(ohash_t, lsh->L);
	for (i = 0; i < lsh->L; i++)
	{
		ret = ohash_init_with_stream(&lsh->hash[i], fin);
		assert(ret == 0);
	}

	cass_close(fin);
	return 0;
}

int LSH_release (cass_table_t *table)
{
	int err = 0;
	uint32_t i;
	LSH_t *lsh = table->__private;
	if (table->loaded && table->dirty) err = LSH_dump(table);
	if (err != 0) return err;
	for (i = 0; i < lsh->L; i++)
	{
		ohash_cleanup(&(lsh->hash[i]));
	}
	free(lsh->hash);
	lsh->hash = NULL;
	return 0;
}

int LSH_dump (cass_table_t *table)
{
	uint32_t i;
	int ret;
	CASS_FILE *fout;
	LSH_t *lsh = table->__private;
	if (lsh->count == 0) return 0;
	assert(lsh->hash != NULL);
	fout = cass_open(table->filename, "w");
	assert(fout != NULL);
	for (i = 0; i < lsh->L; i++)
	{
		ret = ohash_dump_stream(&lsh->hash[i], fout);
		assert(ret == 0);
	}
	cass_close(fout);
	table->dirty = 0;
	return ret;
}

void LSH_stat (LSH_t *lsh)
{
	int i;
	for (i = 0; i < lsh->L; i++)
	{
		printf("%d:", i);
		ohash_stat(&lsh->hash[i]);
	}
}

/*
int LSH_est_dump_file (LSH_est_t *est, char *filename)
{
	int ret;
	CASS_FILE *fout = cass_open(filename, "wb");
	assert(fout != NULL);
	ret = 0;
	ret += cass_write_int(&est->a_step, 1, fout);
	ret += cass_write_double(&est->a_min, 1, fout);
	ret += cass_write_double(&est->a_max, 1, fout);
	ret += cass_write_int(&est->b_step, 1, fout);
	ret += cass_write_double(&est->b_min, 1, fout);
	ret += cass_write_double(&est->b_max, 1, fout);

	assert(ret == 6);

	est->a_delta = (est->a_max - est->a_min) / est->a_step;
	est->b_delta = (est->b_max - est->b_min) / est->b_step;

	type_matrix_dump_stream(double, fout, est->a_step, est->b_step, est->a_table);
	type_matrix_dump_stream(double, fout, est->a_step, est->b_step, est->b_table);

	cass_close(fout);

	return 0;
}

int LSH_est_init_with_file (LSH_est_t *est, char *filename)
{
	int ret;
	cass_size_t row, col;
	CASS_FILE *fin = cass_open(filename, "rb");
	assert(fin != NULL);
	ret = 0;
	ret += cass_read_int32(&est->a_step, 1, fin);
	ret += cass_read_double(&est->a_min, 1, fin);
	ret += cass_read_double(&est->a_max, 1, fin);
	ret += cass_read_int32(&est->b_step, 1, fin);
	ret += cass_read_double(&est->b_min, 1, fin);
	ret += cass_read_double(&est->b_max, 1, fin);

	assert(ret == 6);

	est->a_delta = (est->a_max - est->a_min) / est->a_step;
	est->b_delta = (est->b_max - est->b_min) / est->b_step;

	type_matrix_load_stream(double, fin, &row, &col, &est->a_table);
	assert(row == est->a_step);
	assert(col == est->b_step);
	type_matrix_load_stream(double, fin, &row, &col, &est->b_table);
	assert(row == est->a_step);
	assert(col == est->b_step);

	cass_close(fin);

	return 0;
}
*/

void LSH_est_cleanup (LSH_est_t *est)
{
	matrix_free(est->a_table);
	matrix_free(est->b_table);
}

double LSH_est (LSH_est_t *est, double a, double b, int K)
{
	int A, B;
	double alpha, beta;
	if (!(finite(a) && finite(b))) return 0.0;

	A = (a - est->a_min) / est->a_delta;
	B = (b - est->b_min) / est->b_delta;

	if (A < 0) return 0.0;
	if (B < 0) return 0.0;
	if (A >= est->a_step) A = est->a_step - 1;
	if (B >= est->a_step) B = est->a_step - 1;

	alpha = est->a_table[A][B];
	beta = est->b_table[A][B];

	return alpha * pow(K, beta);
}

int __LSH_query(cass_table_t *table,
                     cass_query_t *query, cass_result_t *result)
{
	LSH_t *lsh = (LSH_t *)table->__private;
	LSH_query_t query2;
	cass_table_t *parent;
	cass_dataset_t *ds;
	cass_vec_dist_t *vec_dist;
	cass_vecset_t *vecset;

	float recall;

	cass_size_t K, L, T;

	uint32_t i, j;

	assert(table->loaded);
	vec_dist = cass_reg_get(&table->env->vec_dist, query->vec_dist_id);
	assert(vec_dist != NULL);
	if (vec_dist->__class != &vec_dist_L2_float) debug("LSH only works for L2 distance for float.\n");

	if (table->parent_id == CASS_ID_INV) debug("LSH query requires parent.\n");
	parent = cass_reg_get(&table->env->table, table->parent_id);
	assert(parent != NULL);
	if (!parent->loaded) debug("LSH query requires parent to be loaded.\n");
	if (!(parent->opr->type & CASS_DATA)) debug("LSH only works on CASS_DATA.\n");

	ds = (cass_dataset_t *)parent->__private;

	K = query->topk;
	if (K == 0) debug("LSH only works for KNN queries.\n");

	L = param_get_int(query->extra_params, "-L", 1);
	T = param_get_int(query->extra_params, "-T", 0);

	recall = param_get_float(query->extra_params, "-recall", 0);

	LSH_query_init(&query2, lsh, ds, K, L, T);

	assert((query->flags & CASS_RESULT_BITMAPS) == 0);
	assert((query->flags & CASS_RESULT_BITMAP) == 0);
	assert((query->flags & CASS_RESULT_LIST) == 0);
	assert(query->flags & CASS_RESULT_LISTS);
	assert(query->candidate == NULL);

	result->flags = CASS_RESULT_LISTS;

	vecset = query->dataset->vecset + query->vecset_id;

	if (query->flags & CASS_RESULT_USERMEM)
	{
		assert(result->u.lists.len >= vecset->num_regions);
	}
	else
	{
		result->flags |= CASS_RESULT_MALLOC;
		ARRAY_INIT_SIZE(result->u.lists, vecset->num_regions);
		result->u.lists.len = vecset->num_regions;
	}

	for (i = 0; i < vecset->num_regions; i++)
	{
		if (query->flags & CASS_RESULT_USERMEM)
		{
			assert(result->u.lists.data[i].size >= query->topk);
		}
		else
		{
			ARRAY_INIT_SIZE(result->u.lists.data[i], query->topk);
		}
		result->u.lists.data[i].len = query->topk;

		if (recall == 0)
		{
			LSH_query(&query2, DATASET_VEC(query->dataset, vecset->start_vecid + i)->u.float_data);
		}
		else
		{
			LSH_query_recall(&query2, DATASET_VEC(query->dataset, vecset->start_vecid + i)->u.float_data, recall);
		}

		for (j = 0; j < K; j++)
		{
			result->u.lists.data[i].data[j] = query2.topk[j];
		}
	}

	if (query->flags & CASS_RESULT_SORT)
	{
		for (j = 0; j < vecset->num_regions; j++)
		{
			TOPK_SORT_MIN(result->u.lists.data[j].data, cass_list_entry_t, dist, query->topk);
		}
		result->flags |= CASS_RESULT_SORT;
	}

	LSH_query_cleanup(&query2);

	return 0;

}

int __LSH_batch_query(cass_table_t *table,
/* the dataset of all the queries should be the same */
	uint32_t count, cass_query_t **queries, cass_result_t **results)
{
	LSH_t *lsh = (LSH_t *)table->__private;
	cass_result_t *result;
	cass_query_t *query;

	LSH_query_t query2;

	const float **points;
	cass_list_entry_t **topks;

	cass_table_t *parent;
	cass_dataset_t *ds;
	cass_vec_dist_t *vec_dist;
	cass_vecset_t *vecset;

	cass_size_t K, L, T, vec_count;

	uint32_t i, j, k;

	query = queries[0];

	assert(table->loaded);
	vec_dist = cass_reg_get(&table->env->vec_dist, query->vec_dist_id);
	assert(vec_dist != NULL);
	if (vec_dist->__class != &vec_dist_L2_float) debug("LSH only works for L2 distance for float.\n");

	if (table->parent_id == CASS_ID_INV) debug("LSH query requires parent.\n");
	parent = cass_reg_get(&table->env->table, table->parent_id);
	assert(parent != NULL);
	if (!parent->loaded) debug("LSH query requires parent to be loaded.\n");
	if (!(parent->opr->type & CASS_DATA)) debug("LSH only works on CASS_DATA.\n");

	ds = (cass_dataset_t *)parent->__private;

	K = query->topk;
	if (K == 0) debug("LSH only works for KNN queries.\n");

	L = param_get_int(query->extra_params, "-L", 1);
	T = param_get_int(query->extra_params, "-T", 0);

	LSH_query_init(&query2, lsh, ds, K, L, T);

	assert((query->flags & CASS_RESULT_BITMAPS) == 0);
	assert((query->flags & CASS_RESULT_BITMAP) == 0);
	assert((query->flags & CASS_RESULT_LIST) == 0);
	assert(query->flags & CASS_RESULT_LISTS);
	assert(query->candidate == NULL);

	vec_count = 0;
	for (i = 0; i < count; i++)
	{
		result = results[i];
		query = queries[i];
		result->flags = CASS_RESULT_LISTS;
		vecset = query->dataset->vecset + query->vecset_id;

		if (query->flags & CASS_RESULT_USERMEM)
		{
			assert(result->u.lists.len >= vecset->num_regions);
		}
		else
		{
			result->flags |= CASS_RESULT_MALLOC;
			ARRAY_INIT_SIZE(result->u.lists, vecset->num_regions);
			result->u.lists.len = vecset->num_regions;
		}

		for (j = 0; j < vecset->num_regions; j++)
		{
			if (query->flags & CASS_RESULT_USERMEM)
			{
				assert(result->u.lists.data[j].size >= query->topk);
			}
			else
			{
				ARRAY_INIT_SIZE(result->u.lists.data[j], query->topk);
			}
			result->u.lists.data[j].len = K;
		}
		vec_count += vecset->num_regions;
	}

	points = type_calloc(const float *, vec_count);
	topks = type_calloc(cass_list_entry_t *, vec_count);

	k = 0;
	for (i = 0; i < count; i++)
	{
		result = results[i];
		query = queries[i];
		vecset = query->dataset->vecset + query->vecset_id;
		for (j = 0; j < vecset->num_regions; j++)
		{
			points[k] = DATASET_VEC(query->dataset, vecset->start_vecid + j)->u.float_data;
			topks[k] = result->u.lists.data[j].data;
			k++;
		}
	}

	assert(k == vec_count);

	if (strstr(query->extra_params, "-ca"))
	{
		LSH_query_batch_ca(&query2, vec_count, points, topks);
	}
	else
	{
		LSH_query_batch(&query2, vec_count, points, topks);
	}

	query = queries[0];

	if (query->flags & CASS_RESULT_SORT)
	{
		for (i = 0; i < vec_count; i++)
		{
			TOPK_SORT_MIN(topks[i], cass_list_entry_t, dist, K);
		}
		for (i = 0; i < count; i++)
		{
			results[i]->flags |= CASS_RESULT_SORT;
		}
	}

	LSH_query_cleanup(&query2);

	return 0;
}

cass_table_opr_t opr_lsh = {
	.name = "lsh",
	.type = CASS_VEC_INDEX,
	.vecset_type = CASS_ANY,
	.vec_type = CASS_VEC_FLOAT,
	.dist_vecset = CASS_ANY,
	.dist_vec = CASS_VEC_DIST_TYPE_L2,

	.cfg = NULL,
	.tune = NULL, //mplsh_tune,
	.init_private = LSH_init_private,
	.batch_insert = LSH_batch_insert,
	.query = __LSH_query, //mplsh_query,
	.batch_query = __LSH_batch_query,
	.load = LSH_load,
	.release = LSH_release,
	.checkpoint_private = LSH_checkpoint_private,
	.restore_private = LSH_restore_private,
	.free_private = LSH_free_private
};


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_linear_theory.h>
#include <gsl/gsl_multimin.h>

typedef struct init_gbpCosmo2gbpCosmo_integrand_params_struct init_gbpCosmo2gbpCosmo_integrand_params;
struct init_gbpCosmo2gbpCosmo_integrand_params_struct {
    double                     inv_s;
    double                     z_source;
    double                     z_target;
    double                     R_1;
    double                     R_2;
    cosmo_info **              cosmo_source;
    cosmo_info **              cosmo_target;
    int                        n_int;
    gsl_integration_workspace *wspace;
};

double init_gbpCosmo2gbpCosmo_integrand(double R, void *params_in) {
    init_gbpCosmo2gbpCosmo_integrand_params *params       = (init_gbpCosmo2gbpCosmo_integrand_params *)params_in;
    double                                   inv_s        = params->inv_s;
    double                                   z_source     = params->z_source;
    double                                   z_target     = params->z_target;
    cosmo_info **                            cosmo_source = params->cosmo_source;
    cosmo_info **                            cosmo_target = params->cosmo_target;
    return (pow(1. - (sigma_R(cosmo_source, inv_s * R, z_source, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER)) /
                         (sigma_R(cosmo_target, R, z_target, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER)),
                2.) /
            R);
}

double init_gbpCosmo2gbpCosmo_minimize_function(const gsl_vector *v_i, void *params_in) {
    // Set variable integrand parameters
    init_gbpCosmo2gbpCosmo_integrand_params *params = (init_gbpCosmo2gbpCosmo_integrand_params *)params_in;
    params->inv_s                                   = gsl_vector_get(v_i, 0);
    params->z_target                                = gsl_vector_get(v_i, 1);

    // Perform integral to minimize
    gsl_function integrand;
    double       delta_i;
    double       abs_error;
    integrand.function = init_gbpCosmo2gbpCosmo_integrand;
    integrand.params   = params_in;
    gsl_integration_qag(&integrand, params->R_1, params->R_2, 0, 1e-3, params->n_int, GSL_INTEG_GAUSS61, params->wspace, &delta_i, &abs_error);
    return (delta_i / take_ln(params->R_2 / params->R_1));
}

void init_gbpCosmo2gbpCosmo(cosmo_info **           cosmo_source,
                            cosmo_info **           cosmo_target,
                            double                  z_min,
                            double                  M_min,
                            double                  M_max,
                            gbpCosmo2gbpCosmo_info *gbpCosmo2gbpCosmo) {
    SID_log("Initializing cosmology scaling...", SID_LOG_OPEN | SID_LOG_TIMER);
    SID_set_verbosity(SID_SET_VERBOSITY_RELATIVE, -1);

    // Store some infor in the gbpCosmo2gbpCosmo_info structure
    gbpCosmo2gbpCosmo->M_min        = M_min;
    gbpCosmo2gbpCosmo->M_max        = M_max;
    gbpCosmo2gbpCosmo->z_min        = z_min;
    gbpCosmo2gbpCosmo->cosmo_source = (*cosmo_source);
    gbpCosmo2gbpCosmo->cosmo_target = (*cosmo_target);

    // Perform minimization
    // const gsl_multimin_fminimizer_type *T=gsl_multimin_fminimizer_nmsimplex2;
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
    gsl_multimin_fminimizer *           s = NULL;
    gsl_vector *                        ss, *x;
    gsl_multimin_function               minex_func;

    // Starting point
    x = gsl_vector_alloc(2);
    gsl_vector_set(x, 0, 1.);    // inv_s
    gsl_vector_set(x, 1, z_min); // z_scaled

    // Set initial step sizes to 1
    ss = gsl_vector_alloc(2);
    gsl_vector_set_all(ss, 1.0);

    // Set parameters
    init_gbpCosmo2gbpCosmo_integrand_params params;
    params.cosmo_source = cosmo_source;
    params.cosmo_target = cosmo_target;
    params.z_source     = z_min;
    params.R_1          = R_of_M(M_min, *cosmo_source);
    params.R_2          = R_of_M(M_max, *cosmo_source);
    params.inv_s        = gsl_vector_get(x, 0);
    params.z_target     = gsl_vector_get(x, 1);
    params.n_int        = 100;
    params.wspace       = gsl_integration_workspace_alloc(params.n_int);

    // Initialize method
    minex_func.n      = 2;
    minex_func.f      = init_gbpCosmo2gbpCosmo_minimize_function;
    minex_func.params = (void *)(&params);
    s                 = gsl_multimin_fminimizer_alloc(T, 2);
    gsl_multimin_fminimizer_set(s, &minex_func, x, ss);

    // Perform minimization
    double size;
    int    status;
    size_t iter     = 0;
    size_t iter_max = 200;
    do {
        iter++;
        status = gsl_multimin_fminimizer_iterate(s);
        if(status)
            SID_exit_error("Error encountered during minimisation in init_gbpCosmo2gbpCosmo() (status=%d).",
                           SID_ERROR_LOGIC, status);
        size   = gsl_multimin_fminimizer_size(s);
        status = gsl_multimin_test_size(size, 1e-2);
    } while(status == GSL_CONTINUE && iter <= iter_max);
    if(status != GSL_SUCCESS)
        SID_exit_error("Failed to converge during minimisation in init_gbpCosmo2gbpCosmo() (status=%d,iter=%d).",
                       SID_ERROR_LOGIC, status, iter);

    // Finalize results
    double Omega_M_source           = ((double *)ADaPS_fetch(*cosmo_source, "Omega_M"))[0];
    double H_Hubble_source          = 1e2 * ((double *)ADaPS_fetch(*cosmo_source, "h_Hubble"))[0];
    double Omega_M_target           = ((double *)ADaPS_fetch(*cosmo_target, "Omega_M"))[0];
    double H_Hubble_target          = 1e2 * ((double *)ADaPS_fetch(*cosmo_target, "h_Hubble"))[0];
    gbpCosmo2gbpCosmo->s_L          = 1. / gsl_vector_get(s->x, 0);
    gbpCosmo2gbpCosmo->s_M          = (Omega_M_target * H_Hubble_target) / (Omega_M_source * H_Hubble_source) * pow((gbpCosmo2gbpCosmo->s_L), 3.);
    gbpCosmo2gbpCosmo->z_min_scaled = gsl_vector_get(s->x, 1);
    ;

    // Calculate growth factors needed for
    //    determining redshift mappings
    gbpCosmo2gbpCosmo->D_prime_z_min = linear_growth_factor(z_min, cosmo_target);
    gbpCosmo2gbpCosmo->D_z_scaled    = linear_growth_factor(gbpCosmo2gbpCosmo->z_min_scaled, cosmo_source);
    gbpCosmo2gbpCosmo->D_ratio       = gbpCosmo2gbpCosmo->D_prime_z_min / gbpCosmo2gbpCosmo->D_z_scaled;

    // Clean-up
    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free(s);
    gsl_integration_workspace_free(params.wspace);
    SID_set_verbosity(SID_SET_VERBOSITY_DEFAULT);
    SID_log("Done.", SID_LOG_CLOSE);
}


#ifndef DIFFEQ_SIM_H
#define DIFFEQ_SIM_H

#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>
#include <iostream>
#include <vector>

using namespace std;

class DiffEq_Sim {
    private:
        double t;      //initial time
        double h;      //time step
        double tmax;   //max time
        double hmin;

    public:
        DiffEq_Sim() {
            t    = 0.0;      //initial time
            h    = 0.1;     //time step
            tmax = 2000;
            hmin = 0.2;
        };

        ~DiffEq_Sim() {};

        int nbins;
        double* y;

        void printY() { for(int i=0; i < nbins; i++) { cout << y[i] << " ";} cout << endl; }

        vector<double> get_state() {
            vector<double> C;
            C.assign(y, y + nbins);
            return C;
        }

        double get_time() { return t; }

        virtual void initialize() {}
        virtual void derivative(const double y[], double dydt[]){}

        static int function(double t, double const y[], double dydt[], void *params) {
            DiffEq_Sim* model = static_cast <DiffEq_Sim*> (params);
            model->derivative(y, dydt);
            return GSL_SUCCESS;
        }

        int run_simulation() {
            gsl_odeiv_evolve*  e = gsl_odeiv_evolve_alloc(nbins);
            gsl_odeiv_control* c = gsl_odeiv_control_y_new(1e-20, 0);
            gsl_odeiv_step*    s = gsl_odeiv_step_alloc(gsl_odeiv_step_rkf45, nbins);
            gsl_odeiv_system sys = {function, NULL, nbins, this };
            while (t < tmax) {  //convergence check here
                int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, tmax, &h, y);
                if (status != GSL_SUCCESS) { return status; }
            }
            return 0;
        }

       int step_simulation( double stepsize ) {
            gsl_odeiv_evolve*  e = gsl_odeiv_evolve_alloc(nbins);
            gsl_odeiv_control* c = gsl_odeiv_control_y_new(1e-5, 0);
            gsl_odeiv_step*    s = gsl_odeiv_step_alloc(gsl_odeiv_step_rkf45, nbins);
            gsl_odeiv_system sys = {function, NULL, nbins, this };

            double tstop = t+stepsize;
            while (t < tstop) {
                int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, tstop, &h, y);
                if (status != GSL_SUCCESS) { return status; }
            }
            return 0;
        }

        /*
        double* advance_simulation(double I_lim) {
            while (t < 2000) {  //convergence check here
                int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t1, &h, y);
                if (y[1] < I_lim) { return y; }
            }
            printY();
            return y;
        }
*/
};

#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>

#include "cosmocalc.h"

static double deltaTm[9] = {200.0, 300.0, 400.0, 600.0, 800.0, 1200.0, 1600.0, 2400.0, 3200.0};
static double ATm[9] = {0.186, 0.2, 0.212, 0.218, 0.248, 0.255, 0.260, 0.260, 0.260};
static double aTm[9] = {1.47, 1.52, 1.56, 1.61, 1.87, 2.13, 2.30, 2.53, 2.66};
static double bTm[9] = {2.57, 2.25, 2.05, 1.87, 1.59, 1.51, 1.46, 1.44, 1.41};
static double cTm[9] = {1.19, 1.27, 1.34, 1.45, 1.58, 1.80, 1.97, 2.24, 2.44};

double mass_function(double m, double a)
{
  return tinker2008_mass_function(m,a,cosmoData.delta);
}

double tinker2008_mass_function(double m, double a, double delta)
{
  double A,am,b,c,alpha;

  static int init = 1;
  static gsl_spline *ATm_spline,*aTm_spline,*bTm_spline,*cTm_spline;
  static gsl_interp_accel *ATm_acc,*aTm_acc,*bTm_acc,*cTm_acc;

  if(init)
    {
#define TINKERCODEPARMS
#ifdef TINKERCODEPARMS
      //first parameter
      ATm[0] = 1.858659e-01 ;
      ATm[1] = 1.995973e-01 ;
      ATm[2] = 2.115659e-01 ;
      ATm[3] = 2.184113e-01 ;
      ATm[4] = 2.480968e-01 ;
      ATm[5] = 2.546053e-01 ;
      ATm[6] = 2.600000e-01 ;
      ATm[7] = 2.600000e-01 ;
      ATm[8] = 2.600000e-01 ;

      //second parameter
      aTm[0] = 1.466904e+00 ;
      aTm[1] = 1.521782e+00 ;
      aTm[2] = 1.559186e+00 ;
      aTm[3] = 1.614585e+00 ;
      aTm[4] = 1.869936e+00 ;
      aTm[5] = 2.128056e+00 ;
      aTm[6] = 2.301275e+00 ;
      aTm[7] = 2.529241e+00 ;
      aTm[8] = 2.661983e+00 ;

      //third parameter
      bTm[0] = 2.571104e+00 ;
      bTm[1] = 2.254217e+00 ;
      bTm[2] = 2.048674e+00 ;
      bTm[3] = 1.869559e+00 ;
      bTm[4] = 1.588649e+00 ;
      bTm[5] = 1.507134e+00 ;
      bTm[6] = 1.464374e+00 ;
      bTm[7] = 1.436827e+00 ;
      bTm[8] = 1.405210e+00 ;

      //fourth parameter
      cTm[0] = 1.193958e+00;
      cTm[1] = 1.270316e+00;
      cTm[2] = 1.335191e+00;
      cTm[3] = 1.446266e+00;
      cTm[4] = 1.581345e+00;
      cTm[5] = 1.795050e+00;
      cTm[6] = 1.965613e+00;
      cTm[7] = 2.237466e+00;
      cTm[8] = 2.439729e+00;
#endif

      ATm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      aTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      bTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      cTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);

      gsl_spline_init(ATm_spline,deltaTm,ATm,9);
      gsl_spline_init(aTm_spline,deltaTm,aTm,9);
      gsl_spline_init(bTm_spline,deltaTm,bTm,9);
      gsl_spline_init(cTm_spline,deltaTm,cTm,9);

      ATm_acc = gsl_interp_accel_alloc();
      aTm_acc = gsl_interp_accel_alloc();
      bTm_acc = gsl_interp_accel_alloc();
      cTm_acc = gsl_interp_accel_alloc();

      init = 0;
    }

  A = gsl_spline_eval(ATm_spline,delta,ATm_acc);
#ifdef TINKERCODEPARMS
  if(delta > 1600.0)
    A = 0.26;
#endif
  am = gsl_spline_eval(aTm_spline,delta,aTm_acc);
  b = gsl_spline_eval(bTm_spline,delta,bTm_acc);
  c = gsl_spline_eval(cTm_spline,delta,cTm_acc);

  double onepz = 1.0/a;
#ifdef TINKERCODEPARMS
  if(onepz > 4.0)
    onepz = 4.0;
#endif

  A *= pow(onepz,-0.14);
  am *= pow(onepz,-0.06);
  alpha = pow(10.0,-1.0*pow(0.75/log10(delta/75.0),1.2));
  b *= pow(onepz,-1.0*alpha);

  //fprintf(stderr,"A = %f, a = %f, b = %f, c = %f\n",A,am,b,c);

  double sigma = sigmaMtophat(m,a);
  double fsigma = A*(pow(sigma/b,-1.0*am) + 1.0)*exp(-1.0*c/sigma/sigma);
  double dm = 1e-6*m;
  double dlnsiginvdm = log(sigmaMtophat(m-dm/2.0,a)/sigmaMtophat(m+dm/2.0,a))/dm;

  return fsigma*RHO_CRIT*cosmoData.OmegaM/m*dlnsiginvdm;
}

static double alphaTm[9] = {0.368, 0.363, 0.385, 0.389, 0.393, 0.365, 0.379, 0.355, 0.327};
static double betaTm[9] = {0.589, 0.585, 0.544, 0.543, 0.564, 0.623, 0.637, 0.673, 0.702};
static double gammaTm[9] = {0.864, 0.922, 0.987, 1.09, 1.20, 1.34, 1.50, 1.68, 1.81};
static double phiTm[9] = {-0.729, -0.789, -0.910, -1.05, -1.20, -1.26, -1.45, -1.50, -1.49};
static double etaTm[9] = {-0.243, -0.261, -0.261, -0.273, -0.278, -0.301, -0.301, -0.319, -0.336};

static double tinker2010_mf_norm_integ(double lnnu, void *p)
{
  double *params = (double*) p;
  double nu = exp(lnnu);
  double fnu,bnu;

  double y = log10(params[4]);
  double A = 1.0 + 0.24*y*exp(-1.0*pow(4.0/y,4.0));
  double _a = 0.44*y - 0.88;
  double B = 0.183;
  double b = 1.5;
  double C = 0.019 + 0.107*y + 0.19*exp(-1.0*pow(4.0/y,4.0));
  double c = 2.4;

  if(nu == 0.0)
    return 0.0;
  else
    {
      fnu = (1.0 + pow(params[0]*nu,-2.0*params[1]))*pow(nu,2.0*params[2])*exp(-1.0*params[3]*nu*nu/2.0);
      bnu = 1.0 - A*pow(nu,_a)/(pow(nu,_a) + pow(1.686,_a)) + B*pow(nu,b) + C*pow(nu,c);

      return bnu*fnu*nu;
    }
}

double tinker2010_mass_function(double m, double a, double delta)
{
  double z,gsigma,nu,dlnsiginvdm;
  double dm = 1e-6*m;
  double alpha,beta,gamma,phi,eta;

  static int init = 1;
  static gsl_spline *alphaTm_spline,*betaTm_spline,*gammaTm_spline,*phiTm_spline,*etaTm_spline;
  static gsl_interp_accel *alphaTm_acc,*betaTm_acc,*gammaTm_acc,*phiTm_acc,*etaTm_acc;
  //static gsl_integration_glfixed_table *gltab;
  static gsl_integration_workspace *w;
  static double aprev = -1.0;
  static double alphaprev = -1.0;

  gsl_function f;
  size_t limit = 1000;
  double epsabs,epsrel;
  double abserr;
  double numin,numax;
  //int i,Nnu,dnu;
  double params[5];

  if(init)
    {
      alphaTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      betaTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      gammaTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      phiTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);
      etaTm_spline = gsl_spline_alloc(gsl_interp_cspline,9);

      gsl_spline_init(alphaTm_spline,deltaTm,alphaTm,9);
      gsl_spline_init(betaTm_spline,deltaTm,betaTm,9);
      gsl_spline_init(gammaTm_spline,deltaTm,gammaTm,9);
      gsl_spline_init(phiTm_spline,deltaTm,phiTm,9);
      gsl_spline_init(etaTm_spline,deltaTm,etaTm,9);

      alphaTm_acc = gsl_interp_accel_alloc();
      betaTm_acc = gsl_interp_accel_alloc();
      gammaTm_acc = gsl_interp_accel_alloc();
      phiTm_acc = gsl_interp_accel_alloc();
      etaTm_acc = gsl_interp_accel_alloc();

      //gltab = gsl_integration_glfixed_table_alloc((size_t) 500);
      w = gsl_integration_workspace_alloc(limit);

      init = 0;
    }

  alpha = gsl_spline_eval(alphaTm_spline,delta,alphaTm_acc);
  beta = gsl_spline_eval(betaTm_spline,delta,betaTm_acc);
  gamma = gsl_spline_eval(gammaTm_spline,delta,gammaTm_acc);
  phi = gsl_spline_eval(phiTm_spline,delta,phiTm_acc);
  eta = gsl_spline_eval(etaTm_spline,delta,etaTm_acc);

  nu = 1.686/sigmaMtophat(m,a);
  z = 1.0/a - 1.0;
  if(z > 3.0)
    z = 3.0;

  beta = beta*pow(1.0+z,0.20);
  phi = phi*pow(1.0+z,-0.08);
  eta = eta*pow(1.0+z,0.27);
  gamma = gamma*pow(1.0+z,-0.01);

  //determine alpha by normalization condition
  if(a != aprev)
    {
      aprev = a;

      params[0] = beta;
      params[1] = phi;
      params[2] = eta;
      params[3] = gamma;
      params[4] = delta;
      f.function = &tinker2010_mf_norm_integ;
      f.params = params;
      numin = 1e-10;
      numax = 1e10;
      epsabs = 1e-6;
      epsrel = 1e-6;

      gsl_integration_qag(&f,log(numin),log(numax),epsabs,epsrel,limit,GSL_INTEG_GAUSS61,w,&alphaprev,&abserr);
      //alphaprev = gsl_integration_glfixed(&f,log(numin),log(numax),gltab);

      /*
      alphaprev = 0.0;
      Nnu = 100;

      dnu = log(numax/numin)/Nnu;
      for(i=0;i<Nnu;++i)
	alphaprev += tinker2010_mf_norm_integ(log(numin)+dnu*i,f.params)*dnu;
      */
      //fprintf(stderr,"alpha = %g, norm = %g, alpha_norm = %g\n",alpha,alphaprev,1.0/alphaprev);

      alphaprev = 1.0/alphaprev;
    }

  alpha = alphaprev;

  gsigma = alpha*(1.0 + pow(beta*nu,-2.0*phi))*pow(nu,2.0*eta)*exp(-1.0*gamma*nu*nu/2.0)*nu;
  dlnsiginvdm = log(sigmaMtophat(m-dm/2.0,a)/sigmaMtophat(m+dm/2.0,a))/dm;

  return gsigma*RHO_CRIT*cosmoData.OmegaM/m*dlnsiginvdm;
}

double bias_function(double m, double a)
{
  return tinker2010_bias(m,a,cosmoData.delta);
}

double tinker2010_bias(double m, double a, double delta)
{
  double nu;
  nu =  1.686/sigmaMtophat(m,a);

  double y = log10(delta);
  double A = 1.0 + 0.24*y*exp(-1.0*pow(4.0/y,4.0));
  double _a = 0.44*y - 0.88;
  double B = 0.183;
  double b = 1.5;
  double C = 0.019 + 0.107*y + 0.19*exp(-1.0*pow(4.0/y,4.0));
  double c = 2.4;

  return 1.0 - A*pow(nu,_a)/(pow(nu,_a) + pow(1.686,_a)) + B*pow(nu,b) + C*pow(nu,c);
}


#pragma once

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

#include <tiny_dnn/tiny_dnn.h>

#include "defines.h"
#include "matrix_vector_ops.h"
#include "genann.h"

#define NUM_STATES 12
#define NUM_INPUTS 4

// Dynamics model from G. Gremillion, S. Humbert paper "System Identification of
// a Quadrotor Micro Air Vehicle" (equation 3 in paper)
// Augmented to include position dynamics in hover
// Dynamics and controls matrix:
// cos(ps)*u  -sin(ps)*v    0     0     0     0     0     0     0     0     0     0     0     0     0     0
// sin(ps)*u  cos(ps)*v     0     0     0     0     0     0     0     0     0     0     0     0     0     0
//  0     0     1     0     0     0     0     0     0     0     0     0     0     0     0     0
//  0     0     0   X_u*u   0     0     0     0     0     0  X_th*th  0     0     0     0     0
//  0     0     0     0   Y_v*v   0     0     0     0  Y_ph*ph  0     0     0     0     0     0
//  0     0     0     0     0   Z_w*w   0     0     0     0     0     0     0     0     0 Z_thr*d_thr
//  0     0     0     0     0     0   L_p*p   0     0  L_ph*ph  0     0 L_la*d_la 0     0     0
//  0     0     0     0     0     0     0   M_q*q   0     0   M_th*th 0     0 M_lo*d_lo 0     0
//  0     0     0     0     0     0     0     0   N_r*r   0     0     0     0     0 N_ya*d_ya 0
//  0     0     0     0     0     0   ph_p*p  0     0     0     0     0 ph_la*d_la 0    0     0
//  0     0     0     0     0     0     0   th_q*q  0     0     0     0     0 th_lo*d_lo 0    0
//  0     0     0     0     0     0     0     0   ps_r*r  0     0     0     0     0 ps_ya*d_ya 0
//
//  Parameters:
//  X_u   =  -0.27996
//  Y_v   =  -0.22566
//  Z_w   =  -1.2991
//  L_p   =  -2.5110
//  M_q   =  -2.4467
//  N_r   =  -0.4948
//  X_th  =  -10.067
//  Y_ph  =   9.8648
//  L_ph  =  -21.358
//  M_th  =  -18.664
//  ph_p  =   0.9655
//  th_q  =   0.9634
//  ps_r  =   0.6748
//  Z_thr =  -39.282
//  L_la  =   11.468
//  M_lo  =   9.5711
//  N_ya  =   3.5647
//  ph_la =   0.0744
//  th_lo =   0.0594
//  ps_ya =   0.0397
//
// State vector:
//    x y z u v w p q r phi theta psi
//
// Control vector:
//    del_lat del_lon del_yaw del_thrust
int dynamics(gsl_vector *dy, double t, const gsl_vector *y, const gsl_vector *u);
int setupDynamics();
int teardownDynamics();

gsl_vector *feedback(gsl_vector *yd, gsl_vector *y);
int setupFeedback();
int teardownFeedback();

gsl_vector *nnFeedback(gsl_vector *yd, gsl_vector *y);
int setupNnFeedback(int dataShape[2]);
int setupNnFeedback(FILE *in);
int teardownNnFeedback();
genann *getFeedbackNn();


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_interp.h>

#include "allvars.h"
#include "proto.h"

void line_convolution()
{

  FILE *fp;
  int info, i, j, nq;
  double flux, err, tline, taup, tcon, fcon, fcon_err, dtau;
  double *yrec_line;
//  double sigma, taud;
  double *Larr, *ybuf, *Cq, *ICq, *ave, *yave;

  gsl_interp_accel *gsl_acc_conv, *gsl_acc_error_conv;
  gsl_interp  *gsl_linear_conv, *gsl_linear_error_conv;

  gsl_acc_conv = gsl_interp_accel_alloc();
  gsl_acc_error_conv = gsl_interp_accel_alloc();

  gsl_linear_conv = gsl_interp_alloc(gsl_interp_linear, ncon);
  gsl_linear_error_conv = gsl_interp_alloc(gsl_interp_linear, ncon);

  gsl_interp_init(gsl_linear_conv, Tcon, Fcon, ncon);
  gsl_interp_init(gsl_linear_error_conv, Tcon, Fcerrs, ncon);

  yrec_line = array_malloc(nline_data);

// ave
//  taud = exp(theta_best[1]);
//  sigma = exp(theta_best[0]) * sqrt(taud/2.0);

  nq = 2*(flag_detrend + 1);
  Larr = workspace;
  ybuf = Larr + nq*nall_data;
  Cq = ybuf + nall_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;
  yave = ave + nall_data;

  set_covar_mat(theta_best);

  for(i=0; i<nall_data*nall_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }

  for(i=0; i<ncon_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[i*nq+0] = 1.0;
      Larr[i*nq+1] = 0.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
      Larr[i*nq + 2] = 0.0;
      Larr[i*nq + 3] = 0.0;
    }
  }
  for(i=0; i<nline_data; i++)
  {
    if(flag_detrend==0)
    {
      Larr[(i+ncon_data)*nq+0] = 0.0;
      Larr[(i+ncon_data)*nq+1] = 1.0;
    }
    else
    {
      Larr[(i+ncon_data)*nq + 0] = 0.0;
      Larr[(i+ncon_data)*nq + 1] = 0.0;
      Larr[(i+ncon_data)*nq + 2] = 1.0;
      Larr[(i+ncon_data)*nq + 3] = Tline_data[i];
    }
  }

  memcpy(ICmat, Cmat, nall_data*nall_data*sizeof(double));
  inverse_mat(ICmat, nall_data, &info);

  multiply_mat_MN(ICmat, Larr, Tmat1, nall_data, nq, nall_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, nall_data);

  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, nall_data, nall_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, nall_data, nq);
  multiply_mat_MN(Tmat2, Fall_data, ave, nq, 1, nall_data);
  multiply_mat_MN(Larr, ave, yave, nall_data, 1, nq);
  //printf("%f %f\n", ave[0], ave[1]);

  dtau = TF_tau[1] - TF_tau[0];
  fp = fopen("data/line_conv.txt", "w");

  for(i=0; i<nline_data; i++)
  {
    flux = 0.0;
    err = 0.0;
    tline = Tline_data[i];
    for(j=0; j<ntau; j++)
    {
      taup = TF_tau[j];
      tcon = tline - taup;
      if(tcon >=Tcon[0] && tcon <= Tcon[ncon-1])
      {
        fcon = gsl_interp_eval(gsl_linear_conv, Tcon, Fcon, tcon, gsl_acc_conv);
        fcon_err = gsl_interp_eval(gsl_linear_error_conv, Tcon, Fcerrs, tcon, gsl_acc_error_conv);
        flux += fcon * TF[j];
        err += pow(fcon_err * TF[j] * dtau, 2);
      }
    }
    flux *= dtau;
    err = sqrt(err);
    yrec_line[i] = flux + yave[i+ncon_data];
    fprintf(fp, "%f  %e %e\n", tline, yrec_line[i]*scale_line, err*scale_line);
  }
  fclose(fp);

  gsl_interp_free(gsl_linear_conv);
  gsl_interp_free(gsl_linear_error_conv);
  gsl_interp_accel_free(gsl_acc_conv);
  gsl_interp_accel_free(gsl_acc_error_conv);
}