#ifndef FILE_PROCESSINGUNIT
#define FILE_PROCESSINGUNIT

#include "../devicedata/memorymanager.h"
#include "timer.h"

#ifndef CPU_ONLY
    #include "gpu_handle.h"

    #ifdef MAGMA
        #include "GpuMagma_queue.h"
        #include "magma_v2.h"
    #endif
#else
    #pragma message("Cpu only mode is activated. Gpu suppport is disabled. If you wish to activate Gpu support, remove compiler flag CPU_ONLY")
#endif

#ifdef __INTEL_CLANG_COMPILER
  #pragma message("Intel Math Kernel Library is used for acceleration of linear algebra operations.")
  #include <mkl.h>
#else
  #pragma message("CBLAS/LAPACK/LAPACKE are used for acceleration of linear algebra operations.")
  #include <cblas.h>
  #include <lapacke.h>
#endif

#include <map>
#include <type_traits>
#include <iostream>
#include <memory>
#include <string>

template<class floating>
class ProcessingUnitDevice;

template<class floating>
using ProcessingUnit = std::shared_ptr<ProcessingUnitDevice<floating>>;

/**
 * Class OperationType is used for choosing the matrix operation
 * type in several operations of interface ProcessingUnitDevice.
 */
enum class OperationType {
    Identical, //!< taking the matrix without any changes
    Transposed, //!< taking the transposition of the matrix
    Hermitian //!< taking the complex conjugate of the matrix
};

/**
 * Class ProcessingUnitDevice is an abstract interface for easy calls to
 * BLAS/LAPACK/LAPACKE-like function calls on a certain device,
 * e.g. for CUDA/cuSOLVER on a Gpu, or for MAGMA on a Gpu.
 *
 * The floating point type is passed as a template parameter.
 *
 * @tparam floating floating point type
 */
template<class floating>
class ProcessingUnitDevice {
public:
    /**
     * Constructor.
     */
    ProcessingUnitDevice();

    /**
     * Destructor.
     */
    virtual ~ProcessingUnitDevice() = default;

    /**
     * Creates a new instance of a Timer stopwatch on the current device.
     * @return new Timer instance
     */
    virtual Timer create_timer() const = 0;

    /**
     * Returns the MemoryManager associated with the current device.
     * @return MemoryManager
     */
    virtual MemoryManager get_memory_manager() const = 0;

    /**
     * Returns the subclass's name in a human-readable format as a string
     * @return subclass's human-readable name
     */
    virtual std::string display() const = 0;

    /**
     * Function ixamax is an abstraction of calls isamax/idamax/etc.
     * It returns the index of the first element having maximum absolute value.
     *
     * See also
     * https://www.netlib.org/lapack/explore-html/d0/d73/group__aux__blas_ga285793254ff0adaf58c605682efb880c.html
     *
     * @param[in] n number of elements of input vector
     * @param[in] x a C-style array containing (1 + (@param n-1) * abs(@param incx) elements of type @tparam floating
     * @param[in] incx storage spacing between elements of @param x
     * @return index of first element having maximum absolute value
     */
    virtual int ixamax(const int n, const floating * const x, const int incx) const = 0;

    /**
     * Function xaxpy is an abstraction of calls saxpy/daxpy/etc.
     * It performs the calculation y := alpha * x + y
     *
     * See also
     * https://www.netlib.org/lapack/explore-html/de/da4/group__double__blas__level1_ga8f99d6a644d3396aa32db472e0cfc91c.html
     *
     * @param[in] n number of elements in input vectors
     * @param[in] alpha scalar alpha
     * @param[in] x a C-style array containing (1 + (@param n-1) * abs(@param incx) elements of type @tparam floating
     * @param[in] incx storage spacing between elements of @param x
     * @param[in,out] y a C-style array containing (1 + (@param n-1) * abs(@param incy) elements of type @tparam floating
     * @param[in] incy storage spacing between elements of @param y
     */
    virtual void xaxpy(const int n, const floating alpha, const floating * const x, const int incx, floating * const y, const int incy) const = 0;

    /**
     * Function xcopy is an abstraction of calls scopy/dcopy/etc.
     * It performs the copy operation dest := source
     *
     * See also
     * https://www.netlib.org/lapack/explore-html/de/da4/group__double__blas__level1_ga21cdaae1732dea58194c279fca30126d.html
     * @param[in] n number of elements in input vectors
     * @param[in] source a C-style array containing (1 + (@param n-1) * abs(@param inc_source) elements of type @tparam floating
     * @param[in] inc_source storage spacing between elements of @param source
     * @param[out] dest a C-style array containing (1 + (@param n-1) * abs(@param inc_dest) elements of type @tparam floating
     * @param[in] inc_dest storage spacing between elements of @param dest
     */
    virtual void xcopy(const int n, const floating * const source, const int inc_source, floating * const dest, const int inc_dest) const = 0;

    /**
     * Function xdot is an abstraction of calls sdot/ddot/etc.
     * It performs the dot product <x, y>
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/de/da4/group__double__blas__level1_ga75066c4825cb6ff1c8ec4403ef8c843a.html
     *
     * @param[in] n number of elements in input vectors
     * @param[in] x a C-style array containing (1 + (@param n-1) * abs(@param incx) elements of type @tparam floating
     * @param[in] incx spacing between elements of @param x
     * @param[in,out] y a C-style array containing (1 + (@param n-1) * abs(@param incy) elements of type @tparam floating
     * @param[in] incy spacing between elements of @param y
     * @return the value of the dot product <x, y>
     */
    virtual floating xdot(const int n, const floating * const x, const int incx, const floating * const y, const int incy) const = 0;

    /**
     * Function xgemm is an abstraction of calls sgemm/dgemm/etc.
     * It performs the matrix product operation C := alpha * trans_A(A) * trans_B(B) + beta * C
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/d1/d54/group__double__blas__level3_gaeda3cbd99c8fb834a60a6412878226e1.html
     *
     * @param[in] trans_A defines the transformation applied to matrix A (see enum class OperationType)
     * @param[in] trans_B defines the transformation applied to matrix B (see enum class OperationType)
     * @param[in] m the number of rows of matrix trans_A(A) and C
     * @param[in] n the number of columns of matrix trans_B(B) and C
     * @param[in] k the number of columns of matrix C
     * @param[in] alpha scalar applied to A
     * @param[in] A matrix stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] lda the first dimension of A
     * @param[in] B matrix stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] ldb the first dimension of B
     * @param[in] beta scalar applied to C
     * @param[in,out] C matrix stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] ldc the first dimension of C
     */
    virtual void xgemm(const OperationType trans_A, const OperationType trans_B,
                       const int m, const int n, const int k, const floating alpha, const floating * const A,
                       const int lda, const floating * const B, const int ldb, const floating beta, floating * const C, const int ldc) const = 0;

    /**
     * Function xgemv is an abstraction of calls sgemv/dgemv/etc.
     * It performs the matrix times vector operation y := alpha * trans(A) * x + beta * y
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/d7/d15/group__double__blas__level2_gadd421a107a488d524859b4a64c1901a9.html
     *
     * @param[in] trans defines the transformation applied to matrix A (see enum class OperationType)
     * @param[in] m the number of rows of matrix A
     * @param[in] n the number of columns of matrix A
     * @param[in] alpha scalar applied to A
     * @param[in] A matrix stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] lda the first dimension of matrix A
     * @param[in] x A C-style array containing (1 + (@param DIM-1) * abs(@param incx) elements of type @tparam floating. DIM is @param n if trans is OperationType::Identical, and @param m else.
     * @param[in] incx storage spacing between elements of @param x
     * @param[in] beta scalar applied to @param y
     * @param[in,out] y A C-style array containing (1 + (@param DIM-1) * abs(@param incy) elements of type @tparam floating. DIM is @param n if trans is OperationType::Identical, and @param m else.
     * @param[in] incy storage spacing between elements of @param y
     */
    virtual void xgemv(const OperationType trans, const int m, const int n,
                       const floating alpha, floating const * const A, const int lda, floating const * const x, const int incx,
                       const floating beta, floating * const y, const int incy) const = 0;

    /**
     * Function xgetrf is an abstraction of calls sgetrf/dgetrf/etc.
     * It computes the PLU factorization of A general M-by-N matrix A. A is overwritten by L and U,
     * and L has unit diagonal elements.
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/dd/d9a/group__double_g_ecomputational_ga0019443faea08275ca60a734d0593e60.html
     *
     * @param[in] m the number of rows of matrix A
     * @param[in] n the number of columns of matrix A
     * @param[in,out] A matrix stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] lda the first dimension of matrix A
     * @param[out] ipiv permutation array, row i of matrix A is interchanged with row ipiv[i]
     * @param[out] info If == 0, successful exit. If < 0, the i-th argument had illegal value. If > 0, U(i, i) is exactly 0 and cannot be used to solve A system of equations.
     */
    virtual void xgetrf(int * const m, int * const n, floating * const A, int * const lda,
                        int * const ipiv, int * const info) const = 0;

    /**
     * Function xgetri is an abstraction of calls sgetri/dgetri/etc.
     * It computes the inverse of A square matrix using its PLU factorization as calculated by xgetrf.
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/dd/d9a/group__double_g_ecomputational_ga56d9c860ce4ce42ded7f914fdb0683ff.html
     *
     * @param[in] n order of square matrix A
     * @param[in,out] A LU-factorization matrix (as calculated by xgetrf) stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] lda the first dimension of matrix A
     * @param[in] ipiv permutation array from xgetrf
     * @param[out] work is temporary working space in form of A C-style array containing elements of type floating. On exit, if info == 0, work[1] returns optimal @param lwork
     * @param[in] lwork dimension of array @param work. If == -1, the routine calculates the optimal size of array @param work.
     * @param[out] info If == 0, successful exit. If < 0, the i-th argument had illegal value. If > 0, U(i, i) is exactly 0 and cannot be used to solve A system of equations.
     */
    virtual void xgetri(const int * const n, floating * const A, const int * const lda, const int * const ipiv,
                        floating * const work, const int * const lwork, int * const info) const = 0;

