/* stable/stable.h
 *
 * Main header file of Libstable. Contains all declarations of the
 * usable functions in the library.
 *
 * Copyright (C) 2013. Javier Royuela del Val
 *                     Federico Simmross Wattenberg
 *
 * 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; version 3 of the License.
 *
 * 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, see <http://www.gnu.org/licenses/>.
 *
 *
 *  Javier Royuela del Val.
 *  E.T.S.I. Telecomunicación
 *  Universidad de Valladolid
 *  Paseo de Belén 15, 47002 Valladolid, Spain.
 *  jroyval@lpi.tel.uva.es
 */

#ifndef _stable_H_
#define _stable_H_

#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include <gsl/gsl_integration.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_vector.h>

#define TINY 1e-50
#define EPS 2.2204460492503131E-16

#define max(a,b) (((a) > (b)) ? (a) : (b))
#define min(a,b) (((a) < (b)) ? (a) : (b))

/******************************************************************************/
/*          Library parameters                                                */
/******************************************************************************/

extern FILE * FLOG;            // Log file (optional)
extern FILE * FINTEG;          // Integrand evaluations output file (debug purposes)

extern unsigned short THREADS;    // threads of execution (0 => total available)
extern unsigned short IT_MAX;     // Maximum # of iterations in quadrature methods
extern unsigned short SUBS;       // # of integration subintervals
extern unsigned short METHOD1;    // Integration method on main subinterval
extern unsigned short METHOD2;    // Integration method on second subinterval
extern unsigned short METHOD3;    // Integration method on third subinterval

extern unsigned short INV_MAXITER; // Maximum # of iterations inversion method

extern double relTOL;     // Relative error tolerance
extern double absTOL;     // Absolut error tolerance
//extern double FACTOR;   //
extern double ALPHA_TH;    // Alpha threshold
extern double BETA_TH;    // Beta threshold
extern double EXP_MAX;    // Exponent maximum value
extern double XXI_TH;     // Zeta threshold
extern double THETA_TH;   // Theta threshold

extern double AUX1; // Auxiliary values
extern double AUX2;

#ifdef DEBUG
extern unsigned int integ_eval; // # of integrand evaluations
#endif

/******************************************************************************/
/******************************************************************************/
// Particular cases
enum
  {
    NOVALID =-1,
    STABLE,
    ALPHA_1,
    GAUSS ,
    CAUCHY,
    LEVY,
    STABLE_B1,
    ALPHA_1_B1
  };

// Function to evaluate
enum
  {
    CDF,
    PDF
  };

// Quadrature methods
enum
  {
    STABLE_QAG2 = 0,
    STABLE_QUADSTEP,
    STABLE_QROMBPOL,
    STABLE_QROMBRAT,
    STABLE_QNG,
    STABLE_QAG1,
    STABLE_QAG5,
    STABLE_VECT
  };

/************************************************************************
 ************************************************************************
 * Scalar methods                                                       *
 ************************************************************************
 ************************************************************************/
/* Scalar methods are those that, for each thread of execution, obtain a
   single evaluation of the desired function, at a single point.
*/

/******************************************************************************/
/*    Stable distribution structure.                                          */
/******************************************************************************/
struct StableDistStruct
  {
    /* Parameters:
    0-parametrization describen in Nolan, 1997 is employed by default
        alpha : stability index
        beta : skewness parameter
        scale: scale parameter
        mu_0 : 0-parametrization location parameter
        mu_1 : correspondig 1-parametrization location parameter    */
    double alpha;
    double beta;
    double sigma;
    double mu_0;
    double mu_1;

    /* Particular cases indicator (Gauss, Cauchy, Levy distribution, alpha==1, etc.) */
    int ZONE;

    /* Pointers to pdf and cdf evaluation functions */
    double(*stable_pdf_point)(struct StableDistStruct *, const double, double *);
    double(*stable_cdf_point)(struct StableDistStruct *, const double, double *);

    /* Precalculated values. */
    double alphainvalpha1;  /* alpha/(alpha-1)*/
    double xi;            /* -beta*tan(alpha*pi/2)*/
    double theta0;        /* 1/alpha*atan(beta*(tan(alpha*pi/2))=atan(-xi)/alpha;*/
    double c1, c2_part, c3;  /* additive and multiplicative constants*/
    double k1;     /* cos(alpha*theta0)^(1/(alpha-1)) = (1+xi^2)^(-0.5/(alpha-1));*/
    double S;     /* (1+xi^2)^(1/(2*alpha));*/
    double Vbeta1; /*pow(1/dist->alpha,dist->alphainvalpha1) *
                     (dist->alpha-1)*pow(-cos(dist->alpha*PI_2),1/(dist->alpha-1))*/

    /* These ones change from point to point of evaluation */
    double theta0_; /* theta0_ = +-theta0 */
    double beta_;
    double xxipow;  /* (x-xi)^(alpha/(alpha-1))*/

    /* gsl integration workspace */
    gsl_integration_workspace * gslworkspace;

    /* gsl random numbers generator */
    gsl_rng * gslrand;
  };

typedef struct StableDistStruct StableDist;
/******************************************************************************/
/*        Auxiliary functions                                                 */
/******************************************************************************/
unsigned int stable_get_THREADS();
void   stable_set_THREADS(unsigned int threads);

unsigned int stable_get_IT_MAX();
void   stable_set_IT_MAX(unsigned int itmax);

unsigned int stable_get_INV_MAXITER();
void   stable_set_INV_MAXITER(unsigned int invmaxiter);

int stable_get_METHOD1();
void stable_set_METHOD1(int method);

int stable_get_METHOD2();
void stable_set_METHOD2(int method);

int stable_get_METHOD3();
void stable_set_METHOD3(int method);

double stable_get_relTOL();
void   stable_set_relTOL(double reltol);

double stable_get_absTOL();
void   stable_set_absTOL(double abstol);

double stable_get_ALPHA_TH();
void   stable_set_ALPHA_TH(double alphath);

double stable_get_BETA_TH();
void   stable_set_BETA_TH(double betath);

double stable_get_XXI_TH();
void   stable_set_XXI_TH(double xxith);

double stable_get_THETA_TH();
void   stable_set_THETA_TH(double thetath);

FILE * stable_get_FINTEG();
FILE * stable_set_FINTEG(char * filename);

FILE * stable_get_FLOG();
FILE * stable_set_FLOG(char * filename);

StableDist *stable_create(double alpha, double beta, double sigma, double mu,
                          int parametrization);

StableDist *stable_copy(StableDist *src_dist);

void stable_free(StableDist *dist);

int stable_setparams(StableDist *dist,
                     double alpha, double beta, double sigma, double mu,
                     int parametrization);

int stable_checkparams(double alpha, double beta, double sigma, double mu,
                       int parametrization);

void error_handler (const char * reason, const char * file,
               int line, int gsl_errno);

/******************************************************************************/
/*   PDF in particular cases                                                  */
/******************************************************************************/

double stable_pdf_point_GAUSS(StableDist *dist, const double x, double *err);

double stable_pdf_point_CAUCHY(StableDist *dist, const double x, double *err);

double stable_pdf_point_LEVY(StableDist *dist, const double x, double *err);

/******************************************************************************/
/*   PDF otherwise                                                            */
/******************************************************************************/

double stable_pdf_point_STABLE(StableDist *dist, const double x, double *err);

double stable_pdf_point_ALPHA_1(StableDist *dist, const double x, double *err);

double stable_pdf_point(StableDist *dist, const double x, double *err);

void stable_pdf(StableDist *dist, const double* x, const unsigned int Nx,
                double *pdf, double *err);

/******************************************************************************/
/*   PDF integrand functions                                                  */
/******************************************************************************/

double stable_pdf_g(double theta, void *dist);
double stable_pdf_g_aux1(double theta, void *args);
double stable_pdf_g_aux2(double theta, void *args);

/******************************************************************************/
/*   CDF in particular cases                                                  */
/******************************************************************************/

double stable_cdf_point_GAUSS(StableDist *dist, const double x, double *err);

double stable_cdf_point_CAUCHY(StableDist *dist, const double x, double *err);

double stable_cdf_point_LEVY(StableDist *dist, const double x, double *err);

/******************************************************************************/
/*   CDF otherwise                                                            */
/******************************************************************************/

double stable_cdf_point_STABLE(StableDist *dist, const double x, double *err);

double stable_cdf_point_ALPHA_1(StableDist *dist, const double x, double *err);

double stable_cdf_point(StableDist *dist, const double x, double *err);

void stable_cdf(StableDist *dist, const double* x, const unsigned int Nx,
                double *cdf, double *err);

/******************************************************************************/
/*   CDF integrad functions                                                   */
/******************************************************************************/

double stable_cdf_g(double theta, void *dist);

/******************************************************************************/
/*   CDF^{-1} (quantiles)                                                     */
/******************************************************************************/

double stable_q_point(StableDist * dist, const double q, double * err);
void   stable_q(StableDist *dist, const double* q, const unsigned int Nq,
                double * inv, double * err);

/************************************************************************
 ************************************************************************
 * Vectorial methods                                                    *
 ************************************************************************
 ************************************************************************/
/*
  Alternative non-parallelized methods of evaluation have been implemented.
  These methods exploit the fact that some calculations are shared between
  different points of evaluation when evaluating the PDF or CDF, so these
  calculations can be realized just once.

  The performance achieved is high, sometimes comparable with parallel
  methods when little precision is required. However, achievable precision
  with these methods is low and non desired behavior of the PDF and CDF
  evaluation is observed.
*/

/* Stable distribution structure for vectorial methods*/

typedef struct
  {
/* Parameters:
    0-parametrization describen in Nolan, 1997 is employed by default
        alpha : stability index
        beta : skewness parameter
        scale: scale parameter
        mu_0 : 0-parametrization location parameter
        mu_1 : correspondig 1-parametrization location parameter    */
    double alpha;
    double beta;
    double sigma;
    double mu_0;
    double mu_1;

    /* Particular cases indicator (Gauss, Cauchy, Levy distribution, alpha==1, etc.) */
    int ZONE;

    /* Precalculated values */
    double alphainvalpha1;     /* alpha/(alpha-1)*/
    double xi;               /* -beta*tan(alpha*pi/2)*/
    double theta0;           /* 1/alpha*atan(beta*(tan(alpha*pi/2))=atan(-xi)/alpha;*/
    double c1, c2_part, c3;  /* additive and multiplicative constants*/
    double k1;               /* cos(alpha*theta0)^(1/(alpha-1)) = (1+xi^2)^(-0.5/(alpha-1));*/
    double S;                /* (1+xi^2)^(1/(2*alpha));*/
    double Vbeta1;           /*pow(1/dist->alpha,dist->alphainvalpha1) *
                                   (dist->alpha-1)*pow(-cos(dist->alpha*PI_2),1/(dist->alpha-1))*/

     /* These ones change from point to point of evaluation */
    double theta0_; /* theta0_ = +-theta0 */
    double beta_;
    double *xxipow;  /* (x-xi)^(alpha/(alpha-1))*/

    gsl_integration_workspace * gslworkspace;
    gsl_rng * gslrand;
  }
StableDistV;

typedef struct
  {
    double (*ptr_funcion)(StableDist *dist, const double x, double *err);
    StableDist *dist;
    const double *x;
    int Nx;
    double *pdf;
    double *err;
  }
StableArgsPdf;

typedef struct
  {
    double (*ptr_funcion)(StableDist *dist, const double x, double *err);
    StableDist *dist;
    const double *x;
    int Nx;
    double *cdf;
    double *err;
  }
StableArgsCdf;

/************************************************************************
 ************************************************************************
 * Parameter estimation                                                 *
 ************************************************************************
 ************************************************************************/

/******************************************************************************/
/*        Parameter estimation structure                                      */
/******************************************************************************/
typedef struct
  {
    StableDist *dist;
    double *data;
    unsigned int length;
    double nu_c;
    double nu_z;
  }
stable_like_params;

/* Estimation functions */

void stable_fit_init(StableDist *dist, const double *data,
             const unsigned int length,  double *nu_c,double *nu_z);

int stable_fit_koutrouvelis(StableDist *dist, const double *data, const unsigned int length);

int stable_fit(StableDist *dist, const double *data, const unsigned int length);

int stable_fit_mle(StableDist *dist, const double *data, const unsigned int length);

int stable_fit_mle2d(StableDist *dist, const double *data, const unsigned int length);

int stable_fit_whole(StableDist *dist, const double *data, const unsigned int length);

/* Auxiliary functions */

gsl_complex stable_samplecharfunc_point(const double* x,
             const unsigned int N, double t);

void stable_samplecharfunc(const double* x, const unsigned int Nx,
             const double* t, const unsigned int Nt, gsl_complex * z);

