/**
 * @file videofeature.h
 * @brief Audio-visual speech recognition front end.
 * @author Munir Georges, John McDonough, Friedrich Faubel
 */
#include "btk.h"

#ifdef AVFORMAT

#ifdef OPENCV

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_sort_vector.h>

#ifdef HAVE_LIBFFTW3
#include <fftw3.h>
#endif

#undef HAVE_CONFIG_H

#include <opencv/cv.h>
#include <opencv/highgui.h>

#define HAVE_CONFIG_H

#include "stream/stream.h"
#include "common/mlist.h"

#ifndef VIDEOFEATURE_H
#define VIDEOFEATURE_H

// ----- definition for class VideoFeature -----
//
class VideoFeature : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  VideoFeature(int mode, unsigned width, unsigned height, const String& nm = "VideoFeature");

  ~VideoFeature();

  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

  void read(const String& fileName, int from, int _to);

  int width() const { return _width; }

  int height() const { return _height; }

  int frameNumber() const { return _frameNumber;}
  int Frames();

 private:
  CvCapture*					_capture;
  IplImage*					_frame;
  IplImage*					_gray;
  IplImage*					_hsv;

  unsigned					_width;
  unsigned					_height;

  gsl_vector_float*				_R;
  gsl_vector_float*				_G;
  gsl_vector_float*				_B;

  String					_filename;
  unsigned					_from;
  unsigned					_to;
  int						_mode;

  bool						_endOfSamples;
  string					_fileName;
  int						_frameNumber;
};

typedef Inherit<VideoFeature, VectorFloatFeatureStreamPtr> VideoFeaturePtr;

// ----- definition for class 'ImageROI' -----
//
class ImageROI : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageROI(const VectorFloatFeatureStreamPtr& src, unsigned width, unsigned height, unsigned x=0, unsigned y=0, unsigned w=2, unsigned h=3, const String& nm = "ImageROI");

  ~ImageROI();
  void setROI(int x,int y);

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

  unsigned width() const { return _width; }

  unsigned height() const { return _height; }

  unsigned x_pos() const { return _x_pos; }

  unsigned y_pos() const { return _y_pos; }

  unsigned w() const { return _w; }

  unsigned h() const { return _h; }

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  const unsigned				_width;
  const unsigned				_height;
  gsl_vector_float*				_srcVec;
  const unsigned				_w;
  const unsigned				_h;
  unsigned					_x_pos;
  unsigned					_y_pos;
};

typedef Inherit<ImageROI, VectorFloatFeatureStreamPtr> ImageROIPtr;

// ----- definition for class ImageSmooth -----
//
class ImageSmooth : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageSmooth(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, int smoothtype, int param1, int param2, const String& nm = "ImageSmooth");

  ~ImageSmooth();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  unsigned					_width;
  unsigned					_height;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  IplImage*					_imgf;
  int						_smoothtype;
  int						_param1;
  int						_param2;
};

typedef Inherit<ImageSmooth, VectorFloatFeatureStreamPtr> ImageSmoothPtr;

// ----- definition for class ImageMorphology -----
//
class ImageMorphology : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageMorphology(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, int type, int param, const String& nm = "ImageMorphology");

  ~ImageMorphology();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  unsigned					_width;
  unsigned					_height;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  IplImage*					_imgf;
  int						_type;
  int						_param;
};

typedef Inherit<ImageMorphology, VectorFloatFeatureStreamPtr> ImageMorphologyPtr;

// ----- definition for class ImageMorphologyEx -----
//
class ImageMorphologyEx : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageMorphologyEx(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, int type, int param, const String& nm = "ImageMorphologyEx");

  ~ImageMorphologyEx();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  unsigned					_width;
  unsigned					_height;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  IplImage*					_imgf;
  IplImage*					_temp;
  int						_type;
  int						_param;
};

typedef Inherit<ImageMorphologyEx, VectorFloatFeatureStreamPtr> ImageMorphologyExPtr;

// ----- definition for class Canny -----
//
class Canny : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  Canny(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, int param0, int param1, int param2, const String& nm = "Canny");

  ~Canny();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  unsigned					_width;
  unsigned					_height;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  IplImage*					_imgf;
  int						_param0;
  int						_param1;
  int						_param2;
};

typedef Inherit<Canny, VectorFloatFeatureStreamPtr> CannyPtr;

// ----- definition for class ImageThreshold -----
//
class ImageThreshold : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageThreshold(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, float param0, int param1, int param2, const String& nm = "Canny");

  ~ImageThreshold();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  bool						_endOfSamples;
  unsigned					_width;
  unsigned					_height;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  IplImage*					_imgf;
  int						_param0;
  int						_param1;
  int						_param2;
};