    /**
     * Function xgetrs is an abstraction of calls sgetrs/dgetrs/etc.
     * It solves A system of linear equations trans(A) * X = B using its PLU factorization as calculated by xgetrf.
     * Matrix A has to be square, and the result is stored in B.
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/dd/d9a/group__double_g_ecomputational_ga58e332cb1b8ab770270843221a48296d.html
     *
     * @param[in] trans defines the transformation applied to matrix A (see enum class OperationType)
     * @param[in] n order of square matrix A
     * @param[in] nrhs number of right hand sides, i.e. the number of columns of B
     * @param[in] A LU-factorization matrix (as calculated by xgetrf) stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] lda the first dimension of matrix A
     * @param[in] ipiv permutation array from xgetrf
     * @param[in,out] B matrix of right hand sides stored as A column-wise C-style array containing elements of type @tparam floating
     * @param[in] ldb the first dimension of matrix B
     * @param[out] info If == 0, successful exit. If < 0, the i-th argument had illegal value.
     */
    virtual void xgetrs(const OperationType trans, const int * const n, const int * const nrhs,
                        const floating * const A, const int * const lda, const int * const ipiv,
                        floating * const B, const int * const ldb, int * const info) const = 0;

    /**
     * Function xscal is an abstraction of calls sscal/dscal/etc.
     * It performs the scaling operation x := alpha * x.
     *
     * See also
     * http://www.netlib.org/lapack/explore-html/de/da4/group__double__blas__level1_ga793bdd0739bbd0e0ec8655a0df08981a.html
     *
     * @param n number of elements in input vectors
     * @param alpha scalar alpha
     * @param x a C-style array containing (1 + (@param n-1) * abs(@param incx) elements of type @tparam floating
     * @param incx storage spacing between elements of @param x
     */
    virtual void xscal(const int n, const floating alpha, floating * const x, const int incx) const = 0;

    /**
     * Returns true if @tparam floating is float.
     * @return true if @tparam floating is float
     */
    constexpr static bool isFloat();

    /**
     * Returns true if @tparam floating is double.
     * @return true if @tparam floating is double
     */
    constexpr static bool isDouble();
};

/**
 * Class Cpu provides linear algebra operations on the Cpu using
 * BLAS/LAPACK/LAPACKE. All memory allocations also take place in
 * ordinary RAM.
 *
 * @tparam floating floating point type, either float or double
 */
template<class floating>
class Cpu : public ProcessingUnitDevice<floating> {
public:
    using ProcessingUnitDevice<floating>::isFloat;
    using ProcessingUnitDevice<floating>::isDouble;

    /**
     * Constructor
     */
    Cpu() = default;

    /**
     * Destructor
     */
    ~Cpu() override = default;

    Timer create_timer() const override;
    MemoryManager get_memory_manager() const override;
    std::string display() const override;

    int ixamax(const int n, const floating * const x, const int incx) const override;
    void xaxpy(const int n, const floating alpha, const floating * const x, const int incx, floating * const y, const int incy) const override;
    void xcopy(const int n, const floating * const source, const int inc_source, floating * const dest, const int inc_dest) const override;
    floating xdot(const int n, const floating * const x, const int incx, const floating * const y, const int incy) const override;
    void xgemm(const OperationType trans_A, const OperationType trans_B,
               const int M, const int N, const int K, const floating alpha, const floating * const A,
               const int lda, const floating * const B, const int ldb, const floating beta, floating * const C, const int ldc) const override;
    void xgemv(const OperationType trans, const int m, const int n,
               const floating alpha, floating const * const A, const int lda, floating const * const x, const int incx,
               const floating beta, floating * const y, const int incy) const override;
    void xgetrf(int * const m, int * const n, floating * const A, int * const lda,
                int * const ipiv, int * const info) const override;
    void xgetri(const int * const n, floating * const A, const int * const lda, const int * const ipiv,
                floating * const work, const int * const lwork, int * const info) const override;
    void xgetrs(const OperationType trans, const int * const n, const int * const nrhs,
                const floating * const A, const int * const lda, const int * const ipiv,
                floating * const B, const int * const ldb, int * const info) const override;
    void xscal(const int N, const floating alpha, floating * const x, const int incx) const override;

private:
    static std::map<OperationType, CBLAS_TRANSPOSE> to_internal_operation_blas; //!< Converts the OperationType to the corresponding CBLAS_TRANSPOSE type
    static std::map<OperationType, const char * const> to_internal_operation_lapack; //!< Converts the OperationType to the corresponding LAPACK transposition type
    static MemoryManager _device_manager; //!< Memory manager associated with Cpu memory
};

#ifndef CPU_ONLY

/**
 * Class Gpu provides linear algebra operations on the Gpu using
 * cuBLAS/cuSOLVER. All memory allocations also take place in
 * Gpu RAM.
 *
 * @tparam floating floating point type, either float or double
 */
template<class floating>
class Gpu : public ProcessingUnitDevice<floating> {
public:
    using ProcessingUnitDevice<floating>::isFloat;
    using ProcessingUnitDevice<floating>::isDouble;

    /**
     * Constructor
     */
    Gpu() = default;

    /**
     * Destructor
     */
    ~Gpu() override = default;

    Timer create_timer() const override;
    MemoryManager get_memory_manager() const override;
    std::string display() const override;

    /**
     * Returns the instance's cuBLAS handle.
     * @return cuBLAS handle
     */
    auto get_cublas_handle() const;

    /**
     * Returns the instance's cuSOLVER handle.
     * @return cuSOLVER handle
     */
    auto get_cusolver_handle() const;

    int ixamax(const int n, const floating * const x, const int incx) const override;
    void xaxpy(const int n, const floating alpha, const floating * const x, const int incx, floating * const y, const int incy) const override;
    void xcopy(const int n, const floating * const source, const int inc_source, floating * const dest, const int inc_dest) const override;
    floating xdot(const int n, const floating * const x, const int incx, const floating * const y, const int incy) const override;
    void xgemm(const OperationType trans_A, const OperationType trans_B,
               const int M, const int N, const int K, const floating alpha, const floating * const A,
               const int lda, const floating * const B, const int ldb, const floating beta, floating * const C, const int ldc) const override;
    void xgemv(const OperationType trans, const int m, const int n,
               const floating alpha, floating const * const A, const int lda, floating const * const x, const int incx,
               const floating beta, floating * const y, const int incy) const override;
    void xgetrf(int * const m, int * const n, floating * const A, int * const lda,
                int * const ipiv, int * const info) const override;
    void xgetri(const int * const n, floating * const A, const int * const lda, const int * const ipiv,
                floating * const work, const int * const lwork, int * const info) const override;
    void xgetrs(const OperationType trans, const int * const n, const int * const nrhs,
                const floating * const A, const int * const lda, const int * const ipiv,
                floating * const B, const int * const ldb, int * const info) const override;
    void xscal(const int N, const floating alpha, floating * const x, const int incx) const override;

private:
    static std::map<OperationType, cublasOperation_t> to_internal_operation; //!< Converts the OperationType to the corresponding cuBLAS operation type
    static GpuHandle _handle; //!< Gpu handle for cuBLAS/cuSOLVER operations
    static MemoryManager _deviceManager; //!< Memory manager associated with CUDA memory

};

#ifdef MAGMA
/**
 * Class GpuMagma provides linear algebra operations on the Gpu using
 * MAGMA. All memory allocations also take place in Gpu RAM.
 *
 * @tparam floating floating point type, either float or double
 */
template<class floating>
class GpuMagma : public ProcessingUnitDevice<floating> {
public:
    using ProcessingUnitDevice<floating>::isFloat;
    using ProcessingUnitDevice<floating>::isDouble;

    /**
     * Constructor
     */
    GpuMagma() = default;

    /**
     * Destructor
     */
    ~GpuMagma() override = default;

    Timer create_timer() const override;
    MemoryManager get_memory_manager() const override;
    std::string display() const override;

    /**
     * Returns the instance's magma queue.
     */
    auto get_magma_queue() const;

    int ixamax(const int n, const floating * const x, const int incx) const override;
    void xaxpy(const int n, const floating alpha, const floating * const x, const int incx, floating * const y, const int incy) const override;
    void xcopy(const int n, const floating * const source, const int inc_source, floating * const dest, const int inc_dest) const override;
    floating xdot(const int n, const floating * const x, const int incx, const floating * const y, const int incy) const override;
    void xgemm(const OperationType trans_A, const OperationType trans_B,
               const int M, const int N, const int K, const floating alpha, const floating * const A,
               const int lda, const floating * const B, const int ldb, const floating beta, floating * const C, const int ldc) const override;
    void xgemv(const OperationType trans, const int m, const int n,
               const floating alpha, floating const * const A, const int lda, floating const * const x, const int incx,
               const floating beta, floating * const y, const int incy) const override;
    void xgetrf(int * const m, int * const n, floating * const A, int * const lda,
                int * const ipiv, int * const info) const override;
    void xgetri(const int * const n, floating * const A, const int * const lda, const int * const ipiv,
                floating * const work, const int * const lwork, int * const info) const override;
    void xgetrs(const OperationType trans, const int * const n, const int * const nrhs,
                const floating * const A, const int * const lda, const int * const ipiv,
                floating * const B, const int * const ldb, int * const info) const override;
    void xscal(const int N, const floating alpha, floating * const x, const int incx) const override;

private:
    static std::map<OperationType, magma_trans_t> to_internal_operation; //!< Converts the OperationType to the corresponding MAGMA transposition type
    static GpuMagmaQueue _queue; //!< MAGMA queue for MAGMA operations
    static MemoryManager _device_manager; //!< Memory manager associated with CUDA memory
};