void stable_fft(double *data, const unsigned int length, double * y);

double stable_loglikelihood(StableDist *dist, double *data, const unsigned int length);

//stable_like_params

int stable_fit_iter_whole(StableDist *dist, const double * data, const unsigned int length);

int stable_fit_iter(StableDist *dist, const double * data,
             const unsigned int length, const double nu_c, const double nu_z);

double stable_loglike_p(stable_like_params *params);

double stable_minusloglikelihood(const gsl_vector * theta, void * p);

/************************************************************************
 ************************************************************************
 * Random numbers generation                                            *
 ************************************************************************
 ************************************************************************/
void stable_rnd(StableDist *dist, double* rnd, unsigned int n);

double stable_rnd_point(StableDist *dist);

void stable_rnd_seed(StableDist * dist, unsigned long int s);
#endif //stable_H


#ifndef CWANNIER_BAND_ENERGY
#define CWANNIER_BAND_ENERGY

#include <stdbool.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include "HTightBinding.h"
#include "ctetra/sum.h"

double BandEnergy(double *E_Fermi, HTightBinding *Hrs, gsl_matrix *R, double num_electrons, int na, int nb, int nc, bool use_cache);

#endif // CWANNIER_BAND_ENERGY


/**
 * @file sad.h
 * @brief Voice activity detection.
 * @author Kenichi Kumatani and John McDonough
 */

#ifndef SAD_H
#define SAD_H

#include <stdio.h>
#include <assert.h>
#define _LOG_SAD_
#ifdef _LOG_SAD_
#include <stdarg.h>
#endif /* _LOG_SAD_ */

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_fit.h>

#include "common/jexception.h"
#include "stream/stream.h"
#include "feature/feature.h"
#include "sad/neural_spnsp_incl.h"

// ----- definition for abstract base class `NeuralNetVAD' -----
//
class NeuralNetVAD {
public:
  NeuralNetVAD(VectorFloatFeatureStreamPtr& cep,
				  unsigned context = 4, unsigned hiddenUnitsN = 1000, unsigned outputUnitsN = 2, float threshold = 0.1, const String& neuralNetFile = "");
  ~NeuralNetVAD();

  bool next(int frame_no = -5);
  void reset();
  void read(const String& neuralNetFile);

private:
  void shift_down_();
  void increment_() { frame_no_++; }
  void update_buffer_(int frame_no);

  const VectorFloatFeatureStreamPtr			cep_;
  const int						frame_reset_no_;
  const unsigned					cepLen_;
  bool							is_speech_;
  int							frame_no_;
  unsigned						framesPadded_;
  unsigned						context_;
  unsigned						hiddenUnitsN_;
  unsigned						outputUnitsN_;
  float							threshold_;
  MLP*							mlp_;
  float**						frame_;
};

typedef refcount_ptr<NeuralNetVAD> NeuralNetVADPtr;

// ----- definition for abstract base class `VAD' -----
//
class VAD {
 public:
  VAD(VectorComplexFeatureStreamPtr& samp);
  ~VAD();

  virtual bool next(int frame_no = -5) = 0;
  virtual void reset() { frame_no_ = frame_reset_no_; }
  const gsl_vector_complex* frame() const { return frame_; }
  virtual void next_speaker() = 0;

 protected:
  void increment_() { frame_no_++; }

  const VectorComplexFeatureStreamPtr			samp_;
  const int						frame_reset_no_;
  const unsigned					fftLen_;
  bool							is_speech_;
  int							frame_no_;
  gsl_vector_complex*					frame_;
};

typedef refcount_ptr<VAD> VADPtr;

// ----- definition for class `EnergyVAD' -----
//
class SimpleEnergyVAD : public VAD {
 public:
  SimpleEnergyVAD(VectorComplexFeatureStreamPtr& samp,
			       double threshold, double gamma = 0.995);
  ~SimpleEnergyVAD();

  virtual bool next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
#endif

 private:
  const double						threshold_;
  const double						gamma_;
  double						spectral_energy_;
};

typedef Inherit<SimpleEnergyVAD, VADPtr> SimpleEnergyVADPtr;

// ----- definition for class `SimpleLikelihoodRatioVAD' -----
//
class SimpleLikelihoodRatioVAD : public VAD {
 public:
  SimpleLikelihoodRatioVAD(VectorComplexFeatureStreamPtr& samp,
					double threshold, double alpha);
  ~SimpleLikelihoodRatioVAD();

  virtual bool next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  void set_variance(const gsl_vector* variance);

#ifdef ENABLE_LEGACY_BTK_API__
  void setVariance(const gsl_vector* variance);
  void nextSpeaker(){ next_speaker(); }
#endif

 private:
  double calc_Ak_(double vk, double gammak, double Rk);

  bool							variance_set_;
  gsl_vector*						noise_variance_;
  gsl_vector*						prev_Ak_;
  gsl_vector_complex*					prev_frame_;
  double						threshold_;
  double						alpha_;
};

typedef Inherit<SimpleLikelihoodRatioVAD, VADPtr> SimpleLikelihoodRatioVADPtr;

// ----- definition for class `EnergyVADFeature' -----
//
class EnergyVADFeature : public VectorFloatFeatureStream {
 public:
  EnergyVADFeature(const VectorFloatFeatureStreamPtr& source, double threshold = 0.5, unsigned bufferLength = 30, unsigned energiesN = 200, const String& nm = "Energy VAD");
  virtual ~EnergyVADFeature();

  virtual const gsl_vector_float* next(int frame_no = -5);
  virtual void reset();
  void next_speaker();

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
#endif

 private:
  static int comparator_(const void* elem1, const void* elem2);
  virtual bool above_threshold_(const gsl_vector_float* vector);

  VectorFloatFeatureStreamPtr			source_;
  bool						recognizing_;

  gsl_vector_float**				buffer_;
  const unsigned				bufferLen_;
  int						bufferX_;
  unsigned					bufferedN_;

  unsigned					abovethresholdN_;
  unsigned					belowThresholdN_;

  unsigned					energiesN_;
  double*					energies_;
  double*					sorted_energies_;
  const unsigned				medianX_;
};

typedef Inherit<EnergyVADFeature, VectorFloatFeatureStreamPtr> EnergyVADFeaturePtr;

// ----- definition for abstract base class `VADMetric' -----
//
class VADMetric :  public Countable {
public:
  VADMetric();
  ~VADMetric();

  virtual double next(int frame_no = -5) = 0;
  virtual void reset() = 0;
  double score(){ return cur_score_;}
  virtual void next_speaker() = 0;

#ifdef  _LOG_SAD_
  bool openLogFile( const String & logfilename );
  int  writeLog( const char *format, ... );
  void closeLogFile();
  void initScore(){ _score=0.0; _scoreX=0; }
  void setScore(double score){ _score=score; _scoreX++; }
  double getAverageScore(){ if(_scoreX==0){return 0;} return(_score/_scoreX); }

  int   frame_no_;
#endif /* _LOG_SAD_ */

protected:
  double                                        cur_score_;

#ifdef  _LOG_SAD_
private:
  FILE *logfp_;
  double _score;
  unsigned _scoreX;
#endif /* _LOG_SAD_ */
};

typedef refcountable_ptr<VADMetric> VADMetricPtr;

// ----- definition for class `EnergyVADMetric' -----
//
class EnergyVADMetric : public VADMetric {
public:
  EnergyVADMetric(const VectorFloatFeatureStreamPtr& source, double initialEnergy = 5.0e+07, double threshold = 0.5, unsigned headN = 4,
		  unsigned tailN = 10, unsigned energiesN = 200, const String& nm = "Energy VAD Metric");

  ~EnergyVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  double energy_percentile(double percentile = 50.0) const;

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
  double energyPercentile(double percentile = 50.0){ return energy_percentile(percentile); }
#endif

private:
  static int comparator_(const void* elem1, const void* elem2);
  virtual bool above_threshold_(const gsl_vector_float* vector);

  VectorFloatFeatureStreamPtr			source_;
  const double					initial_energy_;

  const unsigned				headN_;
  const unsigned				tailN_;
  bool						recognizing_;

  unsigned					abovethresholdN_;
  unsigned					belowThresholdN_;

  unsigned					energiesN_;
  double*					energies_;
  double*					sorted_energies_;
  const unsigned				medianX_;
};

typedef Inherit<EnergyVADMetric, VADMetricPtr> EnergyVADMetricPtr;

// ----- definition for class `MultiChannelVADMetric' -----
//
template <typename ChannelType>
class MultiChannelVADMetric : public VADMetric {
 public:
  MultiChannelVADMetric(unsigned fftLen,
			double sampleRate, double lowCutoff, double highCutoff, const String& nm);
  ~MultiChannelVADMetric();

  void set_channel(ChannelType& chan);

#ifdef ENABLE_LEGACY_BTK_API__
  void setChannel(ChannelType& chan){ set_channel(chan); }
#endif

protected:
  unsigned set_lowX_(double lowCutoff) const;
  unsigned set_highX_(double highCutoff) const;
  unsigned set_binN_() const;

  typedef list<ChannelType>      	ChannelList_;
  ChannelList_				_channelList;

  const unsigned			fftLen_;
  const unsigned			fftLen2_;
  const double				samplerate_;
  const unsigned			lowX_;
  const unsigned			highX_;
  const unsigned			binN_;
  FILE                                 *logfp_;
};

template<> MultiChannelVADMetric<VectorFloatFeatureStreamPtr>::MultiChannelVADMetric(unsigned fftLen, double sampleRate, double lowCutoff, double highCutoff, const String& nm);
template<> MultiChannelVADMetric<VectorComplexFeatureStreamPtr>::MultiChannelVADMetric(unsigned fftLen, double sampleRate, double lowCutoff, double highCutoff, const String& nm);
template<> MultiChannelVADMetric<VectorFloatFeatureStreamPtr>::~MultiChannelVADMetric();
template<> MultiChannelVADMetric<VectorComplexFeatureStreamPtr>::~MultiChannelVADMetric();
template<> void MultiChannelVADMetric<VectorFloatFeatureStreamPtr>::set_channel(VectorFloatFeatureStreamPtr& chan);
template<> void MultiChannelVADMetric<VectorComplexFeatureStreamPtr>::set_channel(VectorComplexFeatureStreamPtr& chan);

typedef MultiChannelVADMetric<VectorFloatFeatureStreamPtr>   FloatMultiChannelVADMetric;
typedef MultiChannelVADMetric<VectorComplexFeatureStreamPtr> ComplexMultiChannelVADMetric;
typedef refcountable_ptr<FloatMultiChannelVADMetric>         FloatMultiChannelVADMetricPtr;
typedef refcountable_ptr<ComplexMultiChannelVADMetric>       ComplexMultiChannelVADMetricPtr;

// ----- definition for class `PowerSpectrumVADMetric' -----
//
/**
   @brief detect voice activity based on the energy comparison
   @usage
   1. construct the object with PowerSpectrumVADMetric().
   2. set the channel data with set_channel().
   3. call next().
   @note the first channel is associated with the target speaker.
 */
class PowerSpectrumVADMetric : public FloatMultiChannelVADMetric {
public:
  PowerSpectrumVADMetric(unsigned fftLen,
			 double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
			 const String& nm = "Power Spectrum VAD Metric");
  PowerSpectrumVADMetric(VectorFloatFeatureStreamPtr& source1, VectorFloatFeatureStreamPtr& source2,
			 double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
			 const String& nm = "Power Spectrum VAD Metric");

  ~PowerSpectrumVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  gsl_vector *get_metrics() const { return powerList_;}
  void set_E0( double E0 ){ E0_ = E0;}
  void clear_channel();

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
  gsl_vector *getMetrics(){ return get_metrics(); }
  void setE0( double E0 ){ set_E0(E0); }
  void clearChannel(){ clear_channel(); }
#endif

protected:
  typedef ChannelList_::iterator		ChannelIterator_;
  gsl_vector                                   *powerList_;
  double                                        E0_;
};

typedef Inherit<PowerSpectrumVADMetric, FloatMultiChannelVADMetricPtr> PowerSpectrumVADMetricPtr;

// ----- definition for class `NormalizedEnergyMetric' -----
//

/**
   @class

   @usage
   @note
*/

class NormalizedEnergyMetric : public  PowerSpectrumVADMetric {
  static double initial_energy_;
public:
  NormalizedEnergyMetric(unsigned fftLen,
		double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
		const String& nm = "TSPS VAD Metric");

  ~NormalizedEnergyMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
};

typedef Inherit<NormalizedEnergyMetric, PowerSpectrumVADMetricPtr> NormalizedEnergyMetricPtr;

// ----- definition for class `CCCVADMetric' -----
//

/**
   @class compute cross-correlation coefficients (CCC) as a function of time delays,
          and average up the n-best values.
   @usage
   @note
*/