typedef Inherit<ImageThreshold, VectorFloatFeatureStreamPtr> ImageThresholdPtr;

/*Do not use this funktion within python - there is a problem with the window-handle or threading....*/
// ----- definition for class imageshow -----
//
class ImageShow : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageShow(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, const String& nm = "ImageShow");

  ~ImageShow();

  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  unsigned					_width;
  unsigned					_height;
  IplImage*					_img;
  gsl_vector_float*				_srcVec;
  bool						_endOfSamples;
};

typedef Inherit<ImageShow, VectorFloatFeatureStreamPtr> ImageShowPtr;

// ----- definition for class 'SaveImage' -----
//
class SaveImage {
 public:
  // sz := number of elements in _vector
  SaveImage(unsigned width, unsigned height);

  ~SaveImage();

  void save(const gsl_vector_float* V, const String& filename);
  void savedouble(const gsl_vector* V, const String& filename);

 private:
  unsigned					_width;
  unsigned					_height;
  IplImage*					_img;
  const String					_filename;
};

typedef refcount_ptr<SaveImage> SaveImagePtr;

// ----- definition for class 'ImageDetection' -----
//
class ImageDetection : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageDetection(VectorFloatFeatureStreamPtr& src, unsigned width, unsigned height, unsigned w, unsigned h,
		 const String& filename, double scale_factor, int min_neighbors, int flags, int min_sizeX, int min_sizeY,
		 const String& nm = "ImageDetection");

  ~ImageDetection();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  double _linearnext(gsl_vector * v);

  VectorFloatFeatureStreamPtr			_src;
  const unsigned				_width;
  const unsigned				_height;
  unsigned					_w;
  unsigned					_h;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;

  const String					_filename;
  CvHaarClassifierCascade*			_pCascade;
  CvMemStorage*					_pStorage;
  CvSeq*					_pRectSeq;
  double					_scale_factor;
  int						_min_neighbors;
  int						_flags;
  CvSize					_min_size;

  int						_x_pos;
  int						_y_pos;

  const int					_his;
  gsl_vector*					_x_his;
  gsl_vector*					_y_his;
  int						_tmp_x;
  int						_tmp_y;
  bool						_endOfSamples;
};

typedef Inherit<ImageDetection, VectorFloatFeatureStreamPtr> ImageDetectionPtr;

// ----- definition for class FaceDetection -----
//
class FaceDetection : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  FaceDetection(VectorFloatFeatureStreamPtr& src, unsigned width, unsigned height, int region,
		const String& filename_eye, double scale_factor_eye, int min_neighbors_eye, int flags_eye, int min_sizeX_eye, int min_sizeY_eye,
		const String& filename_nose, double scale_factor_nose, int min_neighbors_nose, int flags_nose, int min_sizeX_nose, int min_sizeY_nose,
		const String& filename_mouth, double scale_factor_mouth, int min_neighbors_mouth, int flags_mouth, int min_sizeX_mouth, int min_sizeY_mouth,
		const String& nm = "FaceDetection");

  ~FaceDetection();

  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  bool						_endOfSamples;
  VectorFloatFeatureStreamPtr			_src;
  gsl_vector_float*				_srcVec;
  unsigned					_width;
  unsigned					_height;
};

typedef Inherit<FaceDetection, VectorFloatFeatureStreamPtr> FaceDetectionPtr;

// ----- definition for class ImageCentering -----
//
class ImageCentering : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  ImageCentering(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height,	const String& nm = "ImageCentering");

  ~ImageCentering();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  bool                                          _endOfSamples;
  VectorFloatFeatureStreamPtr	                _src;
  gsl_vector_float*                             _srcVec;
  IplImage*					_img;
  IplImage*					_gmi;
  IplImage*					_out;
  unsigned                                      _width;
  unsigned                                      _height;
};

typedef Inherit<ImageCentering, VectorFloatFeatureStreamPtr> ImageCenteringPtr;

// ----- definition for class LinearInterpolation -----
//
class LinearInterpolation : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  LinearInterpolation(const VectorFloatFeatureStreamPtr& src, double fps_src, double fps_dest,
		      const String& nm = "LinearInterpolation");
  ~LinearInterpolation();

  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  VectorFloatFeatureStreamPtr			_src;
  const double					_DeltaTs;
  const double					_DeltaTd;

  int						_sourceFrameX;
  gsl_vector_float*				_x_n;
  bool						_endOfSamples;
};

typedef Inherit<LinearInterpolation, VectorFloatFeatureStreamPtr> LinearInterpolationPtr;

// ----- definition for class OpticalFlowFeature -----
//
class OpticalFlowFeature : public VectorFloatFeatureStream {
 public:
  OpticalFlowFeature(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, const char* filename, const String& nm = "OpticalFlowFeature");