/**
 * Class ReplacementNumber is used for passing the operation to GpuMixed, which is then calculated via
 * GpuMagma instead of ordinary Gpu processing unit.
 */
enum class ReplacementNumber {
    IXAMAX,
    XAXPY,
    XCOPY,
    XDOT,
    XGEMM,
    XGEMV,
    XGETRF,
    XGETRS,
    XSCAL
};

/**
 * Class GpuMixed provides linear algebra operations on the Gpu using
 * cuBLAS/cuSOLVER, except for the one operation specified by replacementNumber,
 * which is calculated using MAGMA.
 * All memory allocations also take place in Gpu RAM.
 *
 * @tparam floating floating point type, either float or double
 */
template<class floating>
class GpuMixed : public ProcessingUnitDevice<floating> {
public:
    /**
     * Constructor
     */
    GpuMixed(const ReplacementNumber replacementNumber = ReplacementNumber::XGETRF);

    /**
     * Destructor
     */
    ~GpuMixed() override = default;

    Timer create_timer() const override;

    MemoryManager get_memory_manager() const override;

    std::string display() const override;

    int ixamax(const int n, const floating * const x, const int incx) const override;
    void xaxpy(const int n, const floating alpha, const floating * const x, const int incx, floating * const y, const int incy) const override;
    void xcopy(const int n, const floating * const source, const int inc_source, floating * const dest, const int inc_dest) const override;
    floating xdot(const int n, const floating * const x, const int incx, const floating * const y, const int incy) const override;
    void xgemm(const OperationType trans_A, const OperationType trans_B,
               const int M, const int N, const int K, const floating alpha, const floating * const A,
               const int lda, const floating * const B, const int ldb, const floating beta, floating * const C, const int ldc) const override;
    void xgemv(const OperationType trans, const int m, const int n,
               const floating alpha, floating const * const A, const int lda, floating const * const x, const int incx,
               const floating beta, floating * const y, const int incy) const override;
    void xgetrf(int * const m, int * const n, floating * const A, int * const lda,
                int * const ipiv, int * const info) const override;
    void xgetri(const int * const n, floating * const A, const int * const lda, const int * const ipiv,
                floating * const work, const int * const lwork, int * const info) const override;
    void xgetrs(const OperationType trans, const int * const n, const int * const nrhs,
                const floating * const A, const int * const lda, const int * const ipiv,
                floating * const B, const int * const ldb, int * const info) const override;
    void xscal(const int N, const floating alpha, floating * const x, const int incx) const override;

    ReplacementNumber get_replacement_number() const;

private:
    static Gpu<floating> _gpu; //!< Gpu processing unit
    static GpuMagma<floating> _gpu_magma; //!< MAGMA processing unit
    ReplacementNumber _replacementNumber; //!< current instance's operation which should be exectued on MAGMA instead of cuBLAS/cuSOLVER
};
#endif
#endif

#include "processingunit.hpp"
#include "processingunit_cpu.hpp"

#ifndef CPU_ONLY
    #include "processingunit_gpu.hpp"
    #ifdef MAGMA
        #include "processingunit_gpu_magma.hpp"
        #include "processingunit_gpu_mixed.hpp"
    #endif
#endif

#endif


#ifndef __DIRECT_KNN_H__
#define __DIRECT_KNN_H__

#include <mpi.h>
#include <utility>
#include <cmath>
#include <vector>
#include <string>
#include <blas.h>
#include <omp.h>
#include <utility>

#define KNN_MAX_BLOCK_SIZE 512
#define KNN_MAX_MATRIX_SIZE 2e7L

// MPI message tags
enum {
  TAG_R,
  TAG_Q,
  TAG_KMIN,
  TAG_ID,
  TAG_SIZE
};

enum {
  VERTICAL,
  HORIZONTAL
};

/**
 * Options struct used to specify how a rectangular-partitioned query should be performed.
 */
struct directQueryParams
{
	char queryType; ///< Perform a k-nearest ('K') or r-near ('R') search.
	int k; ///< Number of neighbors to find (if queryType is 'K').
	double r; ///< Radius for r-near search (if queryType is 'R').
	int refParts; ///< Partition reference points into this many approximately equal parts.
	int queryParts; ///< Partition query points into this many approximately equal parts.
};

struct directQueryResults
{
	std::pair<double, long> *neighbors;
	long *neighborCounts;
};

class maxheap_comp {
    public:
        bool operator() (const std::pair<double, long> &a, const std::pair<double, long> &b) {
            double diff = fabs(a.first-b.first)/a.first;
            if( std::isinf(diff) || std::isnan(diff) ) {      // 0/0 or x/0
                return a.first < b.first;
            }
            if( diff < 1.0e-8 ) {
                return a.second < b.second;
            }
            return a.first < b.first;
        }
};

namespace knn {

   /**
    * Compute the k nearest neighbors of each query point (OBSOLETE: For debugging purposes only; use directKQueryLowMem instead)..
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
    * \param n number of local reference point
    * \param m number of local query points
    * \param k number of neighbors to find for each query point.
    * \param dim dimensionality of reference/query points
    * \return An array of pairs containing the *squared* distance to each point and its
    *	index in the local array.  Note that this is the index of the /point/ rather
    *	than the index of its location in the actual array of doubles.  To access the
    *	point, use (point index)*dim to index ref.
    */
   std::pair<double, long> *directKQuery
              ( double *ref, double *query, long n, long m, long k, int dim);

   /**
    * Compute the k nearest neighbors of each query point (OBSOLETE: For debugging purposes only; use directKQueryLowMem instead)..
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
    * \param n number of local reference point
    * \param m number of local query points
    * \param k number of neighbors to find for each query point.
    * \param dim dimensionality of reference/query points
    * \param result [out] An array of m*k pairs (allocated by caller) containing the
    * *squared* distance to each point and its index in the local array.
    * Note that this is the index of the /point/ rather than the index of its location
    * in the actual array of doubles.  To access the point, use (point index)*dim to index ref.
    * Results for query point i begin at result[i*k].
    * \param dist An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least n*B, where B is the value returned by getBlockSize, and
    * deallocated at some point after the function returns.
    * \param sqnormr An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least n and deallocated at some point after the function returns.
    * \param sqnormq An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least B, where B is the value returned by getBlockSize, and
    * deallocated at some point after the function returns.
    */
 void directKQuery_small_a2a( double *ref, long n, int dim, int k,
                              std::pair<double, long> *result,
                              double *dist = NULL, double *sqnormr = NULL, double *sqnormq = NULL);

   /**
    * Compute the k nearest neighbors of each query point with a smaller memory footprint and usually better performance.
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
    * \param n number of local reference point
    * \param m number of local query points
    * \param k number of neighbors to find for each query point.
    * \param dim dimensionality of reference/query points
    * \param result [out] An array of m*k pairs (allocated by caller) containing the
    * *squared* distance to each point and its
    * index in the local array.  Note that this is the index of the /point/ rather
    * than the index of its location in the actual array of doubles.  To access the
    * point, use (point index)*dim to index ref. Results for query point i begin at result[i*k].
    * \param dist An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least n*B, where B is the value returned by getBlockSize, and
    * deallocated at some point after the function returns.
    * \param sqnormr An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least n and deallocated at some point after the function returns.
    * \param sqnormq An array used for calculating inter-point distances. If NULL or unspecified,
    * the function allocates and deallocates this array internally.  Otherwise, it must be
    * allocated size at least B, where B is the value returned by getBlockSize, and
    * deallocated at some point after the function returns.
    */
   void directKQueryLowMem
              ( double *ref, double *query, long n, long m, long k, int dim, std::pair<double, long> *result,
                double *dist = NULL, double* sqnormr = NULL, double* sqnormq = NULL  );

   /**
    * Find all points in ref that lie within distance sqrt(R) of each
    * point in query.
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
    * \param n number of local reference point
    * \param m number of local query points
    * \param R The *square* of the search radius.
    * \param dim dimensionality of reference/query points
    * \param glob_ids An array of length n containing the global id of each ref point.
    * \param neighbor_count [out] An array of length m containing the number of
    * neighbors found for the corresponding query point (allocated internally).
    * \param neighbors [out] An array of length sum(neighbor_count) containing
    * pairs of ids and squared distances for each neighbor found (allocated
    * internally).
    */
   void directRQuery
           ( double *ref, double *query, long n, long m, double R, int dim, long* glob_ids,
             int **neighbor_count, std::pair<double, long> **neighbors );

  /**
   * Find all points in ref that lie within distance sqrt(R[i]) of query point i.
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
   * \param n number of local reference point
   * \param m number of local query points
   * \param R Array of length m, the *square* of the search radius for each point.
   * \param dim dimensionality of reference/query points
   * \param glob_ids An array of length n containing the global id of each ref point.
   * \param neighbor_count [out] An array of length m containing the number of
   * neighbors found for the corresponding query point (allocated internally).
   * \param neighbors [out] An array of length sum(neighbor_count) containing
   * pairs of ids and squared distances for each neighbor found (allocated
   * internally).
   */
  void directRQueryIndividual
  ( double *ref, double *query, long n, long m, double *R, int dim, long* glob_ids,
   int **neighbor_count, std::pair<double, long> **neighbors );