class CCCVADMetric : public ComplexMultiChannelVADMetric {
public:
  CCCVADMetric(unsigned fftLen, unsigned nCand,
	       double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
	       const String& nm = "CCC VAD Metric");
  ~CCCVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  void set_NCand(unsigned nCand);
  void set_threshold(double threshold){ threshold_ = threshold;}
  gsl_vector *get_metrics() const { return ccList_;}
  void clear_channel();

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
  void setNCand(unsigned nCand){ set_NCand(nCand); }
  void setThreshold(double threshold){ set_threshold(threshold); }
  gsl_vector *getMetrics(){ return get_metrics(); }
  void clearChannel(){ clear_channel(); }
#endif

protected:
  typedef ChannelList_::iterator		ChannelIterator_;
  unsigned                                      nCand_;
  gsl_vector                                   *ccList_;
  gsl_vector_int                               *sample_delays_;
  double                                        threshold_;
  double                                       *pack_cross_spectrum_;
};

typedef Inherit<CCCVADMetric, ComplexMultiChannelVADMetricPtr> CCCVADMetricPtr;

// ----- definition for class `TSPSVADMetric' -----
//

/**
   @class

   @usage
   @note
*/

class TSPSVADMetric : public  PowerSpectrumVADMetric {
  static double initial_energy_;
public:
  TSPSVADMetric(unsigned fftLen,
		double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
		const String& nm = "TSPS VAD Metric");
  ~TSPSVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
};

typedef Inherit<TSPSVADMetric,PowerSpectrumVADMetricPtr> TSPSVADMetricPtr;

// ----- definition for class `NegentropyVADMetric' -----
//
class NegentropyVADMetric : public VADMetric {

protected:
  class ComplexGeneralizedGaussian_ : public Countable {
  public:
    ComplexGeneralizedGaussian_(double shapeFactor = 2.0);
    double logLhood(gsl_complex X, double scaleFactor) const;
    double shapeFactor() const { return shape_factor_; }
    double Bc() const { return Bc_; }
    double normalization() const { return normalization_; }

  protected:
    virtual double calc_Bc_() const;
    virtual double calc_normalization_() const;

    const double				shape_factor_;
    /* const */ double				Bc_;
    /* const */ double				normalization_;
  };

  typedef refcountable_ptr<ComplexGeneralizedGaussian_> ComplexGeneralizedGaussianPtr_;

  typedef list<ComplexGeneralizedGaussianPtr_>	GaussianList_;
  typedef GaussianList_::iterator		GaussianListIterator_;
  typedef GaussianList_::const_iterator		GaussianListConstIterator_;

public:
  NegentropyVADMetric(const VectorComplexFeatureStreamPtr& source, const VectorFloatFeatureStreamPtr& spectralEstimator,
		      const String& shapeFactorFileName = "", double threshold = 0.5,
		      double sampleRate = 16000.0, double lowCutoff = -1.0, double highCutoff = -1.0,
		      const String& nm = "Negentropy VAD Metric");

  ~NegentropyVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  double calc_negentropy(int frame_no);

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
  double calcNegentropy(int frame_no){ return calc_negentropy(frame_no); }
#endif

protected:
  virtual bool above_threshold_(int frame_no);
  unsigned set_lowX_(double lowCutoff) const;
  unsigned set_highX_(double highCutoff) const;
  unsigned set_binN_() const;

  VectorComplexFeatureStreamPtr			source_;
  VectorFloatFeatureStreamPtr			spectral_estimator_;

  GaussianList_					generalized_gaussians_;
  ComplexGeneralizedGaussianPtr_		gaussian_;

  const double					threshold_;

  const unsigned				fftLen_;
  const unsigned				fftLen2_;

  const double					samplerate_;
  const unsigned				lowX_;
  const unsigned				highX_;
  const unsigned				binN_;
};

typedef Inherit<NegentropyVADMetric, VADMetricPtr> NegentropyVADMetricPtr;

// ----- definition for class `MutualInformationVADMetric' -----
//
class MutualInformationVADMetric : public NegentropyVADMetric {
 public:
  MutualInformationVADMetric(const VectorComplexFeatureStreamPtr& source1,
                             const VectorComplexFeatureStreamPtr& source2,
                             const VectorFloatFeatureStreamPtr& spectralEstimator1,
                             const VectorFloatFeatureStreamPtr& spectralEstimator2,
                             const String& shapeFactorFileName = "",
                             double twiddle = -1.0,
                             double threshold = 1.3,
                             double beta = 0.95,
                             double sampleRate = 16000.0,
                             double lowCutoff = 187.0,
                             double highCutoff = 1000.0,
                             const String& nm = "Mutual Information VAD Metric");
  ~MutualInformationVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  double calc_mutual_information(int frame_no);

#ifdef ENABLE_LEGACY_BTK_API__
  double calcMutualInformation(int frame_no){ return calc_mutual_information(frame_no); }
#endif

 protected:
  class JointComplexGeneralizedGaussian_ : public NegentropyVADMetric::ComplexGeneralizedGaussian_ {
 public:
   JointComplexGeneralizedGaussian_(const NegentropyVADMetric::ComplexGeneralizedGaussianPtr_& ggaussian);
   ~JointComplexGeneralizedGaussian_();

   double logLhood(gsl_complex X1, gsl_complex X2, double scaleFactor1, double scaleFactor2, gsl_complex rho12) const;

 private:
   static const double      sqrt_two_;
   static const gsl_complex complex_one_;
   static const gsl_complex complex_zero_;

   virtual double calc_Bc_() const;
   virtual double calc_normalization_() const;

   double lngamma_ratio_(double f) const;
   double lngamma_ratio_joint_(double f) const;
   double match_(double f) const;

   double match_score_marginal_(double f) const;
   double match_score_joint_(double fJ) const;

   static const double				tolerance_;

   gsl_vector_complex*				X_;
   gsl_vector_complex*				scratch_;
   gsl_matrix_complex*				SigmaX_inverse_;
 };

 protected:
 typedef refcountable_ptr<JointComplexGeneralizedGaussian_> JointComplexGeneralizedGaussianPtr_;

 typedef list<JointComplexGeneralizedGaussianPtr_>	JointGaussianList_;
 typedef JointGaussianList_::iterator			JointGaussianListIterator_;
 typedef JointGaussianList_::const_iterator		JointGaussianListConstIterator_;

 typedef vector<gsl_complex>				CrossCorrelationVector_;

 static const double				epsilon_;

 virtual bool above_threshold_(int frame_no);
 double calc_fixed_threshold_();
 double calc_total_threshold_() const;
 void initialize_pdfs_();

 VectorComplexFeatureStreamPtr			source2_;
 VectorFloatFeatureStreamPtr			spectral_estimator2_;

 JointGaussianList_				joint_generalized_gaussians_;
 CrossCorrelationVector_			ccs_; // cross correlations
 const double					fixed_threshold_;
 const double					twiddle_;
 const double					threshold_;
 const double					beta_;
};

typedef Inherit<MutualInformationVADMetric, NegentropyVADMetricPtr> MutualInformationVADMetricPtr;

// ----- definition for class `LikelihoodRatioVADMetric' -----
//
class LikelihoodRatioVADMetric : public NegentropyVADMetric {
 public:
  LikelihoodRatioVADMetric(const VectorComplexFeatureStreamPtr& source1, const VectorComplexFeatureStreamPtr& source2,
			   const VectorFloatFeatureStreamPtr& spectralEstimator1, const VectorFloatFeatureStreamPtr& spectralEstimator2,
			   const String& shapeFactorFileName = "", double threshold = 0.0,
			   double sampleRate = 16000.0, double lowCutoff = 187.0, double highCutoff = 1000.0,
			   const String& nm = "Likelihood VAD Metric");
  ~LikelihoodRatioVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  double calc_likelihood_ratio(int frame_no);

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
  double calcLikelihoodRatio(int frame_no){ return calc_likelihood_ratio(frame_no); }
#endif

 private:
  const VectorComplexFeatureStreamPtr			source2_;
  const VectorFloatFeatureStreamPtr			spectral_estimator2_;
};

typedef Inherit<LikelihoodRatioVADMetric, NegentropyVADMetricPtr> LikelihoodRatioVADMetricPtr;

// ----- definition for class `LowFullBandEnergyRatioVADMetric' -----
//
class LowFullBandEnergyRatioVADMetric : public VADMetric {
public:
  LowFullBandEnergyRatioVADMetric(const VectorFloatFeatureStreamPtr& source, const gsl_vector* lowpass, double threshold = 0.5, const String& nm = "Low- Full-Band Energy Ratio VAD Metric");
  ~LowFullBandEnergyRatioVADMetric();

  virtual double next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
#endif

private:
  virtual bool above_threshold_(int frame_no);
  void calc_auto_correlation_vector_(int frame_no);
  void calc_covariance_matrix_();
  double calc_lower_band_energy_();

  VectorFloatFeatureStreamPtr			source_;
  const unsigned				_lagsN;
  gsl_vector*					_lowpass;
  gsl_vector*					scratch_;
  double*					_autocorrelation;
  gsl_matrix*					_covariance;
};

typedef Inherit<LowFullBandEnergyRatioVADMetric, VADMetricPtr> LowFullBandEnergyRatioVADMetricPtr;

// ----- definition for class `HangoverVADFeature' -----
//
class HangoverVADFeature : public VectorFloatFeatureStream {
 public:
  HangoverVADFeature(const VectorFloatFeatureStreamPtr& source, const VADMetricPtr& metric, double threshold = 0.5,
		     unsigned headN = 4, unsigned tailN = 10, const String& nm = "Hangover VAD Feature");
  virtual ~HangoverVADFeature();

  virtual const gsl_vector_float* next(int frame_no = -5);
  virtual void reset();
  virtual void next_speaker();
  int prefixN() const { return prefixN_ - headN_; }

#ifdef ENABLE_LEGACY_BTK_API__
  void nextSpeaker(){ next_speaker(); }
#endif

 protected:
  typedef pair<VADMetricPtr, double>		MetricPair_;
  typedef vector<MetricPair_>			MetricList_;
  typedef MetricList_::iterator			MetricListIterator_;
  typedef MetricList_::const_iterator		MetricListConstIterator_;

  static const unsigned EnergyVADMetricX		= 0;
  static const unsigned MutualInformationVADMetricX	= 1;
  static const unsigned LikelihoodRatioVADMetricX	= 2;

  static int comparator_(const void* elem1, const void* elem2);
  virtual bool above_threshold_(int frame_no);

  VectorFloatFeatureStreamPtr			source_;
  bool						recognizing_;

  gsl_vector_float**				buffer_;
  const unsigned				headN_;
  const unsigned				tailN_;
  int						bufferX_;
  unsigned					bufferedN_;

  unsigned					abovethresholdN_;
  unsigned					belowThresholdN_;
  unsigned					prefixN_;

  MetricList_					metricList_;
};

typedef Inherit<HangoverVADFeature, VectorFloatFeatureStreamPtr> HangoverVADFeaturePtr;

// ----- definition for class `HangoverMIVADFeature' -----
//
class HangoverMIVADFeature : public HangoverVADFeature {
  static const unsigned EnergyVADMetricX		= 0;
  static const unsigned MutualInformationVADMetricX	= 1;
  static const unsigned PowerVADMetricX			= 2;

 public:
  HangoverMIVADFeature(const VectorFloatFeatureStreamPtr& source,
		       const VADMetricPtr& energyMetric, const VADMetricPtr& mutualInformationMetric, const VADMetricPtr& powerMetric,
		       double energythreshold = 0.5, double mutualInformationThreshold = 0.5, double powerThreshold = 0.5,
		       unsigned headN = 4, unsigned tailN = 10, const String& nm = "Hangover VAD Feature");

  int decision_metric() const { return decision_metric_; }

#ifdef ENABLE_LEGACY_BTK_API__
  int decisionMetric() { return decision_metric(); }
#endif

protected:
  virtual bool above_threshold_(int frame_no);

  int						decision_metric_;
};

typedef Inherit<HangoverMIVADFeature, HangoverVADFeaturePtr> HangoverMIVADFeaturePtr;