  ~OpticalFlowFeature();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  float StringToFloat(const std::string& str_input);
  int StringToInt(const std::string& str_input);

  void disalloc_matrix(float **matrix, long nx, long ny);
  void alloc_matrix(float ***matrix, long nx, long ny);
  float ** _gray;
  float ** _ux;
  float ** _uy;
  typedef float fptype;
  typedef int itype;
  fptype **f1_c1;         /* in     : 1st image, channel 1*/
  fptype **f2_c1;         /* in     : 2nd image, channel 1*/
  fptype **f1_c2;         /* in     : 1st image, channel 2*/
  fptype **f2_c2;         /* in     : 2nd image, channel 2*/
  fptype **f1_c3;         /* in     : 1st image, channel 3*/
  fptype **f2_c3;         /* in     : 2nd image, channel 3*/
  fptype **u;             /* in+out : u component of flow field */
  fptype **v;             /* in+out : v component of flow field */
  itype  _nx;             /* in     : size in x-direction on current grid */
  itype  _ny;             /* in     : size in y-direction on current grid */
  itype  _bx;             /* in     : boundary size in x-direction */
  itype  _by;             /* in     : boundary size in y-direction */
  fptype hx;              /* in     : grid size in x-dir. on current grid */
  fptype hy;              /* in     : grid size in y-dir. on current grid */
  itype  m_type_d;        /* in     : type of data term */
  itype  m_type_s;        /* in     : type of smoothness term */
  fptype m_gamma_ofc1;    /* in     : weight of grey value constancy */
  fptype m_gamma_ofc2;    /* in     : weight of gradient constancy */
  fptype m_gamma_ofc3;    /* in     : weight of Hessian constancy */
  fptype m_gamma_gradnorm;/* in     : weight of gradient norm constancy */
  fptype m_gamma_laplace; /* in     : weight of Laplacian constancy */
  fptype m_gamma_hessdet; /* in     : weight of Hessian det. constancy */
  itype  m_function_d;    /* in     : type of robust function in data term */
  itype  m_function_s;    /* in     : type of robust function in smoothness term */
  fptype m_epsilon_d;     /* in     : parameter data term */
  fptype m_epsilon_s;     /* in     : parameter smoothness term */
  fptype m_power_d;       /* in     : exponent data term */
  fptype m_power_s;       /* in     : exponent smoothness term */
  fptype m_rhox;          /* in     : integration scale in x-direction*/
  fptype m_rhoy;          /* in     : integration scale in y-direction*/
  fptype m_rhox_F;        /* in     : FLOW integration scale in x-direction*/
  fptype m_rhoy_F;        /* in     : FLOW integration scale in y-direction*/
  fptype m_sigma_F;       /* in     : FLOW presmoothing scale */
  fptype m_alpha;         /* in     : smoothness weight */
  itype  m_color;         /* in     : color flag */
  itype  n_solver;        /* in     : general solver */
  itype  n_max_rec_depth; /* in     : max rec depth */
  itype  n_mg_res_prob;   /* in     : resample problem */
  itype  n_theta;         /* in     : discretisation parameter */
  itype  n_mg_cycles;     /* in     : number of cycles */
  itype  n_mg_solver;     /* in     : type of multigrid solver */
  itype  n_mg_pre_relax;  /* in     : number of pre relaxation steps */
  itype  n_mg_post_relax; /* in     : number of post relaxation steps */
  itype  n_mg_rec_calls;  /* in     : number of recursive calls */
  itype  n_mg_centered;   /* in     : intergrid transfer: 0-cell/1-vertex */
  itype  n_mg_mat_order_r;/* in     : matrix multigrid restriction order */
  itype  n_mg_cas_order_r;/* in     : cascadic multigrid restriction order */
  itype  n_mg_cas_order_p;/* in     : cascadic multigrid prolongation order */
  itype  n_mg_cor_order_r;/* in     : correcting multigrid restriction order */
  itype  n_mg_cor_order_p;/* in     : correcting multigrid prolongation order */
  itype  n_iter_out;      /* in     : outer iterations */
  itype  n_iter_in;       /* in     : inner iterations */
  fptype n_omega;         /* in     : overrelaxation parameter */
  fptype n_tau;           /* in     : time step size */
  fptype n_warp_eta;      /* in     : reduction factor */
  itype  n_warp_steps;    /* in     : warping steps per level */
  itype  n_warp_max_rec_depth; /*in : max warp rect depth */
  itype  n_warp_scaling;  /* in     : flag for scaling  */
  itype  n_warp_rem;      /* in     : flag for removing out of bounds estimates */
  itype  n_warp_centered; /* in     : warping intergrid transfer: 0-cell/1-vertex */
  itype  n_warp_order_r;  /* in     : correcting warping restriction order */
  itype  n_warp_order_p;  /* in     : correcting warping prolongation order */
  fptype n_tensor_eps;    /* in     : motion tensor normalisation factor */
  fptype **utruth;        /* in     : x-component of numerical ground truth */
  fptype **vtruth;        /* in     : y-component of numerical ground truth */
  fptype **uref;          /* in     : x-component of problem ground truth */
  fptype **vref;          /* in     : y-component of problem ground truth */
  fptype **error_mag;     /* in     : error magnitude map */
  itype  i_rel_res_flag;  /* in     : flag for relative residual computation */
  fptype *i_rel_res;      /* out    : relative residual */
  itype  i_rel_err_flag;  /* in     : flag for relative error computation */
  fptype *i_rel_err;      /* out    : relative error */
  itype  i_time_flag;     /* in     : flag for runtime computation */
  itype  *i_time_sec;     /* out    : consumed seconds */
  itype  *i_time_usec;    /* out    : consumed microseconds */
  itype  info_flag;       /* in     : flag for information */
  itype  info_step;       /* in     : stepsize for information */
  itype  write_flag;      /* in     : flag for writing out */
  itype  write_step;      /* in     : stepsize for writing out */
  itype  frame_nr;        /* in     : current frame number */