  /**
   * Find all points in ref that lie within distance sqrt(R[i]) of query point i.
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
   * \param n number of local reference point
   * \param m number of local query points
   * \param R Array of length m, the *square* of the search radius for each point.
   * \param dim dimensionality of reference/query points
   * \param glob_ids An array of length n containing the global id of each ref point.
   * \param neighbor_count [out] An array of length m containing the number of
   * neighbors found for the corresponding query point (allocated internally).
   * \param neighbors [out] An array of length sum(neighbor_count) containing
   * pairs of ids and squared distances for each neighbor found (allocated
   * internally).
   */
void directRQueryIndividualK
( double *ref, double *query, long n, long m, int k, double *R, int dim, long *glob_ids,
 int **neighbor_count, std::pair<double, long> **neighbors  );

   /**
    * Computes the distances between all query points and all reference points.
    * \param ref an n*dim-length array of data points.
    * \param query an m*dim-length array of query points.
    * \param n number of reference point
    * \param m number of query points
    * \param dim dimensionality of reference/query points
    * \param dist [out] m*n matrix of the *squared* distances from query points to reference points.
    * \param sqnormr An array of length n used for storing the squared norm of each reference point.
    * If NULL or omitted, allocated and deallocated internally.
    * \param sqnromq An array of length m used for storing the squared norm of each reference point.
    * If NULL or omitted, allocated and deallocated internally.
    * \param useSqnormrInput If true, use precomputed squared norm values contained in sqnormr when
    * this function is called; if false, compute squared norms internally.
    */
   void compute_distances
              ( double *ref, double *query, long n, long m, int dim, double *dist,
                  double* sqnormr = NULL, double* sqnormq = NULL, bool useSqnormrInput = false );

   /**
    * Computes the square of the norm of each row of a matrix.
    * \param a The input matrix (row-major).
    * \param n The number of rows of a.
    * \param dim The number of columns of a.
    * \param b [out] The output array (length n).
    */
   void sqnorm ( double *a, long n, int dim, double *b);

   /**
    * Find the k-nearest neighbors of each query point using distributed cyclic-shift algorithm.
    * \param ref an nlocal*dim-length array of data points.
    * \param query an mlocal*dim-length array of query points.
    * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
    * \param nlocal Number of local reference points.
    * \param mlocal Number of local query points.
    * \param k Number of neighbors to find for each query point.
    * \param dim Dimensionality of reference/query points.
    * \param comm The MPI communicator to use.
    * \return An array of pairs containing the distance to each neighbor and its index;
    * neighbors for point i begin at location i*k.
    * \note All input arrays MUST be allocated with malloc (or equivalent) and deallocated with free;
    * new and delete[] may result in a segmentation fault.
    */
   std::pair<double, long> * dist_directKQuery( double* &ref, double *query,
   						 long* &glob_ids,
   						 int nlocal, int mlocal,
   						 int k,
   						 int dim,
   						 MPI_Comm comm );

   /**
    * Find all reference points lying within distance sqrt(R) of each query point.
    * \param ref an nlocal*dim-length array of data points.
    * \param query an mlocal*dim-length array of query points.
    * \param n Number of global reference points
    * \param m Number of global query points
    * \param nlocal Number of local reference points
    * \param mlocal Number of local query points
    * \param R The *square* of the search radius.
    * \param dim Dimensionality of reference/query points
    * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
    * \param rneighbors [out] An externally allocated array of mlocal vectors, used to store the neighbors of each
    * query point.
    * \param comm The MPI communicator to use.
    */
    void dist_directRQuery
              ( double* &ref, double *query, long n, long m, int nlocal, int mlocal, double R,
                int dim, long* &glob_ids, std::vector< std::pair<double, long> >* rneighbors,
                MPI_Comm comm  );

   /**
    * Find all reference points lying within distance sqrt(R) of each query point,
    * up to a maximum of max_neighbors (not necessarily the nearest).
    * \param ref an nlocal*dim-length array of data points.
    * \param query an mlocal*dim-length array of query points.
    * \param n Number of global reference points
    * \param m Number of global query points
    * \param nlocal Number of local reference points
    * \param mlocal Number of local query points
    * \param R The *square* of the search radius.
    * \param dim Dimensionality of reference/query points
    * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
    * \param rneighbors [out] An externally allocated array of mlocal vectors, used to store the neighbors of each
    * query point.
    * \param max_neighbors The maximum number of neighbors returned for any given query point.
    * \param comm The MPI communicator to use.
    */
    void dist_directRQuery
    ( double* &ref, double *query, long n, long m, int nlocal, int mlocal, double R,
     int dim, long* &glob_ids, std::vector< std::pair<double, long> >* rneighbors, int max_neighbors,
     MPI_Comm comm  );

  /**
   * Find all reference points lying within distance sqrt(R[i]) of query point i.
   * \param ref an nlocal*dim-length array of data points.
   * \param query an mlocal*dim-length array of query points.
   * \param n Number of global reference points
   * \param m Number of global query points
   * \param nlocal Number of local reference points
   * \param mlocal Number of local query points
   * \param R Array of length m, the *square* of the search radius for each query point.
   * \param dim Dimensionality of reference/query points
   * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
   * \param rneighbors [out] An externally allocated array of mlocal vectors, used to store the neighbors of each
   * query point.
   * \param comm The MPI communicator to use.
   */
  void dist_directRQueryIndividual
  ( double* &ref, double *query, long n, long m, int nlocal, int mlocal, double *R,
   int dim, long* &glob_ids, std::vector< std::pair<double, long> >* rneighbors,
   MPI_Comm comm  );

  /**
   * Find all reference points lying within distance sqrt(R[i]) of query point i.
   * \param ref an nlocal*dim-length array of data points.
   * \param query an mlocal*dim-length array of query points.
   * \param n Number of global reference points
   * \param m Number of global query points
   * \param nlocal Number of local reference points
   * \param mlocal Number of local query points
   * \param R Array of length m, the *square* of the search radius for each query point.
   * \param dim Dimensionality of reference/query points
   * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
   * \param rneighbors [out] An externally allocated array of mlocal vectors, used to store the neighbors of each
   * query point.
   * \param comm The MPI communicator to use.
   */
   void dist_directRQueryIndividualK
( double* &ref, double *query, long n, long m, int nlocal, int mlocal, double *R, int k,
 int dim, long* &glob_ids, std::vector< std::pair<double, long> >* rneighbors,
 MPI_Comm comm  );

  /**
   * Find all reference points lying within distance sqrt(R[i]) of query point i,
   * up to a maximum of max_neighbors points (not necessarily the nearest).
   * \param ref an nlocal*dim-length array of data points.
   * \param query an mlocal*dim-length array of query points.
   * \param n Number of global reference points
   * \param m Number of global query points
   * \param nlocal Number of local reference points
   * \param mlocal Number of local query points
   * \param R Array of length m, the *square* of the search radius for each query point.
   * \param dim Dimensionality of reference/query points
   * \param glob_ids An array of length nlocal containing the unique ID of each reference point stored locally.
   * \param rneighbors [out] An externally allocated array of mlocal vectors, used to store the neighbors of each
   * query point.
   * \param max_neighbors The maximum number of neighbors to return for any single query point.
   * \param comm The MPI communicator to use.
   */
   void dist_directRQueryIndividual
   ( double* &ref, double *query, long n, long m, int nlocal, int mlocal, double *R,
    int dim, long* &glob_ids, std::vector< std::pair<double, long> >* rneighbors, int max_neighbors,
    MPI_Comm comm  );

   /**
    * Merges a and b and keeps only the k smallest from each of the m sets of distance/index pairs.
    * \param a one array of m*k distance+index pairs
    * \param b the other array of m*k distance+index pairs
    * \param m the number sets of size k
    * \param k k
    * \return A newly allocated array of size m*k
    */
   std::pair<double, long> *kmin_merge( std::pair<double, long> *a, std::pair<double, long> *b, long m, long k );

   /**
    * Reads reference and query points from files in parallel and partitions them for a query with the rectangular algorithm..
    * \param refFile The name of the file which contains the reference points.
    * \param queryFile The name of the file which contains the query points.
    * \param ref [out] an n*dim array of data points.
    * \param query [out] an m*dim array of query points.
    * \param n number of reference points.
    * \param m number of query points.
    * \param dim dimensionality of reference/query points.
    * \param refParts number of parts to split reference points into (refParts * queryParts = processes).
    * \param queryParts number of parts to split query points into (refParts * queryParts = processes).
    * \param comm The MPI communicator to use.
    * \return A pair object containing the number of reference points (first) and query points (second) stored at this process.
    */
   std::pair<int, int> partitionPoints(std::string refFile, std::string queryFile, double *& ref, double *& query, long n, long m, int dim, int refParts, int queryParts, MPI_Comm comm);

   /**
    * Returns the number of local elements in the partitioning used by dist_directKQuery.
    * \param rank This process's MPI rank.
    * \param size The size of the MPI communicator used.
    * \param num The global number of elements.
    * \return The number of elements this process stores locally.
    */
   inline int getNumLocal( int rank, int size, long num ) {
      if( rank < num%size)
         return std::ceil( (double)num / (double)size );
      else
         return num / size;
   }