// ----- definition for class `HangoverMultiStageVADFeature' -----
//
class HangoverMultiStageVADFeature : public HangoverVADFeature {
 public:
  HangoverMultiStageVADFeature(const VectorFloatFeatureStreamPtr& source,
			       const VADMetricPtr& energyMetric, double energythreshold = 0.5,
			       unsigned headN = 4, unsigned tailN = 10, const String& nm = "HangoverMultiStageVADFeature");
  ~HangoverMultiStageVADFeature();
  int decision_metric() const { return decision_metric_; }
  void set_metric( const VADMetricPtr& metricPtr, double threshold ){
    metricList_.push_back( MetricPair_( metricPtr, threshold ) );
  }

#ifdef ENABLE_LEGACY_BTK_API__
  int decisionMetric() { return decision_metric(); }
  void setMetric( const VADMetricPtr& metricPtr, double threshold ){ set_metric(metricPtr, threshold); }
#endif

#ifdef _LOG_SAD_
  void initScores();
  gsl_vector *getScores();
#endif /* _LOG_SAD_ */

protected:
  virtual bool above_threshold_(int frame_no);
  int          decision_metric_;
#ifdef _LOG_SAD_
  gsl_vector *_scores;
#endif /* _LOG_SAD_ */
};

typedef Inherit<HangoverMultiStageVADFeature, HangoverVADFeaturePtr> HangoverMultiStageVADFeaturePtr;

#endif


/**
 *
 * Copyright 2018, Planet Labs, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdio.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <math.h>

/*  A wrapper around the GSL multi-variate robust regression routine.

    This routine should only ever be called from the Python wrapper function, so if the
    parameters need to be changed, only these two files have to be changed.

    The array variables are given as pointers to doubles, and are referenced as
    1-d arrays. However, the indexes are calculated as though they were multi-dimensional
    arrays, and the dimensions are described below.

    Input Variables
    ***************

    numParams is the number of parameters to be fitted, also equal
        to the number of independent variables
    numImages is the number of images in the stack, and corresponds to the number
        of points in each fit (before removing nulls)
    numRows is the number of rows in the image
    numCols is the number of columns in the image

    The x parameter contains the values of all the independent variables.
    It comes in one of two flavours, depending on the value of the
    perPixelX parameter. If perPixelX is 1, then the x parameter is a 4-dimensional
    array of doubles, with shape
        (numParams, numImages, numRows, numCols)
    otherwise it is a 2-dimensional array with shape
        (numParams, numImages)
    and it is assumed that the independent variables are constant over all pixels.
    This latter is the most likely case.

    The Y variable is notionally a 3-dimensional array of doubles.
    The shape should be
        (numImages, numRows, numCols)
    It contains the values of the dependent variable.

    The nullVal parameter is a scalar double value. Any occurrence of this value in the
    y array will exclude that point from the fit.

    Output Variables
    ****************

    The following variables are calculated within this routine, and passed back via the
    pointers in the parameter list.

    The c array is a 3-dimensional array of doubles. It corresponds to an image
    stack of the fitted coefficients, and its shape is
        (numParams, numRows, numCols)

    The adj_Rsqrd array stores the adjusted R^2 coefficient of determination
        statistic.

*/
void wrap_gsl_multifit_robust(double *x, double *y, double *c, double *adj_Rsqrd,
        int *numIter, double *rmse, int method, int perPixelX,
        int numRows, int numCols, int numImages, int numParams,
        int numRowsX, int numColsX, double nullVal) {
    int row, col, img, param, n, xNdx, yNdx, pixNdx;
    gsl_matrix *gslX, *gslCov;
    gsl_vector *gslY, *gslC;
    gsl_multifit_robust_workspace *workspace;
    gsl_multifit_robust_type *regressionType;
    gsl_multifit_robust_stats stats;
    int gslErrorCode;

    /* Turn off the default error handler, which aborts at the first error. */
    gsl_set_error_handler_off();

    /* Translate the integer type given into the corresponding GSL pointer */
    switch (method) {
        case 1: regressionType = gsl_multifit_robust_bisquare; break;
        case 2: regressionType = gsl_multifit_robust_cauchy; break;
        case 3: regressionType = gsl_multifit_robust_fair; break;
        case 4: regressionType = gsl_multifit_robust_huber; break;
        case 5: regressionType = gsl_multifit_robust_ols; break;
        case 6: regressionType = gsl_multifit_robust_welsch; break;
    }

    /* These structures can be allocated outside the pixel loop */
    gslCov = gsl_matrix_calloc(numParams, numParams);
    gslC = gsl_vector_calloc(numParams);

    /* Loop over all pixels */
    for (row=0; row<numRows; row++) {
        for (col=0; col<numCols; col++) {
            /* Count how many non-null y values we have. */
            n = 0;
            for (img=0; img<numImages; img++) {
                if (y[img*numRows*numCols+row*numCols+col] != nullVal) n++;
            }

            /* Allocate various structures, now we know how many non-nulls */
            if (n >= numParams) {
                workspace = gsl_multifit_robust_alloc(regressionType, n, numParams);
                gslX = gsl_matrix_calloc(n, numParams);
                gslY = gsl_vector_calloc(n);

                /* Copy the data from this pixel into the relevant GSL structures. Note
                   that we skip over null values, based on nulls in the y variable.
                */
                n = 0;
                for (img=0; img<numImages; img++) {
                    yNdx = img*numRows*numCols+row*numCols+col;
                    if (y[yNdx] != nullVal) {
                        gslY->data[n*gslY->stride] = y[yNdx];
                        for (param=0; param<numParams; param++) {
                            if (perPixelX == 0) {
                                xNdx = param * numImages + img;
                            } else {
                                xNdx = param * numRowsX * numColsX * numImages +
                                    img * numRowsX * numColsX + row * numColsX + col;
                            }
                            gslX->data[n*gslX->tda + param] = x[xNdx];
                        }
                        n++;
                    }
                }

                /* Do the regression fit */
                gslErrorCode = gsl_multifit_robust(gslX, gslY, gslC, gslCov, workspace);

                if (gslErrorCode == 0) {
                    /* Copy the coefficients back into the image stack of coefficients */
                    for (param=0; param<numParams; param++) {
                        c[param*numRows*numCols + row*numCols + col] = gslC->data[param*gslC->stride];
                    }

                    /* Copy some useful statistics into their arrays */
                    stats = gsl_multifit_robust_statistics(workspace);
                    pixNdx = row*numCols + col;
                    adj_Rsqrd[pixNdx] = stats.adj_Rsq;
                    numIter[pixNdx] = stats.numit;
                    rmse[pixNdx] = stats.rmse;
                }

                /* Free per-pixel structures */
                gsl_matrix_free(gslX);
                gsl_vector_free(gslY);
                gsl_multifit_robust_free(workspace);
            }
        }
    }

    /* Free the other structures */
    gsl_vector_free(gslC);
    gsl_matrix_free(gslCov);
}


// Copyright 2012 Google Inc. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef UTILS_H_
#define UTILS_H_

#include <gsl/gsl_permutation.h>
#include <gsl/gsl_randist.h>

#include <vector>

using namespace std;

namespace hlda {

// This class provides functionality for summing values,
// for reading data from files and also provides
// an interface to gsl specific methods.
// This class is not thread-safe (see private static gsl_rng* RANDNUMGEN).
class Utils {
 public:
  // Sum up the values in a vector.
  static double Sum(const vector<double>& v);

  // An approximation of the logarithm of a sum.
  // Given log(a) denoted with log_a and log(b) denoted with log_b
  // it approximates log(a + b) as:
  // if log_a < log_b then log(a + b) = log_b + log(1 + exp(log_a - log_b))
  // otherwise log(a + b) = log_a + log(1 + exp(log_b - log_a)).
  static double LogSum(double log_a, double log_b);

  // Sample log probabilities.
  // The log_pr vector keeps for each level
  // the current log probability of the word + the current
  // log probability of the level in the tree.
  // These values are used to sample the new level.
  // The vector should contain at least one element.
  static int SampleFromLogPr(const vector<double>& log_pr);

  // Shuffle the values in a gsl_permutation.
  static void Shuffle(gsl_permutation* permutation, int size);

  // Initialize the random number generator.
  // rng_seed is the random number generator seed.
  static void InitRandomNumberGen(long rng_seed);

  // Return a gsl Gaussian random variate with mean and stdev as parameters
  static double RandGauss(double mean, double stdev);

  // Return a random number using the gsl random number generator.
  static double RandNo();

 private:
  static gsl_rng* RANDNUMGEN;
};

}  // namespace hlda

#endif  // UTILS_H_


//  This matrix class is a C++ wrapper for the GNU Scientific Library
//  Copyright (C)  ULP-IPB Strasbourg

//  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 2 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., 675 Mass Ave, Cambridge, MA 02139, USA.

#ifndef __matrix_vector_operators_h
#define __matrix_vector_operators_h

#include "gsl/gsl_blas.h"
#include <gslwrap/matrix_double.h>
#include <gslwrap/matrix_float.h>
#include <gslwrap/vector_double.h>

namespace gsl
{

inline
vector_float operator*(const matrix_float& m, const vector_float& v)
{
	vector_float y(m.get_rows());
	gsl_blas_sgemv(CblasNoTrans, 1.0, m.gslobj(), v.gslobj(), 0.0, y.gslobj());
	return y;
}

inline
vector operator*(const matrix& m, const vector& v)
{
	vector y(m.get_rows());
	gsl_blas_dgemv(CblasNoTrans, 1.0, m.gslobj(), v.gslobj(), 0.0, y.gslobj());
	return y;
}

}

#endif //__matrix_vector_operators_h


/*
 *  Copyright 2008-2009 NVIDIA Corporation
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

/*! \file defs.h
 *  \brief CBLAS utility definitions for interface routines
 */

#pragma once

#include <cblas.h>

namespace cusp
{
namespace system
{
namespace cpp
{
namespace detail
{
namespace cblas
{

struct cblas_format {};

struct upper   : public cblas_format {};
struct lower   : public cblas_format {};
struct unit    : public cblas_format {};
struct nonunit : public cblas_format {};

struct cblas_row_major { const static CBLAS_ORDER order = CblasRowMajor; };
struct cblas_col_major { const static CBLAS_ORDER order = CblasColMajor; };

template< typename LayoutFormat >
struct Orientation : thrust::detail::eval_if<
                        thrust::detail::is_same<LayoutFormat, cusp::row_major>::value,
                        thrust::detail::identity_<cblas_row_major>,
                        thrust::detail::identity_<cblas_col_major>
                     >
{};

} // end namespace cblas
} // end namespace detail
} // end namespace cpp
} // end namespace system
} // end namespace cusp


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * 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 2 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:45:16 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -hc2hc-backward 16 */

/*
 * This function contains 298 FP additions, 134 FP multiplications,
 * (or, 244 additions, 80 multiplications, 54 fused multiply/add),
 * 49 stack variables, and 128 memory accesses
 */