  bool						_endOfSamples;
  VectorFloatFeatureStreamPtr			_src;
  gsl_vector_float*				_srcVec;
  unsigned					_width;
  unsigned					_height;
};

typedef Inherit<OpticalFlowFeature, VectorFloatFeatureStreamPtr> OpticalFlowFeaturePtr;

// ----- definition for class SnakeImage -----
//
class SnakeImage : public VectorFloatFeatureStream {
 public:
  SnakeImage(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, float alpha, float beta, float gamma, const String& nm = "SnakeImage");

  ~SnakeImage();

  gsl_vector_float* data();
  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  bool                          		_endOfSamples;
  VectorFloatFeatureStreamPtr			_src;
  gsl_vector_float*				_srcVec;
  IplImage*					_img;
  unsigned                      		_width;
  unsigned                      		_height;
  float                       		        _alpha;
  float                       		        _beta ;
  float                       		        _gamma;

};

typedef Inherit<SnakeImage, VectorFloatFeatureStreamPtr> SnakeImagePtr;

// ----- definition for class PCAFeature -----
//
class PCAFeature : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  PCAFeature(const VectorFloatFeatureStreamPtr& src,unsigned width,unsigned height, const char *filename, const char *filename_mean, int n, int k, const String& nm = "PCAFeature");

  ~PCAFeature();

  virtual const gsl_vector_float* next(int frameX = -5);

  virtual void reset();

 private:
  void _floatToDouble(gsl_vector_float *src, gsl_vector * dest);
  void _doubleToFloat(gsl_vector *src, gsl_vector_float * dest);
  void _normalizeRows();

  VectorFloatFeatureStreamPtr			_src;

  const unsigned				_width;
  const unsigned				_height;
  const unsigned				_N;
  const unsigned				_M;

  gsl_matrix*					_evec;
  gsl_vector*					_mean;
  gsl_vector_float*				_srcVec;

  bool						_endOfSamples;
};

typedef Inherit<PCAFeature, VectorFloatFeatureStreamPtr> PCAFeaturePtr;

// ----- definition for class IPCAFeature -----
//
class IPCAFeature : public VectorFloatFeatureStream {
 public:
  // sz := number of elements in _vector
  IPCAFeature(const VectorFloatFeatureStreamPtr& src, unsigned width, unsigned height, const String& filename,  const String& filename_mean, int n, int k, const String& nm = "IPCAFeature");
  ~IPCAFeature();

  virtual const gsl_vector_float* next(int frameX = -5);
  const gsl_vector* get_mean() const { return _mean; }
  const gsl_matrix* get_vec(int i) const { return _evec; }

  virtual void reset();

 private:
  void _floatToDouble(gsl_vector_float* src, gsl_vector* dest);
  void _doubleToFloat(gsl_vector* src, gsl_vector_float* dest);
  void _normalizeRows();

  VectorFloatFeatureStreamPtr			_src;

  const unsigned				_width;
  const unsigned				_height;
  const unsigned				_N;
  const unsigned				_M;

  gsl_matrix*					_evec;
  gsl_vector*					_mean;
  gsl_vector_float*				_srcVec;

  bool						_endOfSamples;
};

typedef Inherit<IPCAFeature, VectorFloatFeatureStreamPtr> IPCAFeaturePtr;

// ----- definition for class PCAEstimator -----
//
class PCAEstimator{
 public:
  // sz := number of elements in _vector
  PCAEstimator(unsigned n,unsigned m);