   /**
    * Returns the offset of the locally stored elements in the global array.
    * \param rank This process's MPI rank.
    * \param size The size of the MPI communicator used.
    * \param num The global number of elements.
    * \return The global array offset.
    */
   inline long getGlobalArrayOffset( int rank, int size, long num ) {
      if( rank < num%size )
         return std::ceil( (double)num / (double)size ) * rank;
      else
         return std::ceil( (double)num / (double)size ) * (num%size) + (rank-(num%size)) * (num/size);
   }

   /**
    * Repartitions points with replication.
    * \param points The points to repartition (modified).
    * \param localPointCount The number of points the process has.
    * \param dim The dimensionality of the points.
    * \param globalIDs The global ID associated with each point (modified).
    * \param partitionCount The number of pieces to partition the points into (the number of replications is implicitly given by size / partitionCount)
    * \param comm The communicator to use.
    * \return The new number of points the process has.
   */
   int directRectRepartition(double *&points, int localPointCount, int dim, int *&globalIDs, int partitionCount, int direction, MPI_Comm comm);

   /**
    * Performs a direct kNN or rNN query using the rectangular-partitioned algorithm, automatically repartitioning points..
    * \param refPts The reference points (modified).
    * \param queryPts The query points (modified).
    * \param localRefCount The number of reference points the process has (modified).
    * \param localQueryCount The number of query points the process has (modified).
    * \param dim The dimensionality of the points.
    * \param refIDs The global IDS of the reference points the process has (modified).
    * \param queryIDs The global IDs of the query points the process has (modified).
    * \param params The parameters of the direct kNN or rNN query.
    * \param comm The communicator to use.
    * \return The result of the query.
   */
   directQueryResults directRectRepartitionAndQuery(double *&refPts, double *&queryPts, long& localRefCount, long& localQueryCount, int dim, int *&refIDs, int *&queryIDs, directQueryParams params, MPI_Comm comm);

   /**
    * Performs a rNN query on a rectangularly partitioned set of points.
    * \param refPoints The reference points.
    * \param queryPoints The query points.
    * \param localRefCount The number of reference points the process has.
    * \param localQueryCount The number of query points the process has.
    * \param dim The dimensionality of the points.
    * \param r The search radius of the query.
    * \param queryComm The communicator to use (all processes within this comm should have the same query points).
    * \return The result of the query.
   */
   directQueryResults directQueryRectR(double *refPoints, double *queryPoints, int localRefCount, int localQueryCount, int dim, double r, MPI_Comm queryComm);

   /**
    * Performs a kNN query on a rectangularly partitioned set of points.
    * \param refPoints The reference points.
    * \param queryPoints The query points.
    * \param localRefCount The number of reference points the process has.
    * \param localQueryCount The number of query points the process has.
    * \param dim The dimensionality of the points.
    * \param k The number of neighbors to find for each query point.
    * \param queryComm The communicator to use (all processes within this comm should have the same query points).
    * \return The result of the query.
   */
   directQueryResults directQueryRectK(double *refPoints, double *queryPoints, int localRefCount, int localQueryCount, int dim, int k, MPI_Comm queryComm);

   /**
    * Determine the number of query points that will be handled by each iteration of the LowMem queries.
    * \param n The number of reference points.
    * \param m The number of query points.
    * \return The block size the query will use.  Can be multiplied by n to determine size of distance matrix.
    */
   inline int getBlockSize(long n, long m) {
     int blocksize;
     if( m > KNN_MAX_BLOCK_SIZE || n > 10000L) {
       blocksize = std::min((long)KNN_MAX_BLOCK_SIZE, m); //number of query points handled in a given iteration
       if(n * (long)blocksize > KNN_MAX_MATRIX_SIZE) blocksize = std::min((long)(KNN_MAX_MATRIX_SIZE/n), (long)blocksize); //Shrink block size if n is huge.
       blocksize = std::max(blocksize, omp_get_max_threads()); //Make sure each thread has some work.
     } else {
        blocksize = m;
     }
     return blocksize;
   }

}
#endif


/*
** compute edge densities
**
** G.Lohmann, Feb 2015
*/

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_histogram.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void GetRank(float *data,gsl_vector *v,gsl_permutation *perm,gsl_permutation *rank);
extern float EdgeCorr(const float *data1,const float *data2,int,int);

int GetAddr(VImage mapimage,int bi,int ri,int ci,int m,int k,int l)
{
  int b = bi+m;
  int r = ri+k;
  int c = ci+l;
  if (b < 0 || r < 0 || c < 0) return -1;
  if (b >= VImageNBands(mapimage) || r >= VImageNRows(mapimage) || c >= VImageNColumns(mapimage)) return -1;
  int ii = (long)VPixel(mapimage,b,r,c,VInteger);
  return ii;
}

int VEdgeNeigbours(size_t id,VImage map,VImage mapimage,int adjdef,int *x)
{
  int nslices = VImageNBands(mapimage);
  int nrows = VImageNRows(mapimage);
  int ncols = VImageNColumns(mapimage);

  int bi = VPixel(map,0,0,id,VShort);
  int ri = VPixel(map,0,1,id,VShort);
  int ci = VPixel(map,0,2,id,VShort);
  if (bi < 1 || bi >= nslices-2) return 0;
  if (ri < 1 || ri >= nrows-2) return 0;
  if (ci < 1 || ci >= ncols-2) return 0;

  int wn=1,rad2=0;
  if (adjdef == 3) {
    wn = 2;
    rad2 = 2*2;
  }

  int n=0;
  int k,l,m;
  for (m=-wn; m<=wn; m++) {
    for (k=-wn; k<=wn; k++) {
      for (l=-wn; l<=wn; l++) {
      	int jj = GetAddr(mapimage,bi,ri,ci,m,k,l);
      	if (jj < 0) continue;  /* outside of brain mask */

      	if (adjdef == 0) {     /* 6 adjacency */
      	  if (ABS(m)+ABS(k)+ABS(l) > 1) continue;
      	}
      	if (adjdef == 1) {     /* 18-adjacency */
      	  if (ABS(m) > 0 && ABS(k) > 0 && ABS(l) > 0) continue;
      	}
	if (adjdef == 3) {     /* sphere */
	  if (m*m + k*k + l*l > rad2) continue;
	}
      	x[n] = jj;
      	n++;
      }
    }
  }
  return n;
}

void VCheckImage(VImage src)
{
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,src);
  FILE *out_file = fopen("test.v","w");
  VWriteFile (out_file, out_list);
}

size_t EdgeDensity(float *E,int *I,int *J,size_t nedges_estimated,
		   gsl_histogram *TedHist,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
		   VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float zthreshold,
		   int nperm,int numperm,int step,float noise_cutoff,int metric)
{
  size_t i;
  size_t nvox=SNR1->size1;
  int nt=SNR1->size2;
  size_t progress=0;
  int rad2 = (int)(elength*elength);
  float tiny=1.0e-6;
  gsl_set_error_handler_off ();

  int iisel=4;

  /* size of neighbourhood */
  int nadj=0;
  if (adjdef == 0) nadj = 7;
  else if (adjdef == 1) nadj = 19;
  else if (adjdef == 2) nadj = 27;
  else if (adjdef == 3) nadj = 33;
  else VError(" illegal adjdef %d",adjdef);
  size_t nedges=0;
  int minadj = 1;

  /* histogram init */
  size_t nbins = gsl_histogram_bins (TedHist);
  double hmax = gsl_histogram_max (TedHist);
  double hmin = gsl_histogram_min (TedHist);

#pragma omp parallel for shared(nedges,progress,TedHist,E,I,J) schedule(dynamic)
  for (i=0; i<nvox; i+=step) {
    if (i%1000 == 0) fprintf(stderr," %ld000\r",(long)progress++);

    int bi = (int)VPixel(map,0,0,i,VShort);
    int ri = (int)VPixel(map,0,1,i,VShort);
    int ci = (int)VPixel(map,0,2,i,VShort);

    int iselect=0;

    int roiflagi = 0;
    if (roi != NULL) {
      if (VGetPixel(roi,bi,ri,ci) > 0.5) roiflagi = 1;
    }

    const float *datax1 = gsl_matrix_float_const_ptr(SNR1,i,0);
    const float *datax2 = gsl_matrix_float_const_ptr(SNR2,i,0);

    int *x = (int *) VCalloc(nadj,sizeof(int));
    int *y = (int *) VCalloc(nadj,sizeof(int));

    gsl_histogram *tmphist = gsl_histogram_alloc (nbins);
    gsl_histogram_set_ranges_uniform (tmphist,hmin,hmax);
    gsl_histogram_reset(tmphist);

    int nadjx = VEdgeNeigbours(i,map,mapimage,adjdef,x);
    if (nadjx < minadj) continue;

    size_t j=0;
    for (j=0; j<i; j+=step) {
      int bj = (int)VPixel(map,0,0,j,VShort);
      int rj = (int)VPixel(map,0,1,j,VShort);
      int cj = (int)VPixel(map,0,2,j,VShort);
      int d = SQR(bi-bj) + SQR(ri-rj) + SQR(ci-cj);
      if (d < rad2) continue;

      int roiflagj = 0;
      if (roi != NULL) {
	if (VGetPixel(roi,bj,rj,cj) > 0.5) roiflagj = 1;
	if (roiflagi + roiflagj != 1) continue;
      }

      const float *datay1 = gsl_matrix_float_const_ptr(SNR1,j,0);
      const float *datay2 = gsl_matrix_float_const_ptr(SNR2,j,0);

      /* edge z-value */
      double z1 = EdgeCorr(datax1,datay1,nt,metric);
      double z2 = EdgeCorr(datax2,datay2,nt,metric);
      double z = (z1-z2);
      if (z < zthreshold) continue;

      int nadjy = VEdgeNeigbours(j,map,mapimage,adjdef,y);
      if (nadjy < minadj) continue;

      /* use every 'step' value, only when estimating noise cutoff, i.e. step > 1 */
      if (iselect%iisel == 0 && step > 1) {
	iselect = 0;
      }
      else if (iselect%iisel != 0 && step > 1) {
	iselect++;
	continue;
      }

      /* inspect local neighbourhood */
      int r,s;
      double kx=0.0,nx=0.0;
      for (s=0; s<nadjx; s++) {
      	size_t ii = (size_t) x[s];
        int bii = (int)VPixel(map,0,0,ii,VShort);
        int rii = (int)VPixel(map,0,1,ii,VShort);
        int cii = (int)VPixel(map,0,2,ii,VShort);

	const float *dataxx1 = gsl_matrix_float_const_ptr(SNR1,ii,0);
	const float *dataxx2 = gsl_matrix_float_const_ptr(SNR2,ii,0);

      	for (r=0; r<nadjy; r++) {
      	  size_t jj = (size_t) y[r];
          int bjj = (int)VPixel(map,0,0,jj,VShort);
          int rjj = (int)VPixel(map,0,1,jj,VShort);
          int cjj = (int)VPixel(map,0,2,jj,VShort);

          int d2 = SQR(bii-bjj) + SQR(rii-rjj) + SQR(cii-cjj);
          if (d2 < rad2) continue;

	  const float *datayy1 = gsl_matrix_float_const_ptr(SNR1,jj,0);
	  const float *datayy2 = gsl_matrix_float_const_ptr(SNR2,jj,0);

	  /* edge z-value */
	  double z1 = EdgeCorr(dataxx1,datayy1,nt,metric);
	  double z2 = EdgeCorr(dataxx2,datayy2,nt,metric);
	  double z = (z1-z2);
          if (ABS(z) < tiny) continue;
      	  if (z > zthreshold) kx++;
          nx++;
     	}
      }
      if (nx < 1.0) continue;
      double ted = kx/nx;

      if (ted < hmin) ted = hmin;
      if (ted > hmax-tiny) ted = hmax-tiny;
      gsl_histogram_increment (tmphist,ted);

      if (E == NULL) continue;
      if (nperm != numperm) continue;
      if (ted < noise_cutoff) continue;

      #pragma omp critical
      {
	if (nedges >= nedges_estimated) {
	  VError(" alloc, nedges_estimated:  %lu",nedges_estimated);
	}
	E[nedges] = ted;    /* task-based edge density */
	I[nedges] = i;      /* voxel address i */
	J[nedges] = j;      /* voxel address j */
	nedges++;
      }
    }
#pragma omp critical
    {
      gsl_histogram_add (TedHist,tmphist);
    }
    gsl_histogram_free (tmphist);
    VFree(x);
    VFree(y);
  }
  return nedges;
}