static const fftw_real K1_961570560 = FFTW_KONST(+1.961570560806460898252364472268478073947867462);
static const fftw_real K390180644 = FFTW_KONST(+0.390180644032256535696569736954044481855383236);
static const fftw_real K1_111140466 = FFTW_KONST(+1.111140466039204449485661627897065748749874382);
static const fftw_real K1_662939224 = FFTW_KONST(+1.662939224605090474157576755235811513477121624);
static const fftw_real K707106781 = FFTW_KONST(+0.707106781186547524400844362104849039284835938);
static const fftw_real K382683432 = FFTW_KONST(+0.382683432365089771728459984030398866761344562);
static const fftw_real K923879532 = FFTW_KONST(+0.923879532511286756128183189396788286822416626);
static const fftw_real K765366864 = FFTW_KONST(+0.765366864730179543456919968060797733522689125);
static const fftw_real K1_847759065 = FFTW_KONST(+1.847759065022573512256366378793576573644833252);
static const fftw_real K1_414213562 = FFTW_KONST(+1.414213562373095048801688724209698078569671875);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_hc2hc_backward_16(fftw_real *A, const fftw_complex *W, int iostride, int m, int dist)
{
     int i;
     fftw_real *X;
     fftw_real *Y;
     X = A;
     Y = A + (16 * iostride);
     {
	  fftw_real tmp279;
	  fftw_real tmp324;
	  fftw_real tmp312;
	  fftw_real tmp299;
	  fftw_real tmp276;
	  fftw_real tmp296;
	  fftw_real tmp309;
	  fftw_real tmp323;
	  fftw_real tmp283;
	  fftw_real tmp291;
	  fftw_real tmp286;
	  fftw_real tmp294;
	  fftw_real tmp301;
	  fftw_real tmp319;
	  fftw_real tmp327;
	  fftw_real tmp326;
	  fftw_real tmp316;
	  fftw_real tmp302;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp277;
	       fftw_real tmp278;
	       fftw_real tmp310;
	       fftw_real tmp297;
	       fftw_real tmp298;
	       fftw_real tmp311;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp277 = X[2 * iostride];
	       tmp278 = X[6 * iostride];
	       tmp310 = tmp277 - tmp278;
	       tmp297 = Y[-2 * iostride];
	       tmp298 = Y[-6 * iostride];
	       tmp311 = tmp298 + tmp297;
	       tmp279 = K2_000000000 * (tmp277 + tmp278);
	       tmp324 = K1_414213562 * (tmp310 + tmp311);
	       tmp312 = K1_414213562 * (tmp310 - tmp311);
	       tmp299 = K2_000000000 * (tmp297 - tmp298);
	  }
	  {
	       fftw_real tmp275;
	       fftw_real tmp308;
	       fftw_real tmp273;
	       fftw_real tmp306;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp274;
		    fftw_real tmp307;
		    fftw_real tmp271;
		    fftw_real tmp272;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp274 = X[4 * iostride];
		    tmp275 = K2_000000000 * tmp274;
		    tmp307 = Y[-4 * iostride];
		    tmp308 = K2_000000000 * tmp307;
		    tmp271 = X[0];
		    tmp272 = X[8 * iostride];
		    tmp273 = tmp271 + tmp272;
		    tmp306 = tmp271 - tmp272;
	       }
	       tmp276 = tmp273 + tmp275;
	       tmp296 = tmp273 - tmp275;
	       tmp309 = tmp306 - tmp308;
	       tmp323 = tmp306 + tmp308;
	  }
	  {
	       fftw_real tmp314;
	       fftw_real tmp318;
	       fftw_real tmp317;
	       fftw_real tmp315;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp281;
		    fftw_real tmp282;
		    fftw_real tmp289;
		    fftw_real tmp290;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp281 = X[iostride];
		    tmp282 = X[7 * iostride];
		    tmp283 = tmp281 + tmp282;
		    tmp314 = tmp281 - tmp282;
		    tmp289 = Y[-iostride];
		    tmp290 = Y[-7 * iostride];
		    tmp291 = tmp289 - tmp290;
		    tmp318 = tmp289 + tmp290;
	       }
	       {
		    fftw_real tmp284;
		    fftw_real tmp285;
		    fftw_real tmp292;
		    fftw_real tmp293;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp284 = X[3 * iostride];
		    tmp285 = X[5 * iostride];
		    tmp286 = tmp284 + tmp285;
		    tmp317 = tmp285 - tmp284;
		    tmp292 = Y[-3 * iostride];
		    tmp293 = Y[-5 * iostride];
		    tmp294 = tmp292 - tmp293;
		    tmp315 = tmp293 + tmp292;
	       }
	       tmp301 = tmp283 - tmp286;
	       tmp319 = tmp317 + tmp318;
	       tmp327 = tmp318 - tmp317;
	       tmp326 = tmp314 + tmp315;
	       tmp316 = tmp314 - tmp315;
	       tmp302 = tmp294 + tmp291;
	  }
	  {
	       fftw_real tmp280;
	       fftw_real tmp287;
	       fftw_real tmp288;
	       fftw_real tmp295;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp280 = tmp276 + tmp279;
	       tmp287 = K2_000000000 * (tmp283 + tmp286);
	       X[8 * iostride] = tmp280 - tmp287;
	       X[0] = tmp280 + tmp287;
	       tmp288 = tmp276 - tmp279;
	       tmp295 = K2_000000000 * (tmp291 - tmp294);
	       X[12 * iostride] = tmp288 + tmp295;
	       X[4 * iostride] = tmp288 - tmp295;
	  }
	  {
	       fftw_real tmp300;
	       fftw_real tmp303;
	       fftw_real tmp304;
	       fftw_real tmp305;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp300 = tmp296 - tmp299;
	       tmp303 = K1_414213562 * (tmp301 - tmp302);
	       X[10 * iostride] = tmp300 - tmp303;
	       X[2 * iostride] = tmp300 + tmp303;
	       tmp304 = tmp296 + tmp299;
	       tmp305 = K1_414213562 * (tmp301 + tmp302);
	       X[6 * iostride] = tmp304 - tmp305;
	       X[14 * iostride] = tmp304 + tmp305;
	  }
	  {
	       fftw_real tmp313;
	       fftw_real tmp320;
	       fftw_real tmp321;
	       fftw_real tmp322;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp313 = tmp309 + tmp312;
	       tmp320 = (K1_847759065 * tmp316) - (K765366864 * tmp319);
	       X[9 * iostride] = tmp313 - tmp320;
	       X[iostride] = tmp313 + tmp320;
	       tmp321 = tmp309 - tmp312;
	       tmp322 = (K765366864 * tmp316) + (K1_847759065 * tmp319);
	       X[5 * iostride] = tmp321 - tmp322;
	       X[13 * iostride] = tmp321 + tmp322;
	  }
	  {
	       fftw_real tmp325;
	       fftw_real tmp328;
	       fftw_real tmp329;
	       fftw_real tmp330;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp325 = tmp323 - tmp324;
	       tmp328 = (K765366864 * tmp326) - (K1_847759065 * tmp327);
	       X[11 * iostride] = tmp325 - tmp328;
	       X[3 * iostride] = tmp325 + tmp328;
	       tmp329 = tmp323 + tmp324;
	       tmp330 = (K1_847759065 * tmp326) + (K765366864 * tmp327);
	       X[7 * iostride] = tmp329 - tmp330;
	       X[15 * iostride] = tmp329 + tmp330;
	  }
     }
     X = X + dist;
     Y = Y - dist;
     for (i = 2; i < m; i = i + 2, X = X + dist, Y = Y - dist, W = W + 15) {
	  fftw_real tmp73;
	  fftw_real tmp98;
	  fftw_real tmp135;
	  fftw_real tmp160;
	  fftw_real tmp182;
	  fftw_real tmp236;
	  fftw_real tmp210;
	  fftw_real tmp248;
	  fftw_real tmp95;
	  fftw_real tmp124;
	  fftw_real tmp138;
	  fftw_real tmp164;
	  fftw_real tmp197;
	  fftw_real tmp216;
	  fftw_real tmp244;
	  fftw_real tmp252;
	  fftw_real tmp80;
	  fftw_real tmp128;
	  fftw_real tmp105;
	  fftw_real tmp161;
	  fftw_real tmp213;
	  fftw_real tmp237;
	  fftw_real tmp189;
	  fftw_real tmp249;
	  fftw_real tmp88;
	  fftw_real tmp115;
	  fftw_real tmp137;
	  fftw_real tmp163;
	  fftw_real tmp204;
	  fftw_real tmp215;
	  fftw_real tmp241;
	  fftw_real tmp251;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp69;
	       fftw_real tmp180;
	       fftw_real tmp131;
	       fftw_real tmp209;
	       fftw_real tmp72;
	       fftw_real tmp208;
	       fftw_real tmp134;
	       fftw_real tmp181;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp67;
		    fftw_real tmp68;
		    fftw_real tmp129;
		    fftw_real tmp130;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp67 = X[0];
		    tmp68 = Y[-8 * iostride];
		    tmp69 = tmp67 + tmp68;
		    tmp180 = tmp67 - tmp68;
		    tmp129 = Y[0];
		    tmp130 = X[8 * iostride];
		    tmp131 = tmp129 - tmp130;
		    tmp209 = tmp129 + tmp130;
	       }
	       {
		    fftw_real tmp70;
		    fftw_real tmp71;
		    fftw_real tmp132;
		    fftw_real tmp133;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp70 = X[4 * iostride];
		    tmp71 = Y[-12 * iostride];
		    tmp72 = tmp70 + tmp71;
		    tmp208 = tmp70 - tmp71;
		    tmp132 = Y[-4 * iostride];
		    tmp133 = X[12 * iostride];
		    tmp134 = tmp132 - tmp133;
		    tmp181 = tmp132 + tmp133;
	       }
	       tmp73 = tmp69 + tmp72;
	       tmp98 = tmp69 - tmp72;
	       tmp135 = tmp131 - tmp134;
	       tmp160 = tmp131 + tmp134;
	       tmp182 = tmp180 - tmp181;
	       tmp236 = tmp180 + tmp181;
	       tmp210 = tmp208 + tmp209;
	       tmp248 = tmp209 - tmp208;
	  }
	  {
	       fftw_real tmp91;
	       fftw_real tmp194;
	       fftw_real tmp119;
	       fftw_real tmp192;
	       fftw_real tmp94;
	       fftw_real tmp191;
	       fftw_real tmp122;
	       fftw_real tmp195;
	       fftw_real tmp116;
	       fftw_real tmp123;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp89;
		    fftw_real tmp90;
		    fftw_real tmp117;
		    fftw_real tmp118;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp89 = Y[-15 * iostride];
		    tmp90 = X[7 * iostride];
		    tmp91 = tmp89 + tmp90;
		    tmp194 = tmp89 - tmp90;
		    tmp117 = Y[-7 * iostride];
		    tmp118 = X[15 * iostride];
		    tmp119 = tmp117 - tmp118;
		    tmp192 = tmp117 + tmp118;
	       }
	       {
		    fftw_real tmp92;
		    fftw_real tmp93;
		    fftw_real tmp120;
		    fftw_real tmp121;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp92 = X[3 * iostride];
		    tmp93 = Y[-11 * iostride];
		    tmp94 = tmp92 + tmp93;
		    tmp191 = tmp92 - tmp93;
		    tmp120 = Y[-3 * iostride];
		    tmp121 = X[11 * iostride];
		    tmp122 = tmp120 - tmp121;
		    tmp195 = tmp120 + tmp121;
	       }
	       tmp95 = tmp91 + tmp94;
	       tmp116 = tmp91 - tmp94;
	       tmp123 = tmp119 - tmp122;
	       tmp124 = tmp116 + tmp123;
	       tmp138 = tmp123 - tmp116;
	       tmp164 = tmp119 + tmp122;
	       {
		    fftw_real tmp193;
		    fftw_real tmp196;
		    fftw_real tmp242;
		    fftw_real tmp243;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp193 = tmp191 - tmp192;
		    tmp196 = tmp194 - tmp195;
		    tmp197 = (K923879532 * tmp193) - (K382683432 * tmp196);
		    tmp216 = (K382683432 * tmp193) + (K923879532 * tmp196);
		    tmp242 = tmp194 + tmp195;
		    tmp243 = tmp191 + tmp192;
		    tmp244 = (K382683432 * tmp242) - (K923879532 * tmp243);
		    tmp252 = (K382683432 * tmp243) + (K923879532 * tmp242);
	       }
	  }
	  {
	       fftw_real tmp76;
	       fftw_real tmp183;
	       fftw_real tmp104;
	       fftw_real tmp184;
	       fftw_real tmp79;
	       fftw_real tmp186;
	       fftw_real tmp101;
	       fftw_real tmp187;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp74;
		    fftw_real tmp75;
		    fftw_real tmp102;
		    fftw_real tmp103;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp74 = X[2 * iostride];
		    tmp75 = Y[-10 * iostride];
		    tmp76 = tmp74 + tmp75;
		    tmp183 = tmp74 - tmp75;
		    tmp102 = Y[-2 * iostride];
		    tmp103 = X[10 * iostride];
		    tmp104 = tmp102 - tmp103;
		    tmp184 = tmp102 + tmp103;
	       }
	       {
		    fftw_real tmp77;
		    fftw_real tmp78;
		    fftw_real tmp99;
		    fftw_real tmp100;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp77 = Y[-14 * iostride];
		    tmp78 = X[6 * iostride];
		    tmp79 = tmp77 + tmp78;
		    tmp186 = tmp77 - tmp78;
		    tmp99 = Y[-6 * iostride];
		    tmp100 = X[14 * iostride];
		    tmp101 = tmp99 - tmp100;
		    tmp187 = tmp99 + tmp100;
	       }
	       tmp80 = tmp76 + tmp79;
	       tmp128 = tmp76 - tmp79;
	       tmp105 = tmp101 - tmp104;
	       tmp161 = tmp104 + tmp101;
	       {
		    fftw_real tmp211;
		    fftw_real tmp212;
		    fftw_real tmp185;
		    fftw_real tmp188;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp211 = tmp183 + tmp184;
		    tmp212 = tmp186 + tmp187;
		    tmp213 = K707106781 * (tmp211 - tmp212);
		    tmp237 = K707106781 * (tmp211 + tmp212);
		    tmp185 = tmp183 - tmp184;
		    tmp188 = tmp186 - tmp187;
		    tmp189 = K707106781 * (tmp185 + tmp188);
		    tmp249 = K707106781 * (tmp185 - tmp188);
	       }
	  }
	  {
	       fftw_real tmp84;
	       fftw_real tmp201;
	       fftw_real tmp110;
	       fftw_real tmp199;
	       fftw_real tmp87;
	       fftw_real tmp198;
	       fftw_real tmp113;
	       fftw_real tmp202;
	       fftw_real tmp107;
	       fftw_real tmp114;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp82;
		    fftw_real tmp83;
		    fftw_real tmp108;
		    fftw_real tmp109;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp82 = X[iostride];
		    tmp83 = Y[-9 * iostride];
		    tmp84 = tmp82 + tmp83;
		    tmp201 = tmp82 - tmp83;
		    tmp108 = Y[-iostride];
		    tmp109 = X[9 * iostride];
		    tmp110 = tmp108 - tmp109;
		    tmp199 = tmp108 + tmp109;
	       }
	       {
		    fftw_real tmp85;
		    fftw_real tmp86;
		    fftw_real tmp111;
		    fftw_real tmp112;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp85 = X[5 * iostride];
		    tmp86 = Y[-13 * iostride];
		    tmp87 = tmp85 + tmp86;
		    tmp198 = tmp85 - tmp86;
		    tmp111 = Y[-5 * iostride];
		    tmp112 = X[13 * iostride];
		    tmp113 = tmp111 - tmp112;
		    tmp202 = tmp111 + tmp112;
	       }
	       tmp88 = tmp84 + tmp87;
	       tmp107 = tmp84 - tmp87;
	       tmp114 = tmp110 - tmp113;
	       tmp115 = tmp107 - tmp114;
	       tmp137 = tmp107 + tmp114;
	       tmp163 = tmp110 + tmp113;
	       {
		    fftw_real tmp200;
		    fftw_real tmp203;
		    fftw_real tmp239;
		    fftw_real tmp240;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp200 = tmp198 + tmp199;
		    tmp203 = tmp201 - tmp202;
		    tmp204 = (K923879532 * tmp200) + (K382683432 * tmp203);
		    tmp215 = (K923879532 * tmp203) - (K382683432 * tmp200);
		    tmp239 = tmp201 + tmp202;
		    tmp240 = tmp199 - tmp198;
		    tmp241 = (K382683432 * tmp239) - (K923879532 * tmp240);
		    tmp251 = (K382683432 * tmp240) + (K923879532 * tmp239);
	       }
	  }
	  {
	       fftw_real tmp81;
	       fftw_real tmp96;
	       fftw_real tmp158;
	       fftw_real tmp162;
	       fftw_real tmp165;
	       fftw_real tmp166;
	       fftw_real tmp157;
	       fftw_real tmp159;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp81 = tmp73 + tmp80;
	       tmp96 = tmp88 + tmp95;
	       tmp158 = tmp81 - tmp96;
	       tmp162 = tmp160 + tmp161;
	       tmp165 = tmp163 + tmp164;
	       tmp166 = tmp162 - tmp165;
	       X[0] = tmp81 + tmp96;
	       Y[-15 * iostride] = tmp162 + tmp165;
	       tmp157 = c_re(W[7]);
	       tmp159 = c_im(W[7]);
	       X[8 * iostride] = (tmp157 * tmp158) + (tmp159 * tmp166);
	       Y[-7 * iostride] = (tmp157 * tmp166) - (tmp159 * tmp158);
	  }
	  {
	       fftw_real tmp170;
	       fftw_real tmp176;
	       fftw_real tmp174;
	       fftw_real tmp178;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp168;
		    fftw_real tmp169;
		    fftw_real tmp172;
		    fftw_real tmp173;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp168 = tmp160 - tmp161;
		    tmp169 = tmp88 - tmp95;
		    tmp170 = tmp168 - tmp169;
		    tmp176 = tmp169 + tmp168;
		    tmp172 = tmp73 - tmp80;
		    tmp173 = tmp164 - tmp163;
		    tmp174 = tmp172 - tmp173;
		    tmp178 = tmp172 + tmp173;
	       }
	       {
		    fftw_real tmp167;
		    fftw_real tmp171;
		    fftw_real tmp175;
		    fftw_real tmp177;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp167 = c_re(W[11]);
		    tmp171 = c_im(W[11]);
		    Y[-3 * iostride] = (tmp167 * tmp170) - (tmp171 * tmp174);
		    X[12 * iostride] = (tmp171 * tmp170) + (tmp167 * tmp174);
		    tmp175 = c_re(W[3]);
		    tmp177 = c_im(W[3]);
		    Y[-11 * iostride] = (tmp175 * tmp176) - (tmp177 * tmp178);
		    X[4 * iostride] = (tmp177 * tmp176) + (tmp175 * tmp178);
	       }
	  }
	  {
	       fftw_real tmp126;
	       fftw_real tmp142;
	       fftw_real tmp140;
	       fftw_real tmp144;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp106;
		    fftw_real tmp125;
		    fftw_real tmp136;
		    fftw_real tmp139;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp106 = tmp98 + tmp105;
		    tmp125 = K707106781 * (tmp115 + tmp124);
		    tmp126 = tmp106 - tmp125;
		    tmp142 = tmp106 + tmp125;
		    tmp136 = tmp128 + tmp135;
		    tmp139 = K707106781 * (tmp137 + tmp138);
		    tmp140 = tmp136 - tmp139;
		    tmp144 = tmp136 + tmp139;
	       }
	       {
		    fftw_real tmp97;
		    fftw_real tmp127;
		    fftw_real tmp141;
		    fftw_real tmp143;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp97 = c_re(W[9]);
		    tmp127 = c_im(W[9]);
		    X[10 * iostride] = (tmp97 * tmp126) + (tmp127 * tmp140);
		    Y[-5 * iostride] = (tmp97 * tmp140) - (tmp127 * tmp126);
		    tmp141 = c_re(W[1]);
		    tmp143 = c_im(W[1]);
		    X[2 * iostride] = (tmp141 * tmp142) + (tmp143 * tmp144);
		    Y[-13 * iostride] = (tmp141 * tmp144) - (tmp143 * tmp142);
	       }
	  }
	  {
	       fftw_real tmp148;
	       fftw_real tmp154;
	       fftw_real tmp152;
	       fftw_real tmp156;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp146;
		    fftw_real tmp147;
		    fftw_real tmp150;
		    fftw_real tmp151;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp146 = tmp135 - tmp128;
		    tmp147 = K707106781 * (tmp115 - tmp124);
		    tmp148 = tmp146 - tmp147;
		    tmp154 = tmp146 + tmp147;
		    tmp150 = tmp98 - tmp105;
		    tmp151 = K707106781 * (tmp138 - tmp137);
		    tmp152 = tmp150 - tmp151;
		    tmp156 = tmp150 + tmp151;
	       }
	       {
		    fftw_real tmp145;
		    fftw_real tmp149;
		    fftw_real tmp153;
		    fftw_real tmp155;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp145 = c_re(W[13]);
		    tmp149 = c_im(W[13]);
		    Y[-iostride] = (tmp145 * tmp148) - (tmp149 * tmp152);
		    X[14 * iostride] = (tmp149 * tmp148) + (tmp145 * tmp152);
		    tmp153 = c_re(W[5]);
		    tmp155 = c_im(W[5]);
		    Y[-9 * iostride] = (tmp153 * tmp154) - (tmp155 * tmp156);
		    X[6 * iostride] = (tmp155 * tmp154) + (tmp153 * tmp156);
	       }
	  }
	  {
	       fftw_real tmp206;
	       fftw_real tmp220;
	       fftw_real tmp218;
	       fftw_real tmp222;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp190;
		    fftw_real tmp205;
		    fftw_real tmp214;
		    fftw_real tmp217;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp190 = tmp182 - tmp189;
		    tmp205 = tmp197 - tmp204;
		    tmp206 = tmp190 - tmp205;
		    tmp220 = tmp190 + tmp205;
		    tmp214 = tmp210 - tmp213;
		    tmp217 = tmp215 - tmp216;
		    tmp218 = tmp214 - tmp217;
		    tmp222 = tmp214 + tmp217;
	       }
	       {
		    fftw_real tmp179;
		    fftw_real tmp207;
		    fftw_real tmp219;
		    fftw_real tmp221;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp179 = c_re(W[12]);
		    tmp207 = c_im(W[12]);
		    X[13 * iostride] = (tmp179 * tmp206) + (tmp207 * tmp218);
		    Y[-2 * iostride] = (tmp179 * tmp218) - (tmp207 * tmp206);
		    tmp219 = c_re(W[4]);
		    tmp221 = c_im(W[4]);
		    X[5 * iostride] = (tmp219 * tmp220) + (tmp221 * tmp222);
		    Y[-10 * iostride] = (tmp219 * tmp222) - (tmp221 * tmp220);
	       }
	  }
	  {
	       fftw_real tmp226;
	       fftw_real tmp232;
	       fftw_real tmp230;
	       fftw_real tmp234;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp224;
		    fftw_real tmp225;
		    fftw_real tmp228;
		    fftw_real tmp229;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp224 = tmp210 + tmp213;
		    tmp225 = tmp204 + tmp197;
		    tmp226 = tmp224 - tmp225;
		    tmp232 = tmp224 + tmp225;
		    tmp228 = tmp182 + tmp189;
		    tmp229 = tmp215 + tmp216;
		    tmp230 = tmp228 - tmp229;
		    tmp234 = tmp228 + tmp229;
	       }
	       {
		    fftw_real tmp223;
		    fftw_real tmp227;
		    fftw_real tmp231;
		    fftw_real tmp233;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp223 = c_re(W[8]);
		    tmp227 = c_im(W[8]);
		    Y[-6 * iostride] = (tmp223 * tmp226) - (tmp227 * tmp230);
		    X[9 * iostride] = (tmp227 * tmp226) + (tmp223 * tmp230);
		    tmp231 = c_re(W[0]);
		    tmp233 = c_im(W[0]);
		    Y[-14 * iostride] = (tmp231 * tmp232) - (tmp233 * tmp234);
		    X[iostride] = (tmp233 * tmp232) + (tmp231 * tmp234);
	       }
	  }
	  {
	       fftw_real tmp246;
	       fftw_real tmp256;
	       fftw_real tmp254;
	       fftw_real tmp258;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp238;
		    fftw_real tmp245;
		    fftw_real tmp250;
		    fftw_real tmp253;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp238 = tmp236 - tmp237;
		    tmp245 = tmp241 + tmp244;
		    tmp246 = tmp238 - tmp245;
		    tmp256 = tmp238 + tmp245;
		    tmp250 = tmp248 + tmp249;
		    tmp253 = tmp251 - tmp252;
		    tmp254 = tmp250 - tmp253;
		    tmp258 = tmp250 + tmp253;
	       }
	       {
		    fftw_real tmp235;
		    fftw_real tmp247;
		    fftw_real tmp255;
		    fftw_real tmp257;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp235 = c_re(W[10]);
		    tmp247 = c_im(W[10]);
		    X[11 * iostride] = (tmp235 * tmp246) + (tmp247 * tmp254);
		    Y[-4 * iostride] = (tmp235 * tmp254) - (tmp247 * tmp246);
		    tmp255 = c_re(W[2]);
		    tmp257 = c_im(W[2]);
		    X[3 * iostride] = (tmp255 * tmp256) + (tmp257 * tmp258);
		    Y[-12 * iostride] = (tmp255 * tmp258) - (tmp257 * tmp256);
	       }
	  }
	  {
	       fftw_real tmp262;
	       fftw_real tmp268;
	       fftw_real tmp266;
	       fftw_real tmp270;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp260;
		    fftw_real tmp261;
		    fftw_real tmp264;
		    fftw_real tmp265;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp260 = tmp248 - tmp249;
		    tmp261 = tmp241 - tmp244;
		    tmp262 = tmp260 + tmp261;
		    tmp268 = tmp260 - tmp261;
		    tmp264 = tmp236 + tmp237;
		    tmp265 = tmp251 + tmp252;
		    tmp266 = tmp264 - tmp265;
		    tmp270 = tmp264 + tmp265;
	       }
	       {
		    fftw_real tmp259;
		    fftw_real tmp263;
		    fftw_real tmp267;
		    fftw_real tmp269;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp259 = c_re(W[6]);
		    tmp263 = c_im(W[6]);
		    Y[-8 * iostride] = (tmp259 * tmp262) - (tmp263 * tmp266);
		    X[7 * iostride] = (tmp263 * tmp262) + (tmp259 * tmp266);
		    tmp267 = c_re(W[14]);
		    tmp269 = c_im(W[14]);
		    Y[0] = (tmp267 * tmp268) - (tmp269 * tmp270);
		    X[15 * iostride] = (tmp269 * tmp268) + (tmp267 * tmp270);
	       }
	  }
     }
     if (i == m) {
	  fftw_real tmp7;
	  fftw_real tmp51;
	  fftw_real tmp19;
	  fftw_real tmp43;
	  fftw_real tmp39;
	  fftw_real tmp47;
	  fftw_real tmp59;
	  fftw_real tmp64;
	  fftw_real tmp14;
	  fftw_real tmp56;
	  fftw_real tmp24;
	  fftw_real tmp32;
	  fftw_real tmp29;
	  fftw_real tmp33;
	  fftw_real tmp54;
	  fftw_real tmp65;
	  fftw_real tmp63;
	  fftw_real tmp66;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp3;
	       fftw_real tmp15;
	       fftw_real tmp38;
	       fftw_real tmp57;
	       fftw_real tmp6;
	       fftw_real tmp35;
	       fftw_real tmp18;
	       fftw_real tmp58;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp1;
		    fftw_real tmp2;
		    fftw_real tmp36;
		    fftw_real tmp37;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp1 = X[0];
		    tmp2 = X[7 * iostride];
		    tmp3 = tmp1 + tmp2;
		    tmp15 = tmp1 - tmp2;
		    tmp36 = Y[0];
		    tmp37 = Y[-7 * iostride];
		    tmp38 = tmp36 + tmp37;
		    tmp57 = tmp36 - tmp37;
	       }
	       {
		    fftw_real tmp4;
		    fftw_real tmp5;
		    fftw_real tmp16;
		    fftw_real tmp17;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp4 = X[4 * iostride];
		    tmp5 = X[3 * iostride];
		    tmp6 = tmp4 + tmp5;
		    tmp35 = tmp4 - tmp5;
		    tmp16 = Y[-4 * iostride];
		    tmp17 = Y[-3 * iostride];
		    tmp18 = tmp16 + tmp17;
		    tmp58 = tmp16 - tmp17;
	       }
	       tmp7 = tmp3 + tmp6;
	       tmp51 = tmp3 - tmp6;
	       tmp19 = tmp15 - tmp18;
	       tmp43 = tmp15 + tmp18;
	       tmp39 = tmp35 + tmp38;
	       tmp47 = tmp38 - tmp35;
	       tmp59 = tmp57 - tmp58;
	       tmp64 = tmp58 + tmp57;
	  }
	  {
	       fftw_real tmp10;
	       fftw_real tmp20;
	       fftw_real tmp23;
	       fftw_real tmp53;
	       fftw_real tmp13;
	       fftw_real tmp25;
	       fftw_real tmp28;
	       fftw_real tmp52;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp8;
		    fftw_real tmp9;
		    fftw_real tmp21;
		    fftw_real tmp22;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp8 = X[2 * iostride];
		    tmp9 = X[5 * iostride];
		    tmp10 = tmp8 + tmp9;
		    tmp20 = tmp8 - tmp9;
		    tmp21 = Y[-2 * iostride];
		    tmp22 = Y[-5 * iostride];
		    tmp23 = tmp21 + tmp22;
		    tmp53 = tmp21 - tmp22;
	       }
	       {
		    fftw_real tmp11;
		    fftw_real tmp12;
		    fftw_real tmp26;
		    fftw_real tmp27;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp11 = X[iostride];
		    tmp12 = X[6 * iostride];
		    tmp13 = tmp11 + tmp12;
		    tmp25 = tmp11 - tmp12;
		    tmp26 = Y[-iostride];
		    tmp27 = Y[-6 * iostride];
		    tmp28 = tmp26 + tmp27;
		    tmp52 = tmp27 - tmp26;
	       }
	       tmp14 = tmp10 + tmp13;
	       tmp56 = tmp10 - tmp13;
	       tmp24 = tmp20 - tmp23;
	       tmp32 = tmp20 + tmp23;
	       tmp29 = tmp25 - tmp28;
	       tmp33 = tmp25 + tmp28;
	       tmp54 = tmp52 - tmp53;
	       tmp65 = tmp53 + tmp52;
	  }
	  X[0] = K2_000000000 * (tmp7 + tmp14);
	  X[8 * iostride] = -(K2_000000000 * (tmp65 + tmp64));
	  tmp63 = tmp7 - tmp14;
	  tmp66 = tmp64 - tmp65;
	  X[4 * iostride] = K1_414213562 * (tmp63 - tmp66);
	  X[12 * iostride] = -(K1_414213562 * (tmp63 + tmp66));
	  {
	       fftw_real tmp61;
	       fftw_real tmp62;
	       fftw_real tmp55;
	       fftw_real tmp60;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp61 = tmp51 - tmp54;
	       tmp62 = tmp59 - tmp56;
	       X[6 * iostride] = (K765366864 * tmp61) - (K1_847759065 * tmp62);
	       X[14 * iostride] = -((K1_847759065 * tmp61) + (K765366864 * tmp62));
	       tmp55 = tmp51 + tmp54;
	       tmp60 = tmp56 + tmp59;
	       X[2 * iostride] = (K1_847759065 * tmp55) - (K765366864 * tmp60);
	       X[10 * iostride] = -((K765366864 * tmp55) + (K1_847759065 * tmp60));
	  }
	  {
	       fftw_real tmp45;
	       fftw_real tmp49;
	       fftw_real tmp48;
	       fftw_real tmp50;
	       fftw_real tmp44;
	       fftw_real tmp46;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp44 = K707106781 * (tmp32 + tmp33);
	       tmp45 = tmp43 - tmp44;
	       tmp49 = tmp43 + tmp44;
	       tmp46 = K707106781 * (tmp24 - tmp29);
	       tmp48 = tmp46 + tmp47;
	       tmp50 = tmp47 - tmp46;
	       X[3 * iostride] = (K1_662939224 * tmp45) - (K1_111140466 * tmp48);
	       X[11 * iostride] = -((K1_111140466 * tmp45) + (K1_662939224 * tmp48));
	       X[7 * iostride] = (K390180644 * tmp49) - (K1_961570560 * tmp50);
	       X[15 * iostride] = -((K1_961570560 * tmp49) + (K390180644 * tmp50));
	  }
	  {
	       fftw_real tmp31;
	       fftw_real tmp41;
	       fftw_real tmp40;
	       fftw_real tmp42;
	       fftw_real tmp30;
	       fftw_real tmp34;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp30 = K707106781 * (tmp24 + tmp29);
	       tmp31 = tmp19 + tmp30;
	       tmp41 = tmp19 - tmp30;
	       tmp34 = K707106781 * (tmp32 - tmp33);
	       tmp40 = tmp34 + tmp39;
	       tmp42 = tmp39 - tmp34;
	       X[iostride] = (K1_961570560 * tmp31) - (K390180644 * tmp40);
	       X[9 * iostride] = -((K390180644 * tmp31) + (K1_961570560 * tmp40));
	       X[5 * iostride] = (K1_111140466 * tmp41) - (K1_662939224 * tmp42);
	       X[13 * iostride] = -((K1_662939224 * tmp41) + (K1_111140466 * tmp42));
	  }
     }
}