  ~PCAEstimator();

  void clearaccu();
  void accumulate(gsl_vector_float *V);
  void estimate();
  void save(const String& filename);

  const gsl_vector* get_mean()   const { return _mean; }
  const gsl_matrix* get_vec()    const { return _evec; }
  const gsl_vector* get_eigval() const { return _eval; }

 private:
  void     _floatToDouble(gsl_vector_float* src, gsl_vector* dest);
  void     _doubleToFloat(gsl_vector* src, gsl_vector_float* dest);
  unsigned					_N;
  unsigned					_M;

  unsigned					_in_data;
  gsl_vector*					_sum;    //sum per row
  gsl_vector*					_mean;

  gsl_matrix*					_data;   // original data, one image in each row
  gsl_matrix*					_mdata;  // data without row mean
  gsl_matrix*					_mdatat; // data'

  gsl_matrix*					_cov;    // cov = 1/M * (mdata' * data)
  gsl_vector*					_eval;   //eigen values
  gsl_matrix*					_evec;   //eigen vector
};

typedef refcount_ptr<PCAEstimator> PCAEstimatorPtr;

// ----- definition for class PCAModEstimator -----
//
class PCAModEstimator {
 public:
  // sz := number of elements in _vector
  PCAModEstimator(unsigned n, unsigned m);

  ~PCAModEstimator();

  const gsl_vector* get_mean()    const { return _mean; }
  const gsl_matrix* get_data()    const { return _data; }
  const gsl_matrix* get_mdata()   const { return _mdata; }
  const gsl_vector* get_eigval()  const { return _eval; }
  const gsl_matrix* get_eigface() const { return _eigface; }
  const gsl_matrix* get_evecc()   const { return _evecc; }
  const gsl_matrix* get_evec()    const { return _evec; }
  const gsl_matrix* get_cov()     const { return _cov; }

  void clearaccu();
  void accumulate(gsl_vector_float* V);
  void estimate();
  void save(const char* filename1, const char* filename2);

 private:
  void _floatToDouble(gsl_vector_float* src, gsl_vector* dest);
  void _doubleToFloat(gsl_vector* src, gsl_vector_float* dest);
  void _normalizeRows();
  void _normalizeColumn();

  const unsigned				_N;
  const unsigned				_M;

  unsigned					_indata;
  gsl_matrix*					_data;   // original data, one image in each row
  gsl_matrix*					_mdata;  // data without row mean
  gsl_vector*					_sum;    // sum per row
  gsl_vector*					_mean;   // sum per row
  gsl_matrix*					_mdatat; // data'

  gsl_matrix*					_cov;    // cov = 1/M * (mdata' * data)
  gsl_vector*					_eval;   // eigen values
  gsl_matrix*					_evec;   // eigen vector
  gsl_matrix*					_evecc;  // eigen vector
  gsl_matrix*					_evecct; // eigen vector
  gsl_matrix*					_eigface;// eigen vector
};

typedef refcount_ptr<PCAModEstimator> PCAModEstimatorPtr;

#endif

#endif

#endif


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

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

#ifdef SUBFIND
#include "subfind.h"

/*! Structure for communication during the density computation. Holds data that is sent to other processors.
 */
static struct nearestdata_in
{
  MyDouble Pos[3];
  MyIDType ID;
  MyFloat Hsml;
  MyFloat Density;
  MyFloat Dist[2];
  int Count;
  long long Index[2];
  int NodeList[NODELISTLENGTH];
}
 *NearestDataIn, *NearestDataGet;

static struct nearestdata_out
{
  MyFloat Dist[2];
  long long Index[2];
  int Count;
}
 *NearestDataResult, *NearestDataOut;