/* estimate number of truly needed edges */
size_t EstimateEdges(float *E,int *I,int *J,size_t old_estimate,
		     gsl_histogram *TedHist,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
		     VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float zthr,
		     float noise_cutoff,int metric)
{
  float tmp_cutoff = -1.0;
  int step = 2;
  int nperm=0,numperm=1;

  gsl_histogram_reset(TedHist);
  EdgeDensity(E,I,J,old_estimate,TedHist,SNR1,SNR2,n1,n2,roi,map,mapimage,(int)adjdef,elength,zthr,
	      nperm,numperm,step,tmp_cutoff,metric);

  double sd = gsl_histogram_sigma (TedHist);
  if (sd < 1.0e-6) VError(" stdev of TedHist: %f",sd);

  size_t i0 = 0;
  if (gsl_histogram_find (TedHist,(double)noise_cutoff,&i0) != GSL_SUCCESS)
    VError (" err finding noise cutoff index, %f",noise_cutoff);

  size_t hbins = gsl_histogram_bins(TedHist);
  gsl_histogram_pdf *cdf = gsl_histogram_pdf_alloc(hbins);
  gsl_histogram_pdf_init (cdf,TedHist);

  double qx = 1.0 - cdf->sum[i0];
  if (qx > 1) qx = 1.0;
  size_t new_estimate = (size_t) (qx*(double)old_estimate);

  gsl_histogram_reset(TedHist);
  gsl_histogram_pdf_free (cdf);
  return new_estimate;
}


/*
 * 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:43:55 EST 1999 */

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

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -real2hc 12 */

/*
 * This function contains 38 FP additions, 8 FP multiplications,
 * (or, 34 additions, 4 multiplications, 4 fused multiply/add),
 * 18 stack variables, and 24 memory accesses
 */
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);

/*
 * 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_real2hc_12(const fftw_real *input, fftw_real *real_output, fftw_real *imag_output, int istride, int real_ostride, int imag_ostride)
{
     fftw_real tmp5;
     fftw_real tmp25;
     fftw_real tmp11;
     fftw_real tmp23;
     fftw_real tmp30;
     fftw_real tmp35;
     fftw_real tmp10;
     fftw_real tmp26;
     fftw_real tmp12;
     fftw_real tmp18;
     fftw_real tmp29;
     fftw_real tmp34;
     fftw_real tmp31;
     fftw_real tmp32;
     ASSERT_ALIGNED_DOUBLE;
     {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = input[0];
	  tmp2 = input[4 * istride];
	  tmp3 = input[8 * istride];
	  tmp4 = tmp2 + tmp3;
	  tmp5 = tmp1 + tmp4;
	  tmp25 = tmp1 - (K500000000 * tmp4);
	  tmp11 = tmp3 - tmp2;
     }
     {
	  fftw_real tmp19;
	  fftw_real tmp20;
	  fftw_real tmp21;
	  fftw_real tmp22;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp19 = input[9 * istride];
	  tmp20 = input[istride];
	  tmp21 = input[5 * istride];
	  tmp22 = tmp20 + tmp21;
	  tmp23 = tmp19 - (K500000000 * tmp22);
	  tmp30 = tmp19 + tmp22;
	  tmp35 = tmp21 - tmp20;
     }
     {
	  fftw_real tmp6;
	  fftw_real tmp7;
	  fftw_real tmp8;
	  fftw_real tmp9;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp6 = input[6 * istride];
	  tmp7 = input[10 * istride];
	  tmp8 = input[2 * istride];
	  tmp9 = tmp7 + tmp8;
	  tmp10 = tmp6 + tmp9;
	  tmp26 = tmp6 - (K500000000 * tmp9);
	  tmp12 = tmp8 - tmp7;
     }
     {
	  fftw_real tmp14;
	  fftw_real tmp15;
	  fftw_real tmp16;
	  fftw_real tmp17;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp14 = input[3 * istride];
	  tmp15 = input[7 * istride];
	  tmp16 = input[11 * istride];
	  tmp17 = tmp15 + tmp16;
	  tmp18 = tmp14 - (K500000000 * tmp17);
	  tmp29 = tmp14 + tmp17;
	  tmp34 = tmp16 - tmp15;
     }
     real_output[3 * real_ostride] = tmp5 - tmp10;
     imag_output[3 * imag_ostride] = tmp29 - tmp30;
     tmp31 = tmp5 + tmp10;
     tmp32 = tmp29 + tmp30;
     real_output[6 * real_ostride] = tmp31 - tmp32;
     real_output[0] = tmp31 + tmp32;
     {
	  fftw_real tmp37;
	  fftw_real tmp38;
	  fftw_real tmp33;
	  fftw_real tmp36;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp37 = tmp34 + tmp35;
	  tmp38 = tmp11 + tmp12;
	  imag_output[2 * imag_ostride] = K866025403 * (tmp37 - tmp38);
	  imag_output[4 * imag_ostride] = K866025403 * (tmp38 + tmp37);
	  tmp33 = tmp25 - tmp26;
	  tmp36 = K866025403 * (tmp34 - tmp35);
	  real_output[5 * real_ostride] = tmp33 - tmp36;
	  real_output[real_ostride] = tmp33 + tmp36;
     }
     {
	  fftw_real tmp27;
	  fftw_real tmp28;
	  fftw_real tmp13;
	  fftw_real tmp24;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp27 = tmp25 + tmp26;
	  tmp28 = tmp18 + tmp23;
	  real_output[2 * real_ostride] = tmp27 - tmp28;
	  real_output[4 * real_ostride] = tmp27 + tmp28;
	  tmp13 = K866025403 * (tmp11 - tmp12);
	  tmp24 = tmp18 - tmp23;
	  imag_output[imag_ostride] = tmp13 - tmp24;
	  imag_output[5 * imag_ostride] = -(tmp13 + tmp24);
     }
}

fftw_codelet_desc fftw_real2hc_12_desc =
{
     "fftw_real2hc_12",
     (void (*)()) fftw_real2hc_12,
     12,
     FFTW_FORWARD,
     FFTW_REAL2HC,
     266,
     0,
     (const int *) 0,
};


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
/*

Given a block system
A x = b,
or
( K G )(u) = (f)
( D C )(p)   (h)

We define a symmetrically scaled system,
L A R R^-1 x = L b,		A'x' = b'
where L = diag(L1, 1), R = diag(R1,1) are block diagonal
and L_i and R_i are both diagonal matrirces.

The scaling produces
( L1   )( K G )( R1   )( R1   )^-1(u) = ( L1   )(f)
Du + Cp = (h)
and we solve
( L1   )( K G )( R1   )(v) = ( L1   )(f)
 Du + Cp  = (h)
or
( L1*K*R1, L1*G )(v) = (L1*f)
( D*R1  ,  C    )(p) = ( h  )

The solution u,p is recovered via
(u) = (R1*v)
(p) = (p)

*/

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>