static const int twiddle_order[] =
{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
fftw_codelet_desc fftw_hc2hc_backward_16_desc =
{
     "fftw_hc2hc_backward_16",
     (void (*)()) fftw_hc2hc_backward_16,
     16,
     FFTW_BACKWARD,
     FFTW_HC2HC,
     366,
     15,
     twiddle_order,
};


#include <math.h>
#include <string.h>

#include <gsl/gsl_matrix.h>

#include <gpc/gpc.h>
#include <egsl/egsl_macros.h>

#include "../csm_all.h"

#include "icp.h"

int icp_loop(struct sm_params*params, const double*q0, double*x_new,
	double*total_error, int*valid, int*iterations) {
	if(any_nan(q0,3)) {
		sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
		return 0;
	}

	LDP laser_sens = params->laser_sens;
	double x_old[3], delta[3], delta_old[3] = {0,0,0};
	copy_d(q0, 3, x_old);
	unsigned int hashes[params->max_iterations];
	int iteration;

	sm_debug("icp: starting at  q0 =  %s  \n", friendly_pose(x_old));

	if(JJ) jj_loop_enter("iterations");

	int all_is_okay = 1;

	for(iteration=0; iteration<params->max_iterations;iteration++) {
		if(JJ) jj_loop_iteration();
		if(JJ) jj_add_double_array("x_old", x_old, 3);

		egsl_push_named("icp_loop iteration");
		sm_debug("== icp_loop: starting iteration. %d  \n", iteration);

		/** Compute laser_sens's points in laser_ref's coordinates
		    by roto-translating by x_old */
		ld_compute_world_coords(laser_sens, x_old);

		/** Find correspondences (the naif or smart way) */
		if(params->use_corr_tricks)
			find_correspondences_tricks(params);
		else
			find_correspondences(params);

		/** If debug_verify_tricks, make sure that find_correspondences_tricks()
		    and find_correspondences() return the same answer */
			if(params->debug_verify_tricks)
				debug_correspondences(params);

		/* If not many correspondences, bail out */
		int num_corr = ld_num_valid_correspondences(laser_sens);
		double fail_perc = 0.05;
		if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
			sm_error("	: before trimming, only %d correspondences.\n",num_corr);
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration"); /* loop context */
			break;
		}

		if(JJ) jj_add("corr0", corr_to_json(laser_sens->corr, laser_sens->nrays));

		/* Kill some correspondences (using dubious algorithm) */
		if(params->outliers_remove_doubles)
			kill_outliers_double(params);

		int num_corr2 = ld_num_valid_correspondences(laser_sens);

		if(JJ) jj_add("corr1", corr_to_json(laser_sens->corr, laser_sens->nrays));

		double error=0;
		/* Trim correspondences */
		kill_outliers_trim(params, &error);
		int num_corr_after = ld_num_valid_correspondences(laser_sens);

		if(JJ) {
			jj_add("corr2", corr_to_json(laser_sens->corr, laser_sens->nrays));
			jj_add_int("num_corr0", num_corr);
			jj_add_int("num_corr1", num_corr2);
			jj_add_int("num_corr2", num_corr_after);
		}

		*total_error = error;
		*valid = num_corr_after;

		sm_debug("  icp_loop: total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);

		/* If not many correspondences, bail out */
		if(num_corr_after < fail_perc * laser_sens->nrays){
			sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration"); /* loop context */
			break;
		}

		/* Compute next estimate based on the correspondences */
		if(!compute_next_estimate(params, x_old, x_new)) {
			sm_error("  icp_loop: Cannot compute next estimate.\n");
			all_is_okay = 0;
			egsl_pop_named("icp_loop iteration");
			break;
		}

		pose_diff_d(x_new, x_old, delta);

		{
			sm_debug("  icp_loop: killing. laser_sens has %d/%d rays valid,  %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
			if(JJ) {
				jj_add_double_array("x_new", x_new, 3);
				jj_add_double_array("delta", delta, 3);
			}
		}
		/** Checks for oscillations */
		hashes[iteration] = ld_corr_hash(laser_sens);

		{
			sm_debug("  icp_loop: it. %d  hash=%d nvalid=%d mean error = %f, x_new= %s\n",
				iteration, hashes[iteration], *valid, *total_error/ *valid,
				friendly_pose(x_new));
		}

		/** PLICP terminates in a finite number of steps! */
		if(params->use_point_to_line_distance) {
			int loop_detected = 0; /* TODO: make function */
			int a; for(a=iteration-1;a>=0;a--) {
				if(hashes[a]==hashes[iteration]) {
					sm_debug("icpc: oscillation detected (cycle length = %d)\n", iteration-a);
					loop_detected = 1;
					break;
				}
			}
			if(loop_detected) {
				egsl_pop_named("icp_loop iteration");
				break;
			}
		}

		/* This termination criterium is useless when using
		   the point-to-line-distance; however, we put it here because
		   one can choose to use the point-to-point distance. */
		if(termination_criterion(params, delta)) {
			egsl_pop_named("icp_loop iteration");
			break;
		}

		copy_d(x_new, 3, x_old);
		copy_d(delta, 3, delta_old);

		egsl_pop_named("icp_loop iteration");
	}

	if(JJ) jj_loop_exit();

	*iterations = iteration+1;

	return all_is_okay;
}

int termination_criterion(struct sm_params*params, const double*delta){
	double a = norm_d(delta);
	double b = fabs(delta[2]);
	return (a<params->epsilon_xy) && (b<params->epsilon_theta);
}

int compute_next_estimate(struct sm_params*params,
	const double x_old[3], double x_new[3])
{
	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	struct gpc_corr c[laser_sens->nrays];

	int i; int k=0;
	for(i=0;i<laser_sens->nrays;i++) {
		if(!laser_sens->valid[i])
			continue;

		if(!ld_valid_corr(laser_sens,i))
			continue;

		int j1 = laser_sens->corr[i].j1;
		int j2 = laser_sens->corr[i].j2;

		c[k].valid = 1;

		if(laser_sens->corr[i].type == corr_pl) {

			c[k].p[0] = laser_sens->points[i].p[0];
			c[k].p[1] = laser_sens->points[i].p[1];
			c[k].q[0] = laser_ref->points[j1].p[0];
			c[k].q[1] = laser_ref->points[j1].p[1];

			/** TODO: here we could use the estimated alpha */
			double diff[2];
			diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
			diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
			double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
			double normal[2];
			normal[0] = +diff[1] * one_on_norm;
			normal[1] = -diff[0] * one_on_norm;

			double cos_alpha = normal[0];
			double sin_alpha = normal[1];

			c[k].C[0][0] = cos_alpha*cos_alpha;
			c[k].C[1][0] =
			c[k].C[0][1] = cos_alpha*sin_alpha;
			c[k].C[1][1] = sin_alpha*sin_alpha;

/*			sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d,  alpha_seg=%f, cos=%f sin=%f \n", k,i,
				rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/

#if 0
			/* Note: it seems that because of numerical errors this matrix might be
			   not semidef positive. */
			double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
			double trace = c[k].C[0][0] + c[k].C[1][1];

			int semidef = (det >= 0) && (trace>0);
			if(!semidef) {
	/*			printf("%d: Adjusting correspondence weights\n",i);*/
				double eps = -det;
				c[k].C[0][0] += 2*sqrt(eps);
				c[k].C[1][1] += 2*sqrt(eps);
			}
#endif
		} else {
			c[k].p[0] = laser_sens->points[i].p[0];
			c[k].p[1] = laser_sens->points[i].p[1];

			projection_on_segment_d(
				laser_ref->points[j1].p,
				laser_ref->points[j2].p,
				laser_sens->points_w[i].p,
				c[k].q);

			/* Identity matrix */
			c[k].C[0][0] = 1;
			c[k].C[1][0] = 0;
			c[k].C[0][1] = 0;
			c[k].C[1][1] = 1;
		}

		double factor = 1;

		/* Scale the correspondence weight by a factor concerning the
		   information in this reading. */
		if(params->use_ml_weights) {
			int have_alpha = 0;
			double alpha = 0;
			if(!is_nan(laser_ref->true_alpha[j1])) {
				alpha = laser_ref->true_alpha[j1];
				have_alpha = 1;
			} else if(laser_ref->alpha_valid[j1]) {
				alpha = laser_ref->alpha[j1];;
				have_alpha = 1;
			} else have_alpha = 0;

			if(have_alpha) {
				double pose_theta = x_old[2];
				/** Incidence of the ray
					Note that alpha is relative to the first scan (not the world)
					and that pose_theta is the angle of the second scan with
					respect to the first, hence it's ok. */
				double beta = alpha - (pose_theta + laser_sens->theta[i]);
				factor = 1 / square(cos(beta));
			} else {
				static int warned_before = 0;
				if(!warned_before) {
					sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]."
					          "Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");
					sm_error("Writing laser_ref: \n");
					ld_write_as_json(laser_ref, stderr);
					warned_before = 1;
				}
			}
		}

		/* Weight the points by the sigma in laser_sens */
		if(params->use_sigma_weights) {
			if(!is_nan(laser_sens->readings_sigma[i])) {
				factor *= 1 / square(laser_sens->readings_sigma[i]);
			} else {
				static int warned_before = 0;
				if(!warned_before) {
					sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");
					sm_error("Writing laser_sens: \n");
					ld_write_as_json(laser_sens, stderr);
				}
			}
		}

		c[k].C[0][0] *= factor;
		c[k].C[1][0] *= factor;
		c[k].C[0][1] *= factor;
		c[k].C[1][1] *= factor;

		k++;
	}

	/* TODO: use prior for odometry */
	double std = 0.11;
	const double inv_cov_x0[9] =
		{1/(std*std), 0, 0,
		 0, 1/(std*std), 0,
		 0, 0, 0};

	int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
	if(!ok) {
		sm_error("gpc_solve_valid failed\n");
		return 0;
	}

	double old_error = gpc_total_error(c, k, x_old);
	double new_error = gpc_total_error(c, k, x_new);

	sm_debug("\tcompute_next_estimate: old error: %f  x_old= %s \n", old_error, friendly_pose(x_old));
	sm_debug("\tcompute_next_estimate: new error: %f  x_new= %s \n", new_error, friendly_pose(x_new));
	sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);

	double epsilon = 0.000001;
	if(new_error > old_error + epsilon) {
		sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf  new error: %lf  x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
	}

	return 1;
}