void subfind_find_nearesttwo(void)
{
  int i, j, k, l, ndone, ndone_flag, dummy;
  int ngrp, sendTask, recvTask, place, nexport, nimport, maxexport;

  if(ThisTask == 0)
    printf("Start finding nearest two (%d particles on task=%d)\n", NumPartGroup, ThisTask);

  /* allocate buffers to arrange communication */

  Ngblist = (int *) mymalloc(NumPartGroup * sizeof(int));
  Dist2list = (double *) mymalloc(NumPartGroup * sizeof(double));

  All.BunchSize =
    (int) ((All.BufferSize * 1024 * 1024) / (sizeof(struct data_index) + sizeof(struct data_nodelist) +
					     sizeof(struct nearestdata_in) + sizeof(struct nearestdata_out) +
					     sizemax(sizeof(struct nearestdata_in),
						     sizeof(struct nearestdata_out))));
  DataIndexTable = (struct data_index *) mymalloc(All.BunchSize * sizeof(struct data_index));
  DataNodeList = (struct data_nodelist *) mymalloc(All.BunchSize * sizeof(struct data_nodelist));
  maxexport = All.BunchSize - 2 * NTopleaves / NODELISTLENGTH;

  for(i = 0; i < NumPartGroup; i++)
    NgbLoc[i].count = 0;

  /* we will repeat the whole thing for those particles where we didn't find enough neighbours */

  i = 0;			/* begin with this index */

  do
    {
      for(j = 0; j < NTask; j++)
	{
	  Send_count[j] = 0;
	  Exportflag[j] = -1;
	}

      /* do local particles and prepare export list */

      for(nexport = 0; i < NumPartGroup; i++)
	{
	  if(subfind_nearesttwo_evaluate(i, 0, &nexport, Send_count) < 0)
	    break;
	}

      qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);

      MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

      for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
	{
	  Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
	  nimport += Recv_count[j];

	  if(j > 0)
	    {
	      Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
	      Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
	    }
	}

      NearestDataGet = (struct nearestdata_in *) mymalloc(nimport * sizeof(struct nearestdata_in));
      NearestDataIn = (struct nearestdata_in *) mymalloc(nexport * sizeof(struct nearestdata_in));

      /* prepare particle data for export */
      for(j = 0; j < nexport; j++)
	{
	  place = DataIndexTable[j].Index;

	  NearestDataIn[j].Pos[0] = P[place].Pos[0];
	  NearestDataIn[j].Pos[1] = P[place].Pos[1];
	  NearestDataIn[j].Pos[2] = P[place].Pos[2];
	  NearestDataIn[j].Hsml = P[place].DM_Hsml;
	  NearestDataIn[j].ID = P[place].ID;
	  NearestDataIn[j].Density = P[place].u.DM_Density;
	  NearestDataIn[j].Count = NgbLoc[place].count;
	  for(k = 0; k < NgbLoc[place].count; k++)
	    {
	      NearestDataIn[j].Dist[k] = R2Loc[place].dist[k];
	      NearestDataIn[j].Index[k] = NgbLoc[place].index[k];
	    }

	  memcpy(NearestDataIn[j].NodeList,
		 DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
	}

      /* exchange particle data */
      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	{
	  sendTask = ThisTask;
	  recvTask = ThisTask ^ ngrp;

	  if(recvTask < NTask)
	    {
	      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		{
		  /* get the particles */
		  MPI_Sendrecv(&NearestDataIn[Send_offset[recvTask]],
			       Send_count[recvTask] * sizeof(struct nearestdata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A,
			       &NearestDataGet[Recv_offset[recvTask]],
			       Recv_count[recvTask] * sizeof(struct nearestdata_in), MPI_BYTE,
			       recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		}
	    }
	}

      myfree(NearestDataIn);
      NearestDataResult = (struct nearestdata_out *) mymalloc(nimport * sizeof(struct nearestdata_out));
      NearestDataOut = (struct nearestdata_out *) mymalloc(nexport * sizeof(struct nearestdata_out));

      /* now do the particles that were sent to us */
      for(j = 0; j < nimport; j++)
	subfind_nearesttwo_evaluate(j, 1, &dummy, &dummy);

      if(i >= NumPartGroup)
	ndone_flag = 1;
      else
	ndone_flag = 0;

      MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

      /* get the result */
      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	{
	  sendTask = ThisTask;
	  recvTask = ThisTask ^ ngrp;
	  if(recvTask < NTask)
	    {
	      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		{
		  /* send the results */
		  MPI_Sendrecv(&NearestDataResult[Recv_offset[recvTask]],
			       Recv_count[recvTask] * sizeof(struct nearestdata_out),
			       MPI_BYTE, recvTask, TAG_DENS_B,
			       &NearestDataOut[Send_offset[recvTask]],
			       Send_count[recvTask] * sizeof(struct nearestdata_out),
			       MPI_BYTE, recvTask, TAG_DENS_B, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		}
	    }
	}

      /* add the result to the local particles */
      for(j = 0; j < nexport; j++)
	{
	  place = DataIndexTable[j].Index;

	  for(k = 0; k < NearestDataOut[j].Count; k++)
	    {
	      if(NgbLoc[place].count >= 1)
		if(NgbLoc[place].index[0] == NearestDataOut[j].Index[k])
		  continue;

	      if(NgbLoc[place].count == 2)
		if(NgbLoc[place].index[1] == NearestDataOut[j].Index[k])
		  continue;

	      if(NgbLoc[place].count < 2)
		{
		  l = NgbLoc[place].count;
		  NgbLoc[place].count++;
		}
	      else
		{
		  if(R2Loc[place].dist[0] > R2Loc[place].dist[1])
		    l = 0;
		  else
		    l = 1;

		  if(NearestDataOut[j].Dist[k] >= R2Loc[place].dist[l])
		    continue;
		}

	      R2Loc[place].dist[l] = NearestDataOut[j].Dist[k];
	      NgbLoc[place].index[l] = NearestDataOut[j].Index[k];

	      if(NgbLoc[place].count == 2)
		if(NgbLoc[place].index[0] == NgbLoc[place].index[1])
		  {
		    /*
		       printf("taaa=%d i=%d  task_0=%d index_0=%d  task_1=%d index_1=%d\n",
		       ThisTask, place,
		       NgbLoc[place].task[0], NgbLoc[place].index[0],
		       NgbLoc[place].task[1], NgbLoc[place].index[1]);

		       printf
		       ("NearestDataOut[j].Count=%d  l=%d k=%d task_0=%d index_0=%d  task_1=%d index_1=%d\n",
		       NearestDataOut[j].Count, l, k, NearestDataOut[j].Task[0], NearestDataOut[j].Index[0],
		       NearestDataOut[j].Task[1], NearestDataOut[j].Index[1]);
		     */

		    endrun(112);
		  }
	    }
	}

      myfree(NearestDataOut);
      myfree(NearestDataResult);
      myfree(NearestDataGet);
    }
  while(ndone < NTask);

  myfree(DataNodeList);
  myfree(DataIndexTable);

  myfree(Dist2list);
  myfree(Ngblist);
}

/*! This function represents the core of the SPH density computation. The
 *  target particle may either be local, or reside in the communication
 *  buffer.
 */
int subfind_nearesttwo_evaluate(int target, int mode, int *nexport, int *nsend_local)
{
  int j, k, n, count;
  MyIDType ID;
  long long index[2];
  double dist[2];
  int startnode, numngb, numngb_inbox, listindex = 0;
  double hmax;
  double h, h2, hinv, hinv3;
  double r2, density;
  MyDouble *pos;

  numngb = 0;

  if(mode == 0)
    {
      ID = P[target].ID;
      density = P[target].u.DM_Density;
      pos = P[target].Pos;
      h = P[target].DM_Hsml;
      count = NgbLoc[target].count;
      for(k = 0; k < count; k++)
	{
	  dist[k] = R2Loc[target].dist[k];
	  index[k] = NgbLoc[target].index[k];
	}
    }
  else
    {
      ID = NearestDataGet[target].ID;
      density = NearestDataGet[target].Density;
      pos = NearestDataGet[target].Pos;
      h = NearestDataGet[target].Hsml;
      count = NearestDataGet[target].Count;
      for(k = 0; k < count; k++)
	{
	  dist[k] = NearestDataGet[target].Dist[k];
	  index[k] = NearestDataGet[target].Index[k];
	}
    }

  h2 = h * h;
  hinv = 1.0 / h;
  hinv3 = hinv * hinv * hinv;

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = NearestDataGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  numngb = 0;

  if(count == 2)
    if(index[0] == index[1])
      {
	/*      printf("T-A-S-K=%d target=%d mode=%d  task_0=%d index_0=%d  task_1=%d index_1=%d\n",
	   ThisTask, target, mode, task[0], index[0], task[1], index[1]);
	 */
	endrun(1232);
      }

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  numngb_inbox =
	    subfind_ngb_treefind_nearesttwo(pos, h, target, &startnode, mode, &hmax, nexport, nsend_local);

	  if(numngb_inbox < 0)
	    return -1;

	  for(n = 0; n < numngb_inbox; n++)
	    {
	      j = Ngblist[n];
	      r2 = Dist2list[n];

	      if(P[j].ID != ID)	/* exclude the self-particle */
		{
		  if(P[j].u.DM_Density > density)	/* we only look at neighbours that are denser */
		    {
		      if(count < 2)
			{
			  dist[count] = r2;
			  index[count] = (((long long) ThisTask) << 32) + j;
			  count++;
			}
		      else
			{
			  if(dist[0] > dist[1])
			    k = 0;
			  else
			    k = 1;

			  if(r2 < dist[k])
			    {
			      dist[k] = r2;
			      index[k] = (((long long) ThisTask) << 32) + j;
			    }
			}
		    }

		  numngb++;
		}
	    }
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = NearestDataGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  if(mode == 0)
    {
      NgbLoc[target].count = count;
      for(k = 0; k < count; k++)
	{
	  R2Loc[target].dist[k] = dist[k];
	  NgbLoc[target].index[k] = index[k];
	}

      if(count == 2)
	if(NgbLoc[target].index[0] == NgbLoc[target].index[1])
	  {
	    /*
	       printf("TASK=%d target=%d  task_0=%d index_0=%d  task_1=%d index_1=%d\n",
	       ThisTask, target,
	       NgbLoc[target].task[0], NgbLoc[target].index[0],
	       NgbLoc[target].task[1], NgbLoc[target].index[1]);
	     */
	    endrun(2);
	  }
    }
  else
    {
      NearestDataResult[target].Count = count;
      for(k = 0; k < count; k++)
	{
	  NearestDataResult[target].Dist[k] = dist[k];
	  NearestDataResult[target].Index[k] = index[k];
	}

      if(count == 2)
	if(NearestDataResult[target].Index[0] == NearestDataResult[target].Index[1])
	  {
	    /*
	       printf("TASK!!=%d target=%d  task_0=%d index_0=%d  task_1=%d index_1=%d\n",
	       ThisTask, target,
	       NearestDataResult[target].Task[0], NearestDataResult[target].Index[0],
	       NearestDataResult[target].Task[1], NearestDataResult[target].Index[1]);
	     */
	    endrun(22);
	  }
    }

  return 0;
}

int subfind_ngb_treefind_nearesttwo(MyDouble searchcenter[3], double hsml, int target, int *startnode,
				    int mode, double *hmax, int *nexport, int *nsend_local)
{
  int numngb, no, p, task, nexport_save, exported = 0;
  struct NODE *current;
  double dx, dy, dz, dist, r2;

#ifdef PERIODIC
  double xtmp;
#endif

  nexport_save = *nexport;

  *hmax = 0;
  numngb = 0;
  no = *startnode;

  while(no >= 0)
    {
      if(no < All.MaxPart)	/* single particle */
	{
	  p = no;
	  no = Nextnode[no];

#ifdef DENSITY_SPLIT_BY_TYPE
	  if(!((1 << P[p].Type) & (DENSITY_SPLIT_BY_TYPE)))
	    continue;
#else
	  if(!((1 << P[p].Type) & (FOF_PRIMARY_LINK_TYPES)))
	    continue;
#endif

	  dist = hsml;
	  dx = NGB_PERIODIC_LONG_X(P[p].Pos[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(P[p].Pos[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(P[p].Pos[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  if((r2 = (dx * dx + dy * dy + dz * dz)) > dist * dist)
	    continue;

	  Dist2list[numngb] = r2;
	  Ngblist[numngb++] = p;
	}
      else
	{
	  if(no >= All.MaxPart + MaxNodes)	/* pseudo particle */
	    {
	      if(mode == 1)
		endrun(12312);

	      if(target >= 0)	/* if no target is given, export will not occur */
		{
		  exported = 1;
		  if(Exportflag[task = DomainTask[no - (All.MaxPart + MaxNodes)]] != target)
		    {
		      Exportflag[task] = target;
		      Exportnodecount[task] = NODELISTLENGTH;
		    }

		  if(Exportnodecount[task] == NODELISTLENGTH)
		    {
		      if(*nexport >= All.BunchSize)
			{
			  *nexport = nexport_save;
			  if(nexport_save == 0)
			    endrun(13004);	/* in this case, the buffer is too small to process even a single particle */
			  for(task = 0; task < NTask; task++)
			    nsend_local[task] = 0;
			  for(no = 0; no < nexport_save; no++)
			    nsend_local[DataIndexTable[no].Task]++;
			  return -1;
			}
		      Exportnodecount[task] = 0;
		      Exportindex[task] = *nexport;
		      DataIndexTable[*nexport].Task = task;
		      DataIndexTable[*nexport].Index = target;
		      DataIndexTable[*nexport].IndexGet = *nexport;
		      *nexport = *nexport + 1;
		      nsend_local[task]++;
		    }

		  DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]++] =
		    DomainNodeIndex[no - (All.MaxPart + MaxNodes)];

		  if(Exportnodecount[task] < NODELISTLENGTH)
		    DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]] = -1;
		}

	      no = Nextnode[no - MaxNodes];
	      continue;
	    }

	  current = &Nodes[no];

	  if(mode == 1)
	    {
	      if(current->u.d.bitflags & (1 << BITFLAG_TOPLEVEL))	/* we reached a top-level node again, which means that we are done with the branch */
		{
		  *startnode = -1;
		  return numngb;
		}
	    }

	  no = current->u.d.sibling;	/* in case the node can be discarded */

	  dist = hsml + 0.5 * current->len;;
	  dx = NGB_PERIODIC_LONG_X(current->center[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(current->center[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(current->center[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  /* now test against the minimal sphere enclosing everything */
	  dist += FACT1 * current->len;
	  if(dx * dx + dy * dy + dz * dz > dist * dist)
	    continue;

	  no = current->u.d.nextnode;	/* ok, we need to open the node */
	}
    }

  *startnode = -1;
  return numngb;
}

#endif