#include <StGermain/libStGermain/src/StGermain.h>
#include <StgDomain/libStgDomain/src/StgDomain.h>

#include "common-driver-utils.h"
#include "stokes_Kblock_scaling.h"

/* private prototypes */
PetscErrorCode BSSCR_MatStokesKBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym );
PetscErrorCode BSSCR_mat_kblock_invert_scalings( MatStokesBlockScaling BA );

/* A x = b -> A'x' = b' */
/*
Note this routine actually modifies the matrix and rhs b.
*/

// updated
PetscErrorCode BSSCR_MatStokesKBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	Mat K,G,D;
	Vec L1,R1;
	Vec f,u;

	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	/* get the subblock solution and rhs */
	if( x != PETSC_NULL ) {
		VecNestGetSubVec( x, 0, &u );
		VecPointwiseDivide( u, u,R1); /* x <- x * 1/R1 */
	}
	if( b != PETSC_NULL ) {
		VecNestGetSubVec( b, 0, &f );
		VecPointwiseMult( f, f,L1); /* f <- f * L1 */
	}

	/* Scale matrices */
	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );

	if( K != PETSC_NULL ) {		MatDiagonalScale( K, L1,R1 );		}
	if( G != PETSC_NULL ) {		MatDiagonalScale( G, L1,PETSC_NULL );		}
	if( D != PETSC_NULL && !sym ) {	MatDiagonalScale( D, PETSC_NULL,R1 );		}

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockScalingCreate( MatStokesBlockScaling *_BA )
{
	MatStokesBlockScaling BA;
	PetscErrorCode ierr;

	ierr = PetscMalloc( sizeof(struct _p_MatStokesBlockScaling), &BA );CHKERRQ(ierr);

	BA->Lz = PETSC_NULL;
	BA->Rz = PETSC_NULL;

	BA->scaling_exists              = PETSC_FALSE;
	BA->scalings_have_been_inverted = PETSC_FALSE;
	BA->system_has_been_scaled      = PETSC_FALSE;

	*_BA = BA;

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockScalingDestroy( MatStokesBlockScaling BA )
{

	if( BA->scaling_exists == PETSC_FALSE ) PetscFunctionReturn(0);

	if( BA->Lz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Lz );
		BA->Lz = PETSC_NULL;
	}

	if( BA->Rz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Rz );
		BA->Rz = PETSC_NULL;
	}

	PetscFree( BA );

	PetscFunctionReturn(0);
}