/* im_invfft
 *
 * Copyright: 1990, N. Dessipris.
 *
 * Author: Nicos Dessipris
 * Written on: 12/04/1990
 * Modified on :
 * 28/6/95 JC
 *	- rewritten, based on new im_fwfft() code
 * 10/9/98 JC
 *	- frees memory more quickly
 * 2/4/02 JC
 *	- fftw code added
 * 13/7/02 JC
 *	- Type reset
 * 27/2/03 JC
 *	- tiny speed-up ... save 1 copy on write
 * 22/1/04 JC
 *	- oops, fix for segv on wider than high fftw transforms
 * 3/11/04
 *	- added fftw3 support
 * 7/2/10
 * 	- gtkdoc
 */

/*

    This file is part of VIPS.

    VIPS is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation; either version 2 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 Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 */

/*

    These files are distributed with VIPS - http://www.vips.ecs.soton.ac.uk

 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif /*HAVE_CONFIG_H*/
#include <vips/intl.h>

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

#ifdef HAVE_FFTW
#include <fftw.h>
#endif /*HAVE_FFTW*/

#ifdef HAVE_FFTW3
#include <fftw3.h>
#endif /*HAVE_FFTW3*/

#include <vips/vips.h>
#include <vips/internal.h>

#ifdef WITH_DMALLOC
#include <dmalloc.h>
#endif /*WITH_DMALLOC*/