/*
A is 2x2 block matrix
b and x are 2x1 block vectors
*/
// updated
PetscErrorCode BSSCR_MatKBlock_ConstructScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, PetscTruth sym )
{

	if( BA->scaling_exists == PETSC_FALSE ) {
      //PetscInt M,N;
		//PetscTruth is_block;

		/* check A is 2x2 block matrix */
		/* Stg_PetscTypeCompare( (PetscObject)A, "block", &is_block ); */
		/* if (is_block==PETSC_FALSE) { */
		/* 	Stg_SETERRQ( PETSC_ERR_SUP, "Only valid for MatType = block" ); */
		/* } */
		/* MatGetSize( A, &M, &N ); */
		/* if ( (M!=2) || (N!=2) ) { */
		/* 	Stg_SETERRQ2( PETSC_ERR_SUP, "Only valid for 2x2 block. Yours has dimension %Dx%D", M,N ); */
		/* } */

		VecDuplicate( x, &BA->Lz );
		VecDuplicate( x, &BA->Rz );

		BA->scaling_exists = PETSC_TRUE;
	}

//	if( BA->user_build_scaling != PETSC_NULL ) {
//		BA->user_build_scaling( A,b,x,BA->scaling_ctx);
//	}
//	else {
	BSSCR_MatStokesKBlockDefaultBuildScaling(BA,A,b,x, sym);
//	}

	BA->scalings_have_been_inverted = PETSC_FALSE;

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_mat_kblock_invert_scalings( MatStokesBlockScaling BA )
{
	Vec L1,R1;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	VecReciprocal(L1);
	VecReciprocal(R1);

	/* toggle inversion flag */
	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {   BA->scalings_have_been_inverted = PETSC_FALSE;  }
	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {  BA->scalings_have_been_inverted = PETSC_TRUE;   }

	PetscFunctionReturn(0);
}

/* updated */
PetscErrorCode BSSCR_MatStokesKBlockScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	if( BA->scaling_exists == PETSC_FALSE ) {
	    BSSCR_MatKBlock_ConstructScaling( BA,A,b,x, sym );
	}

	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {
		BSSCR_mat_kblock_invert_scalings(BA); /* to undo inversion */
	}

	BSSCR_MatStokesKBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_TRUE;

	/*
	PetscPrintf( PETSC_COMM_WORLD, "Post Scaling \n");
	MatBlock_ReportOperatorScales(A);
	*/

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockUnScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{

	if( BA->system_has_been_scaled == PETSC_FALSE ) {
		printf("Warning: MatBlock has not been scaled !! \n");
		PetscFunctionReturn(0);
	}

	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {
		BSSCR_mat_kblock_invert_scalings(BA);
	}

	BSSCR_MatStokesKBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_FALSE;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_MatStokesKBlockReportOperatorScales( Mat A, PetscTruth sym )
{
	Vec rA, rG;
	PetscInt loc;
	PetscReal min, max;
	Mat K,G,D,C;
	//PetscTruth is_block;

	/* check A is 2x2 block matrix */
	/* Stg_PetscTypeCompare( (PetscObject)A, "block", &is_block ); */
	/* if (is_block==PETSC_FALSE) { */
	/* 	Stg_SETERRQ( PETSC_ERR_SUP, "Only valid for MatType = block" ); */
	/* } */
	/* MatGetSize( A, &M, &N ); */
	/* if ( (M!=2) || (N!=2) ) { */
	/* 	Stg_SETERRQ2( PETSC_ERR_SUP, "Only valid for 2x2 block. Yours has dimension %Dx%D", M,N ); */
	/* } */

	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	MatGetVecs( K, PETSC_NULL, &rA );
	VecDuplicate( rA, &rG );

	/* Report the row max and mins */
	if (K!=PETSC_NULL) {
		MatGetRowMaxAbs( K, rA, PETSC_NULL );
		VecMax( rA, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(K) = %g \n", max );

		MatGetRowMinAbs( K, rA, PETSC_NULL );
		VecMin( rA, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(K) = %g \n\n", min );
	}

	if( G != PETSC_NULL ) {
		MatGetRowMaxAbs( G, rG, PETSC_NULL );
		VecMax( rG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(G) = %g \n", max );

		MatGetRowMinAbs( G, rG, PETSC_NULL );
		VecMin( rG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(G) = %g \n", min );
	}

	if( D != PETSC_NULL && !sym ) {
                Vec rD;

                MatGetVecs( D, PETSC_NULL, &rD );
		MatGetRowMaxAbs( D, rD, PETSC_NULL );
		VecMax( rD, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(D) = %g \n", max );

		MatGetRowMinAbs( D, rD, PETSC_NULL );
		VecMin( rD, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(D) = %g \n", min );

                Stg_VecDestroy(&rD );
	}

	if( C != PETSC_NULL ) {
		Vec cG;

		MatGetVecs( G, &cG, PETSC_NULL );
		MatGetRowMaxAbs( C, cG, PETSC_NULL );
		VecMax( cG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(C) = %g \n", max );

		MatGetRowMinAbs( C, cG, PETSC_NULL );
		VecMin( cG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(C) = %g \n\n", min );

		Stg_VecDestroy(&cG);
	}

	Stg_VecDestroy(&rA );
	Stg_VecDestroy(&rG );

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockDefaultBuildScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, PetscTruth sym )
{
	Mat K;
	Vec rA;
	Vec L1, R1;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	rA = L1;

	MatNestGetSubMat( A, 0,0, &K );

	/* Get diag of K */
	MatGetDiagonal( K, rA);
	VecSqrt( rA );
	VecReciprocal( rA );

	/* leave Shat alone as the Schur Compliment is unchanged by any consistent scaling of K */
	//MatGetDiagonal( Shat, rC);
	//VecSqrt( rC );
	//VecReciprocal( rC );

	/* no scaling for pressure */
	//VecSet( rC, 1.0 );

	VecCopy( L1, R1 );

	PetscFunctionReturn(0);
}


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_histogram.h>
#include"mpi.h"

const double PI = 3.14159;

double rz(double red);

struct galaxy {
  double x, y, z, d, w;
};

void N2divide( int * n, int N, int Ntot);

int main (int argc, char **argv) {

  FILE * DD;
  FILE * PP;

  const int Nr = 150;
  const int Nm = 100;
  const double RSTEP = 1.0;
  const double MSTEP = 0.01;

  const int maxNgal = 40000000;
  const double RMIN = 0.0;
  const double RMAX = 150.0;

  int *n;

  char DDname[400];
  char PPname[400];

  int bit;
  int bittot;

  sprintf(DDname,argv[1]);

  MPI_Init (&argc, &argv);
  int numproc;
  int totproc;
  MPI_Comm_rank (MPI_COMM_WORLD, &numproc);
  MPI_Comm_size (MPI_COMM_WORLD, &totproc);

  n = (int *) calloc(totproc + 1, sizeof(int));

  sprintf(PPname,"%s.RR",argv[2]);

  DD = fopen(DDname,"r");
  printf("%s %d\n",DDname,numproc);
  struct galaxy * gal;
  double * DDcount;
  double * DDcount_in_place;
  gal = (struct galaxy*)malloc(maxNgal*sizeof(struct galaxy));
  DDcount = (double*)calloc(Nr*Nm,sizeof(double));
  DDcount_in_place = (double*)calloc(Nr*Nm,sizeof(double));
  struct galaxy * galp;
  galp = gal;

  int N = 0;
  int Ntest = 0;
  double sumW = 0;
  double sumW_in_place = 0;
  double ra, dec, red, dist;
  double weight = 1.0;
  double weight1;
  double weight2;
  double weight3;
  double weight4;
  double weightFKP;
  int indexFKP;
  double ddummy;
  char line[2000];
  for (int i = 0; i < 0; i ++) {
  fgets (line, 2000, DD);
  printf("%s\n", line);
  }
  int counter = 0;
  while (fscanf(DD,"%le %le %le\n",&ra,&dec,&red) != EOF) {
    counter ++;
    if (counter%3 != 0) {
      continue;
    }
    ra *= PI/180.0;
    dec *= PI/180.0;
    dist = rz(red);
    galp->x = dist*cos(dec)*cos(ra);
    galp->y = dist*cos(dec)*sin(ra);
    galp->z = dist*sin(dec);
    galp->d = dist;
//    galp->w = weight1 * weight2;
    galp++;
    N++;
  }
  fclose(DD);
  n[0] = 1;
  N2divide(n, totproc, N);
  n[totproc] = N;
  n[0] = 0;
  bit = n[numproc];
  bittot = n[numproc + 1];
  for (int i = 0; i < totproc + 1; i++) {
    printf("n[%d] = %d\n", i, n[i]);
  }
  double mu;
  double d1, d2, d3;
  double x1, y1, z1, x2, y2, z2, gd1, gd2, gw1, gw2;
  double r2, rat;
  double rr;
  double xb, yb, zb, db2;
  struct galaxy * galp1, * galp2;
  galp1 = gal;
  galp1 += bit;
  for (int i = bit; i < bittot; i++) {
    x1 = galp1->x; y1 = galp1->y; z1 = galp1->z; gd1 = galp1->d; //gw1 = galp1->w;
    galp2 = gal + i + 1;
    for (int j = i + 1; j < N; j++) {
       x2 = galp2->x; y2 = galp2->y; z2 = galp2->z; gd2 = galp2->d; //gw2 = galp2->w;
       //sumW += gw1*gw2;
       sumW += 1.0;
       d1 = x1 - x2;
       d2 = y1 - y2;
       d3 = z1 - z2;
       r2 = d1*d1 + d2*d2 + d3*d3;
       if (r2 > RMAX*RMAX || r2 < RMIN*RMIN) {
	   galp2++;
         continue;
       }
       rat = gd1/gd2;
       xb = x1 + x2*rat;
       yb = y1 + y2*rat;
       zb = z1 + z2*rat;
       db2 = xb*xb + yb*yb + zb*zb;
       mu = fabs((xb*d1 + yb*d2 + zb*d3)/sqrt(r2)/sqrt(db2));
       rr = sqrt(r2);
	int binr = (int)((rr - RMIN)/RSTEP);
	int binm = (int)(mu/MSTEP);
	if (binr >= 0 && binm >= 0 && binr < Nr && binm < Nm){
	int ind = binr + Nr*binm;
//	DDcount[ind] += gw1*gw2;
        DDcount[ind] += 1.0;
	}
       galp2++;
    }
    galp1++;
  }
  free(gal);

  MPI_Reduce(DDcount, DDcount_in_place, Nr*Nm, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&sumW, &sumW_in_place, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  if (numproc == 0) {
    PP = fopen(PPname,"w");
    printf("%s\n", PPname);
    fprintf(PP,"# weighted number: %lf\n",sumW_in_place);
    fprintf(PP,"# RBINS: %d\n",Nr);
    fprintf(PP,"# MBINS: %d\n",Nm);
    int k, l;
    for (k = 0; k < Nm; k++) {
      for (l = 0; l < Nr; l++) {
      fprintf(PP,"%lf ",DDcount_in_place[k*Nr + l]);
      }
      fprintf(PP,"\n");
    }
    fclose(PP);
  }

  free(DDcount);
  free(DDcount_in_place);

  free(n);
  MPI_Finalize ();

  return 0;
}

double f (double x, void * p) {
  double Om = 0.310;
  double ff = 2997.92458/sqrt(Om*(1.0 + x)*(1.0 + x)*(1.0 + x) + 1.0 - Om);
  return ff;
}

double rz (double red) {
  gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);
  double result, error;
  gsl_function F;
  F.function = &f;
  gsl_integration_qags(&F, 0, red, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
  return result;
}

void N2divide( int * n, int N, int Ntot) {
   double dummy;
   for (int i = 1; i < N; i++) {
     dummy = double(Ntot) - 0.5;
     dummy -=
sqrt((2*double(Ntot)-1)*(2*double(Ntot)-1)-4*((2*double(Ntot)-n[i-1])*(n[i-1]-1)+double(Ntot)*(double(Ntot)-1)/N))/2.0;
     n[i] = int(dummy);
   }

   return;

}


/*
 * 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:44:04 EST 1999 */

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

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -notwiddleinv 5 */

/*
 * This function contains 32 FP additions, 12 FP multiplications,
 * (or, 26 additions, 6 multiplications, 6 fused multiply/add),
 * 16 stack variables, and 20 memory accesses
 */
static const fftw_real K250000000 = FFTW_KONST(+0.250000000000000000000000000000000000000000000);
static const fftw_real K951056516 = FFTW_KONST(+0.951056516295153572116439333379382143405698634);
static const fftw_real K587785252 = FFTW_KONST(+0.587785252292473129168705954639072768597652438);
static const fftw_real K559016994 = FFTW_KONST(+0.559016994374947424102293417182819058860154590);

/*
 * 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 fftwi_no_twiddle_5(const fftw_complex *input, fftw_complex *output, int istride, int ostride)
{
     fftw_real tmp1;
     fftw_real tmp27;
     fftw_real tmp8;
     fftw_real tmp10;
     fftw_real tmp21;
     fftw_real tmp22;
     fftw_real tmp14;
     fftw_real tmp28;
     fftw_real tmp26;
     fftw_real tmp17;
     ASSERT_ALIGNED_DOUBLE;
     tmp1 = c_re(input[0]);
     tmp27 = c_im(input[0]);
     {
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  fftw_real tmp5;
	  fftw_real tmp6;
	  fftw_real tmp7;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp2 = c_re(input[istride]);
	  tmp3 = c_re(input[4 * istride]);
	  tmp4 = tmp2 + tmp3;
	  tmp5 = c_re(input[2 * istride]);
	  tmp6 = c_re(input[3 * istride]);
	  tmp7 = tmp5 + tmp6;
	  tmp8 = tmp4 + tmp7;
	  tmp10 = K559016994 * (tmp4 - tmp7);
	  tmp21 = tmp2 - tmp3;
	  tmp22 = tmp5 - tmp6;
     }
     {
	  fftw_real tmp12;
	  fftw_real tmp13;
	  fftw_real tmp24;
	  fftw_real tmp15;
	  fftw_real tmp16;
	  fftw_real tmp25;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp12 = c_im(input[istride]);
	  tmp13 = c_im(input[4 * istride]);
	  tmp24 = tmp12 + tmp13;
	  tmp15 = c_im(input[2 * istride]);
	  tmp16 = c_im(input[3 * istride]);
	  tmp25 = tmp15 + tmp16;
	  tmp14 = tmp12 - tmp13;
	  tmp28 = tmp24 + tmp25;
	  tmp26 = K559016994 * (tmp24 - tmp25);
	  tmp17 = tmp15 - tmp16;
     }
     c_re(output[0]) = tmp1 + tmp8;
     {
	  fftw_real tmp18;
	  fftw_real tmp20;
	  fftw_real tmp11;
	  fftw_real tmp19;
	  fftw_real tmp9;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp18 = (K587785252 * tmp14) - (K951056516 * tmp17);
	  tmp20 = (K951056516 * tmp14) + (K587785252 * tmp17);
	  tmp9 = tmp1 - (K250000000 * tmp8);
	  tmp11 = tmp9 - tmp10;
	  tmp19 = tmp10 + tmp9;
	  c_re(output[2 * ostride]) = tmp11 - tmp18;
	  c_re(output[3 * ostride]) = tmp11 + tmp18;
	  c_re(output[ostride]) = tmp19 - tmp20;
	  c_re(output[4 * ostride]) = tmp19 + tmp20;
     }
     c_im(output[0]) = tmp27 + tmp28;
     {
	  fftw_real tmp23;
	  fftw_real tmp31;
	  fftw_real tmp30;
	  fftw_real tmp32;
	  fftw_real tmp29;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp23 = (K951056516 * tmp21) + (K587785252 * tmp22);
	  tmp31 = (K587785252 * tmp21) - (K951056516 * tmp22);
	  tmp29 = tmp27 - (K250000000 * tmp28);
	  tmp30 = tmp26 + tmp29;
	  tmp32 = tmp29 - tmp26;
	  c_im(output[ostride]) = tmp23 + tmp30;
	  c_im(output[4 * ostride]) = tmp30 - tmp23;
	  c_im(output[2 * ostride]) = tmp31 + tmp32;
	  c_im(output[3 * ostride]) = tmp32 - tmp31;
     }
}

fftw_codelet_desc fftwi_no_twiddle_5_desc =
{
     "fftwi_no_twiddle_5",
     (void (*)()) fftwi_no_twiddle_5,
     5,
     FFTW_BACKWARD,
     FFTW_NOTW,
     122,
     0,
     (const int *) 0,
};