#ifdef HAVE_FFTW
/* Call fftw for a 1 band image.
 */
static int
invfft1( IMAGE *dummy, IMAGE *in, IMAGE *out )
{
	fftwnd_plan plan;

	IMAGE *cmplx = im_open_local( out, "invfft1:1", "t" );

	/* Make dp complex image.
	 */
	if( !cmplx || im_pincheck( in ) || im_poutcheck( out ) )
		return( -1 );
	if( in->Coding != IM_CODING_NONE || in->Bands != 1 ) {
                im_error( "im_invfft", "%s", _( "one band uncoded only" ) );
                return( -1 );
	}
	if( im_clip2fmt( in, cmplx, IM_BANDFMT_DPCOMPLEX ) )
                return( -1 );

	/* Make the plan for the transform. Yes, they really do use nx for
	 * height and ny for width.
	 */
	if( !(plan = fftw2d_create_plan( in->Ysize, in->Xsize,
		FFTW_BACKWARD,
		FFTW_MEASURE | FFTW_USE_WISDOM | FFTW_IN_PLACE )) ) {
                im_error( "im_invfft",
			"%s", _( "unable to create transform plan" ) );
		return( -1 );
	}

	fftwnd_one( plan, (fftw_complex *) cmplx->data, NULL );

	fftwnd_destroy_plan( plan );

	/* Copy to out.
	 */
        if( im_copy( cmplx, out ) )
                return( -1 );

	return( 0 );
}
#else /*!HAVE_FFTW*/
#ifdef HAVE_FFTW3
/* Complex to complex inverse transform.
 */
static int
invfft1( IMAGE *dummy, IMAGE *in, IMAGE *out )
{
	fftw_plan plan;

	IMAGE *cmplx = im_open_local( out, "invfft1:1", "t" );

	/* We have to have a separate buffer for the planner to work on.
	 */
	double *planner_scratch = IM_ARRAY( dummy,
		in->Xsize * in->Ysize * 2, double );

	/* Make dp complex image.
	 */
	if( !cmplx || im_pincheck( in ) || im_poutcheck( out ) )
		return( -1 );
	if( in->Coding != IM_CODING_NONE || in->Bands != 1 ) {
                im_error( "im_invfft",
			"%s", _( "one band uncoded only" ) );
                return( -1 );
	}
	if( im_clip2fmt( in, cmplx, IM_BANDFMT_DPCOMPLEX ) )
                return( -1 );

	/* Make the plan for the transform. Yes, they really do use nx for
	 * height and ny for width.
	 */
	if( !(plan = fftw_plan_dft_2d( in->Ysize, in->Xsize,
		(fftw_complex *) planner_scratch,
		(fftw_complex *) planner_scratch,
		FFTW_BACKWARD,
		0 )) ) {
                im_error( "im_invfft",
			"%s", _( "unable to create transform plan" ) );
		return( -1 );
	}

	fftw_execute_dft( plan,
		(fftw_complex *) cmplx->data, (fftw_complex *) cmplx->data );

	fftw_destroy_plan( plan );

	/* Copy to out.
	 */
        if( im_copy( cmplx, out ) )
                return( -1 );

	return( 0 );
}
#else /*!HAVE_FFTW3*/
/* Fall back to VIPS's built-in fft
 */
static int
invfft1( IMAGE *dummy, IMAGE *in, IMAGE *out )
{
	int bpx = im_ispoweroftwo( in->Xsize );
	int bpy = im_ispoweroftwo( in->Ysize );
	float *buf, *q, *p1, *p2;
	int x, y;

	/* Buffers for real and imaginary parts.
	 */
	IMAGE *real = im_open_local( dummy, "invfft1:1", "t" );
	IMAGE *imag = im_open_local( dummy, "invfft1:2", "t" );

	/* Temps.
	 */
	IMAGE *t1 = im_open_local( dummy, "invfft1:3", "p" );
	IMAGE *t2 = im_open_local( dummy, "invfft1:4", "p" );

	if( !real || !imag || !t1 )
		return( -1 );
        if( im_pincheck( in ) || im_outcheck( out ) )
                return( -1 );
        if( in->Coding != IM_CODING_NONE ||
		in->Bands != 1 || !im_iscomplex( in ) ) {
                im_error( "im_invfft",
			"%s", _( "one band complex uncoded only" ) );
                return( -1 );
	}
	if( !bpx || !bpy ) {
		im_error( "im_invfft",
			"%s", _( "sides must be power of 2" ) );
		return( -1 );
	}

	/* Make sure we have a single-precision complex input image.
	 */
	if( im_clip2fmt( in, t1, IM_BANDFMT_COMPLEX ) )
		return( -1 );

	/* Extract real and imag parts. We have to complement the imaginary.
	 */
	if( im_c2real( t1, real ) )
		return( -1 );
	if( im_c2imag( t1, t2 ) || im_lintra( -1.0, t2, 0.0, imag ) )
		return( -1 );

	/* Transform!
	 */
	if( im__fft_sp( (float *) real->data, (float *) imag->data,
		bpx - 1, bpy - 1 ) ) {
                im_error( "im_invfft",
			"%s", _( "fft_sp failed" ) );
                return( -1 );
	}

	/* WIO to out.
	 */
        if( im_cp_desc( out, in ) )
                return( -1 );
	out->BandFmt = IM_BANDFMT_COMPLEX;
        if( im_setupout( out ) )
                return( -1 );
	if( !(buf = (float *) IM_ARRAY( dummy,
		IM_IMAGE_SIZEOF_LINE( out ), PEL )) )
		return( -1 );

	/* Gather together real and imag parts.
	 */
	for( p1 = (float *) real->data, p2 = (float *) imag->data,
		y = 0; y < out->Ysize; y++ ) {
		q = buf;

		for( x = 0; x < out->Xsize; x++ ) {
			q[0] = *p1++;
			q[1] = *p2++;
			q += 2;
		}

		if( im_writeline( y, out, (PEL *) buf ) )
			return( -1 );
	}

	return( 0 );
}
#endif /*HAVE_FFTW3*/
#endif /*HAVE_FFTW*/

/**
 * im_invfft:
 * @in: input image
 * @out: output image
 *
 * Transform an image from Fourier space to real space. The result is complex.
 * If you are OK with a real result, use im_invfftr() instead, it's quicker.
 *
 * VIPS uses the fftw3 or fftw2 Fourier transform libraries if possible. If
 * they were not available when VIPS was built, it falls back to it's own
 * FFT functions which are slow and only work for square images whose sides
 * are a power of two.
 *
 * See also: im_invfftr(), im_fwfft(), im_disp_ps().
 *
 * Returns: 0 on success, -1 on error.
 */
int
im_invfft( IMAGE *in, IMAGE *out )
{
	IMAGE *dummy = im_open( "im_invfft:1", "p" );

	if( !dummy )
		return( -1 );
	if( im__fftproc( dummy, in, out, invfft1 ) ) {
		im_close( dummy );
		return( -1 );
	}
	im_close( dummy );

	if( out->Bands == 1 )
		out->Type = IM_TYPE_B_W;
	else
		out->Type = IM_TYPE_MULTIBAND;

	return( 0 );
}


#include <gsl/gsl_math.h>
#include "gsl_cblas.h"
#include "cblas.h"

void
cblas_ssyr (const enum CBLAS_ORDER order, const enum CBLAS_UPLO Uplo,
	    const int N, const float alpha, const float *X, const int incX,
	    float *A, const int lda)
{
#define BASE float
#include "source_syr.h"
#undef BASE
}