//
// Copyright Jesse Manning 2007
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GELSS_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GELSS_HPP

#include <algorithm>

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

    namespace lapack {

        namespace detail {

            inline void gelss(const integer_t m, const integer_t n, const integer_t nrhs,
                              float *a, const integer_t lda, float *b, const integer_t ldb,
                              float *s, const float rcond, integer_t *rank, float *work,
                              const integer_t lwork, integer_t *info)
            {
                LAPACK_SGELSS(&m, &n, &nrhs, a, &lda, b, &ldb, s, &rcond, rank, work, &lwork, info);
            }

            inline void gelss(const integer_t m, const integer_t n, const integer_t nrhs,
                              double *a, const integer_t lda, double *b, const integer_t ldb,
                              double *s, const double rcond, integer_t *rank, double *work,
                              const integer_t lwork, integer_t *info)
            {
                LAPACK_DGELSS(&m, &n, &nrhs, a, &lda, b, &ldb, s, &rcond, rank, work, &lwork, info);
            }

            inline void gelss(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_f *a, const integer_t lda, traits::complex_f *b,
                              const integer_t ldb, float *s, const float rcond, integer_t *rank,
                              traits::complex_f *work, const integer_t lwork, float *rwork, integer_t *info)
            {
                LAPACK_CGELSS(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, info);
            }

            inline void gelss(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_d *a, const integer_t lda, traits::complex_d *b,
                              const integer_t ldb, double *s, const double rcond, integer_t *rank,
                              traits::complex_d *work, const integer_t lwork, double *rwork, integer_t *info)
            {
                LAPACK_ZGELSS(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, info);
            }

            // gelss for real type
            template <typename MatrA, typename MatrB, typename VecS, typename Work>
            int gelss(MatrA& A, MatrB& B, VecS& s, Work& work)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                detail::gelss(traits::matrix_size1(A),
                              traits::matrix_size2(A),
                              traits::matrix_size2(B),
                              traits::matrix_storage(A),
                              traits::leading_dimension(A),
                              traits::matrix_storage(B),
                              traits::leading_dimension(B),
                              traits::vector_storage(s),
                              rcond,
                              &rank,
                              traits::vector_storage(work),
                              traits::vector_size(work),
                              &info);

                return info;
            }

            // gelss for complex type
            template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
            int gelss(MatrA& A, MatrB& B, VecS& s, Work& work, RWork& rwork)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                detail::gelss(traits::matrix_size1(A),
                              traits::matrix_size2(A),
                              traits::matrix_size2(B),
                              traits::matrix_storage(A),
                              traits::leading_dimension(A),
                              traits::matrix_storage(B),
                              traits::leading_dimension(B),
                              traits::vector_storage(s),
                              rcond,
                              &rank,
                              traits::vector_storage(work),
                              traits::vector_size(work),
                              traits::vector_storage(rwork),
                              &info);

                return info;
            }

            // default minimal workspace functor
            template <int N>
            struct Gelss { };

            // specialization for gelss (sgelss, dgelss)
            template <>
            struct Gelss<1>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename traits::matrix_traits<MatrA>::value_type val_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t rhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    const std::ptrdiff_t maxmnr = std::max(maxmn, rhs);    // maxmnr = maxmn > rhs ? maxmn : rhs

                    traits::detail::array<val_t> work(3*minmn + std::max(2*minmn, maxmnr));

                    return gelss(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename traits::matrix_traits<MatrA>::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t rhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, rhs);    // maxmnr = maxmn > rhs ? maxmn : rhs

                    val_t temp_work;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelss(traits::matrix_size1(A),
                        traits::matrix_size2(A),
                        traits::matrix_size2(B),
                        traits::matrix_storage(A),
                        traits::leading_dimension(A),
                        traits::matrix_storage(B),
                        traits::leading_dimension(B),
                        traits::vector_storage(s),
                        rcond,
                        &rank,
                        &temp_work, //traits::vector_storage(work),
                        -1,         //traits::vector_size(work),
                        &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);

                    return gelss(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace1<Work> workspace) const
                {
                    return gelss(A, B, s, workspace.select(typename traits::matrix_traits<MatrA>::value_type()));
                }
            };

            // specialization for gelss (cgelss, zgelss)
            template <>
            struct Gelss<2>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename traits::matrix_traits<MatrA>::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t rhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    const std::ptrdiff_t maxmnr = std::max(maxmn, rhs);    // maxmnr = maxmn > rhs ? maxmn : rhs

                    traits::detail::array<val_t> work(2*minmn + maxmnr);
                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, (5*minmn)));

                    return gelss(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t rhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, rhs);    // maxmnr = maxmn > rhs ? maxmn : rhs

                    val_t temp_work;
                    real_t temp_rwork;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelss(traits::matrix_size1(A),
                                  traits::matrix_size2(A),
                                  traits::matrix_size2(B),
                                  traits::matrix_storage(A),
                                  traits::leading_dimension(A),
                                  traits::matrix_storage(B),
                                  traits::leading_dimension(B),
                                  traits::vector_storage(s),
                                  rcond,
                                  &rank,
                                  &temp_work,   //traits::vector_storage(work),
                                  -1,           //traits::vector_size(work),
                                  &temp_rwork,
                                  &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);
                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, (5*minmn)));

                    return gelss(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace2<Work, RWork> workspace) const
                {
                  typedef typename traits::matrix_traits<MatrA>::value_type    value_type ;
                  typedef typename traits::type_traits<value_type>::real_type real_type ;
                    return gelss(A, B, s, workspace.select(value_type()), workspace.select(real_type()));
                }
            };

        } // detail

        // gelss
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  s:          vector to store singular values on output, length >= max(1, min(m,n))
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename VecS, typename Work>
        int gelss(MatrA& A, MatrB& B, VecS& s, Work workspace)
        {
            typedef typename traits::matrix_traits<MatrA>::value_type val_t;

            return detail::Gelss<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

        // gelss, no singular values are returned
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename Work>
        int gelss(MatrA& A, MatrB& B, Work workspace)
        {
            typedef typename traits::matrix_traits<MatrA>::value_type val_t;
            typedef typename traits::type_traits<val_t>::real_type real_t;

            const std::ptrdiff_t m = traits::matrix_size1(A);
            const std::ptrdiff_t n = traits::matrix_size2(A);

            const std::ptrdiff_t s_size = std::max<std::ptrdiff_t>(1, std::min(m,n));
            traits::detail::array<real_t> s(s_size);

            return detail::Gelss<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

    } // namespace lapack

}}}

#endif


#ifndef _DIAUMPIRE_BSPLINE_HPP_
#define _DIAUMPIRE_BSPLINE_HPP_

#include <vector>
#include <fstream>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/algorithm/clamp.hpp>
#include <boost/range/numeric.hpp>
#include "ScanData.hpp"

namespace {
    //implements relative method - do not use for comparing with zero
    //use this most of the time, tolerance needs to be meaningful in your context
    template<typename TReal>
    static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = std::numeric_limits<TReal>::epsilon())
    {
        TReal diff = std::fabs(a - b);
        if (diff <= tolerance)
            return true;

        if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
            return true;

        return false;
    }

    //supply tolerance that is meaningful in your context
    //for example, default tolerance may not work if you are comparing double with float
    template<typename TReal>
    static bool isApproximatelyZero(TReal a, TReal tolerance = std::numeric_limits<TReal>::epsilon())
    {
        if (std::fabs(a) <= tolerance)
            return true;
        return false;
    }

    //use this when you want to be on safe side
    //for example, don't start rover unless signal is above 1
    template<typename TReal>
    static bool isDefinitelyLessThan(TReal a, TReal b, TReal tolerance = std::numeric_limits<TReal>::epsilon(), bool orEqualTo = false)
    {
        TReal diff = a - b;
        if (diff < tolerance)
            return orEqualTo;

        if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
            return true;

        return false;
    }
    template<typename TReal>
    static bool isDefinitelyGreaterThan(TReal a, TReal b, TReal tolerance = std::numeric_limits<TReal>::epsilon(), bool orEqualTo = false)
    {
        TReal diff = b - a;
        if (diff < tolerance)
            return orEqualTo;

        if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
            return true;

        return false;
    }

    //implements ULP method
    //use this when you are only concerned about floating point precision issue
    //for example, if you want to see if a is 1.0 by checking if its within
    //10 closest representable floating point numbers around 1.0.
    template<typename TReal>
    static bool isWithinPrecisionInterval(TReal a, TReal b, unsigned int interval_size = 1)
    {
        TReal min_a = a - (a - std::nextafter(a, std::numeric_limits<TReal>::lowest())) * interval_size;
        TReal max_a = a + (std::nextafter(a, std::numeric_limits<TReal>::max()) - a) * interval_size;

        return min_a <= b && max_a >= b;
    }
}

namespace DiaUmpire {

#ifdef DIAUMPIRE_DEBUG
    ofstream bsplineLog("bspline-log.txt");
    ofstream bsplineOutputLog("bspline-output-log.txt");
#endif

/**
 * B-spline smoothing
 * @author Chih-Chiang Tsou <chihchiang.tsou@gmail.com>
 */
class BSpline
{
    std::vector<double> bspline_T_;

    public:

    XYPointCollection Run(const XYPointCollection& data, int PtNum, int smoothDegree, int logId)
    {
        XYPointCollection bsplineCollection;
        int p = smoothDegree;
        int n = data.Data.size() - 1;
        int m = data.Data.size() + p;
        bspline_T_.resize(m + p);

        if (data.Data.size() <= (size_t) p) {
            return data;
        }

        for (int i = 0; i <= n; i++) {
            bspline_T_[i] = 0;
            bspline_T_[m - i] = 1;
        }
        double intv = 1.0 / (m - 2 * p);
        for (int i = 1; i <= (m - 1); i++) {
            bspline_T_[p + i] = bspline_T_[p + i - 1] + intv;
        }

        double t;
        for (int i = 0; i <= PtNum; i++) {
            t = ((double)i / PtNum);
            XYData pt = getbspline(data, t, n, p);
            bsplineCollection.AddPoint(pt);
        }
        if (isDefinitelyLessThan(bsplineCollection.Data.back().getX(), data.Data.back().getX(), 1e-8f)) {
            bsplineCollection.AddPoint(data.Data[data.PointCount() - 1]);
        }
        if (isDefinitelyGreaterThan(bsplineCollection.Data[0].getX(), data.Data[0].getX(), 1e-8f)) {
            bsplineCollection.AddPoint(data.Data[0]);
        }

#ifdef DIAUMPIRE_DEBUG
        if (logId > 0)
        {
            bsplineLog << setprecision(10)
                << (bsplineCollection.Data.back().getX() == data.Data.back().getX())
                << " " << isDefinitelyLessThan(bsplineCollection.Data.back().getX(), data.Data.back().getX(), 1e-8f)
                << " " << (bsplineCollection.Data.back().getX() < data.Data.back().getX())
                << " " << bsplineCollection.Data.back().getX()
                << " " << data.Data.back().getX()
                << "\n";
            boost::format pkFormat(" %.3f");
            bsplineOutputLog << logId << " " << data.size() << " " << bsplineCollection.size();
            for (auto& pk : bsplineCollection.Data)
                bsplineOutputLog << (pkFormat % pk.x).str();
            bsplineOutputLog << "\n";
        }
#endif

        return bsplineCollection;
    }

    XYData getbspline(const XYPointCollection& data, double t, int n, int p)
    {
        XYData pt(0, 0);

        int itp = 0;
        for (int i = 0; i <= n; i++) {
            pt.x = (pt.getX() + data.Data[itp].getX() * bspline_base(i, p, t));
            pt.y = (pt.getY() + data.Data[itp].getY() * bspline_base(i, p, t));
            itp++;
        }
        return pt;
    }

    double bspline_base(int i, int p, double t)
    {
        double n, c1, c2;
        double tn1 = 0;
        double tn2 = 0;
        if (p == 0) {
            if (bspline_T_[i] <= t && t < bspline_T_[i + 1] && bspline_T_[i] < bspline_T_[i + 1]) {
                n = 1;
            }
            else {
                n = 0;
            }
        }
        else {
            if ((bspline_T_[i + p] - bspline_T_[i]) == 0) {
                c1 = 0;
            }
            else {
                tn1 = bspline_base(i, (p - 1), t);
                c1 = (t - bspline_T_[i]) / (bspline_T_[i + p] - bspline_T_[i]);
            }
            if ((bspline_T_[i + p + 1] - bspline_T_[i + 1]) == 0) {
                c2 = 0;
            }
            else {
                tn2 = bspline_base((i + 1), (p - 1), t);
                c2 = (bspline_T_[i + p + 1] - t) / (bspline_T_[i + p + 1] - bspline_T_[i + 1]);
            }
            n = (c1 * tn1) + (c2 * tn2);
        }
        return n;
    }
};

class LinearInterpolation
{
    public:

    XYPointCollection Run(const XYPointCollection& data, int PtNum)
    {
        std::vector<XYData> Smoothdata(PtNum);
        float intv = (data.Data[data.PointCount() - 1].getX() - data.Data[0].getX()) / (float)PtNum;
        float rt = data.Data[0].getX();
        for (int i = 0; i < PtNum; i++) {
            Smoothdata[i] = XYData{ intv * i + rt, -1 };
        }
        int index = 0;
        for (const XYData& point : data.Data)
        {
            //XYData closet = Smoothdata[index];
            bool found = false;
            for (int i = index; i < PtNum - 1; i++) {
                if (Smoothdata[i].getX() <= point.getX() && Smoothdata[i + 1].getX() > point.getX()) {
                    Smoothdata[i].y = (point.getY());
                    index = i;
                    found = true;
                    break;
                }
            }
            if (!found) {
                Smoothdata[PtNum - 1].y = (point.getY());
                index = PtNum - 1;
            }
        }

        bool gapfound = false;
        int startidx = 0;
        int endidx = 0;
        float startintensity = Smoothdata[0].getY();
        float endintensity = Smoothdata[0].getY();

        for (int i = 1; i < PtNum; i++)
        {
            if (gapfound && Smoothdata[i].getY() != -1) {
                endidx = i;
                endintensity = Smoothdata[i].getY();
                Smoothdata[(startidx + endidx) / 2].y = ((startintensity + endintensity) / 2);
                i = startidx;
                gapfound = false;
            }
            if (!gapfound && Smoothdata[i].getY() == -1) {
                startidx = i - 1;
                startintensity = Smoothdata[i - 1].getY();
                gapfound = true;
            }
        }
        XYPointCollection returndata;
        swap(returndata.Data, Smoothdata);
        return returndata;
    }
};

/*
 *This class implements the Continuous Wavelet Transform (CWT), Mexican Hat,
 * over raw datapoints of a certain spectrum. After get the spectrum in the
 * wavelet's time domain, we use the local maxima to detect possible peaks in
 * the original raw datapoints.
 * Described in Tautenhahn, R., Bottcher, C. & Neumann, S.
 * Highly sensitive feature detection for high resolution LC/MS.
 * BMC Bioinformatics 9, 504 (2008).
 */
class WaveletMassDetector
{
    /**
     * Parameters of the wavelet, NPOINTS is the number of wavelet values to use
     * The WAVELET_ESL & WAVELET_ESL indicates the Effective Support boundaries
     */
    double NPOINTS;
    int WAVELET_ESL = -5;
    int WAVELET_ESR = 5;
    #define waveletDebug false
    const InstrumentParameter& parameter;
    std::vector<XYData>& DataPoint;
    double waveletWindow = (double) 0.3;
    std::vector<float> MEXHAT;
    double NPOINTS_half;

    public:

    WaveletMassDetector(const InstrumentParameter& parameter, std::vector<XYData>& DataPoint, int NoPoints) : parameter(parameter), DataPoint(DataPoint)
    {
        NPOINTS = NoPoints;

        double wstep = ((WAVELET_ESR - WAVELET_ESL) / NPOINTS);
        MEXHAT.resize(NPOINTS);

        double waveletIndex = WAVELET_ESL;
        for (int j = 0; j < NPOINTS; j++)
        {
            // Pre calculate the values of the wavelet
            MEXHAT[j] = cwtMEXHATreal(waveletIndex, waveletWindow, 0.0);
            waveletIndex += wstep;
        }

        NPOINTS_half = NPOINTS / 2;
        d = (int) NPOINTS / (WAVELET_ESR - WAVELET_ESL);
    }
    int d;
    //ArrayList<XYData>[] waveletCWT;

    //List of peak ridge (local maxima)
    std::vector<std::unique_ptr<std::vector<XYData>>> PeakRidge;

    void Run()
    {
        //"Intensities less than this value are interpreted as noise",
        //"Scale level",
        //"Number of wavelet'scale (coeficients) to use in m/z peak detection"
        //"Wavelet window size (%)",
        //"Size in % of wavelet window to apply in m/z peak detection");
        int maxscale = (int) (std::max(std::min((DataPoint[DataPoint.size() - 1].getX() - DataPoint[0].getX()), parameter.MaxCurveRTRange), 0.5f) * parameter.NoPeakPerMin / (WAVELET_ESR + WAVELET_ESR));

        //waveletCWT = new ArrayList[15];
        PeakRidge.resize(maxscale);
        //XYData maxint = new XYData(0f, 0f);
        for (int scaleLevel = 0; scaleLevel < maxscale; scaleLevel++)
        {
            std::vector<XYData> wavelet = performCWT(scaleLevel * 2 + 5);
            PeakRidge[scaleLevel] = std::make_unique<std::vector<XYData>>();
            //waveletCWT[scaleLevel] = wavelet;
            XYData lastpt = wavelet[0];
            XYData localmax { 0, 0 };
            XYData startpt = wavelet[0];

            bool increasing = false;
            bool decreasing = false;
            XYData localmaxint { 0, 0 };

            for (size_t cwtidx = 1; cwtidx < wavelet.size(); cwtidx++)
            {
                XYData& CurrentPoint = wavelet[cwtidx];
                if (CurrentPoint.getY() > lastpt.getY()) {//the peak is increasing
                    if (decreasing) {//first increasing point, last point was a possible local minimum
                        //check if the peak was symetric
                        if (localmax.y > 0 && (lastpt.getY() <= startpt.getY() || abs(lastpt.getY() - startpt.getY()) / localmax.getY() < parameter.SymThreshold)) {
                            PeakRidge[scaleLevel]->push_back(localmax);
                            localmax = CurrentPoint;
                            startpt = lastpt;
                        }
                    }
                    increasing = true;
                    decreasing = false;
                } else if (CurrentPoint.getY() < lastpt.getY()) {//peak decreasing
                    if (increasing) {//first point decreasing, last point was a possible local maximum
                        if (localmax.getY() < lastpt.getY()) {
                            localmax = lastpt;
                        }
                    }
                    decreasing = true;
                    increasing = false;
                }
                lastpt = CurrentPoint;
                if (CurrentPoint.getY() > localmaxint.getY()) {
                    localmaxint = CurrentPoint;
                }
                if (cwtidx == wavelet.size() - 1 && decreasing) {
                    if (localmax.y > 0 && (CurrentPoint.getY() <= startpt.getY() || abs(CurrentPoint.getY() - startpt.getY()) / localmax.getY() < parameter.SymThreshold)) {
                        PeakRidge[scaleLevel]->push_back(localmax);
                    }
                }
            }

            if (!waveletDebug) {
                wavelet.clear();
                //wavelet = null;
            }
        }
    }

    private:

    /**
     * Perform the CWT over raw data points in the selected scale level
     *
     *
     */
    std::vector<XYData> performCWT(int scaleLevel)
    {
        int length = DataPoint.size();
        std::vector<XYData> cwtDataPoints(length);

        int a_esl = scaleLevel * WAVELET_ESL;
        int a_esr = scaleLevel * WAVELET_ESR;
        int NPOINTS_half = (int) this->NPOINTS_half;
        int NPOINTS = (int) this->NPOINTS;
        double sqrtScaleLevel = sqrt(scaleLevel);
        /*std::vector<float> intensities(length + a_esr);
        std::vector<float> yValues(DataPoint.size());
        for(size_t i=0 ;i < yValues.size(); ++i)
            yValues[i] = DataPoint[i].y;*/

        for (int dx = 0; dx < length; dx++)
        {
            /*
             * Compute wavelet boundaries
             */
            int t1 = a_esl + dx;
            if (t1 < 0) {
                t1 = 0;
            }
            int t2 = a_esr + dx;
            if (t2 >= length) {
                t2 = (length - 1);
            }

            /*
             * Perform convolution
             */
            float intensity = 0;
            int ind;
            //float* intensityPtr = &intensities[0];
            //float* yValuesPtr = &yValues[t1];
            //float* MEXHATPtr = &MEXHAT[0];
            for (int i = t1; i <= t2; ++i/*, ++yValuesPtr*/) {
                ind = NPOINTS_half + (d * (i - dx) / scaleLevel);
                ind = boost::algorithm::clamp(ind, 0, NPOINTS - 1);
                intensity += DataPoint[i].y * MEXHAT[ind];
                //intensity += *yValuesPtr * MEXHATPtr[ind];
                //intensityPtr[i] = yValuesPtr[i] * MEXHATPtr[ind];
            }
            //intensity = std::accumulate(intensities.begin() + t1, intensities.begin() + t2 + 1, 0.f);
            intensity /= sqrtScaleLevel;
            // Eliminate the negative part of the wavelet map
            if (intensity < 0) {
                intensity = 0;
            }
            cwtDataPoints[dx].x = DataPoint[dx].getX();
            cwtDataPoints[dx].y = intensity;
        }
        return cwtDataPoints;
    }

    /**
     * This function calculates the wavelets's coefficients in Time domain
     *
     * @param double x Step of the wavelet
     * @param double a Window Width of the wavelet
     * @param double b Offset from the center of the peak
     */
    double cwtMEXHATreal(double x, double window, double b)
    {
        /*
         * c = 2 / ( sqrt(3) * pi^(1/4) )
         */
        double c = 0.8673250705840776;
        double TINY = 1E-200;
        double x2;

        if (window == 0.0) {
            window = TINY;
        }
        //x-b=t
        //window=delta
        x = (x - b) / window;
        x2 = x * x;
        return c * (1.0 - x2) * std::exp(-x2 / 2);
    }
    /**
     * This function searches for maximums from wavelet data points
     */
};

struct MassDefect
{
    bool InMassDefectRange(float mass, float d)
    {
        //upper = 0.00052738*x + 0.066015 +0.1
        //lower = 0.00042565*x + 0.00038210 -0.1

        double u = GetMassDefect(0.00052738*mass + 0.066015 + d);
        double l = GetMassDefect(0.00042565*mass + 0.00038210 - d);

        double defect = GetMassDefect(mass);
        if (u > l) {
            return (defect >= l && defect <= u);
        }
        return (defect >= l || defect <= u);
    }

    double GetMassDefect(double mass)
    {
        return mass - std::floor(mass);
    }
};

struct Regression
{
    /// <summary>
    /// Equation class: y=mx+b
    /// </summary>
    struct Equation {

        float Bvalue;
        float Mvalue;
        float SDvalue;
        float R2value;
        int NoPoints;
        float CorrelationCoffe;

        string GetEquationText() {
            return "Y=(" + lexical_cast<string>(std::round(Mvalue * 1000) / 1000) + ")X+" + lexical_cast<string>(std::round(Bvalue * 1000) / 1000);
        }
    };

    Equation equation;
    int MinPoint = 3;

    Regression(const XYPointCollection& pointset) : pointset(pointset)
    {
        FindEquation();
    }

    bool valid() {
        return pointset.PointCount() >= MinPoint;
    }

    float GetX(float y) {
        return (y - equation.Bvalue) / equation.Mvalue;
    }

    float GetY(float x) {
        return equation.Mvalue * x + equation.Bvalue;
    }

    float GetR2() {
        ComputeR2();
        return equation.R2value;
    }

    private:

    const XYPointCollection& pointset;
    float SigXY = 0;
    float SigX = 0;
    float SigY = 0;
    float SigX2 = 0;
    float SigY2 = 0;
    float SST;
    float SSR;
    float SXX;
    float SYY;
    float SXY;
    float MeanY;
    float MeanX;
    float max_x = 0;
    float min_x = std::numeric_limits<float>::max();
    float max_y = 0;
    float min_y = std::numeric_limits<float>::max();

    void FindEquation()
    {
        for (int i = 0; i < pointset.PointCount(); i++) {
            const XYData& point = pointset.Data[i];
            SigXY += point.getX() * point.getY();
            SigX += point.getX();
            SigY += point.getY();
            SigX2 += point.getX() * point.getX();
            SigY2 += point.getY() * point.getY();
            if (point.getX() > max_x) {
                max_x = point.getX();
            }
            if (point.getX() < min_x) {
                min_x = point.getX();
            }
            if (point.getY() > max_y) {
                max_y = point.getY();
            }
            if (point.getY() < min_y) {
                min_y = point.getY();
            }
        }
        equation.Mvalue = ((pointset.PointCount() * SigXY) - (SigX * SigY)) / ((pointset.PointCount() * SigX2) - (SigX * SigX));
        equation.Bvalue = (SigY - (equation.Mvalue * SigX)) / pointset.PointCount();
        equation.NoPoints = pointset.PointCount();
        MeanY = SigY / pointset.PointCount();
        MeanX = SigX / pointset.PointCount();
        //ComputeSD();
        //ComputeCorrelationCoff();
    }

    void ComputeCorrelationCoff()
    {
        ComputeSXY();
        ComputeSXX();
        ComputeSYY();
        equation.CorrelationCoffe = (float)(SXY / std::pow((double)SXX * SYY, 0.5));
    }

    void ComputeSXY()
    {
        SXY = 0;
        for (int i = 0; i < pointset.PointCount(); i++) {
            SXY += (pointset.Data[i].getX() - MeanX) * (pointset.Data[i].getY() - MeanY);
        }
    }

    void ComputeSXX()
    {
        SXX = 0;
        for (int i = 0; i < pointset.PointCount(); i++) {
            SXX += (pointset.Data[i].getX() - MeanX) * (pointset.Data[i].getX() - MeanX);
        }
    }

    void ComputeSYY()
    {
        SYY = 0;
        for (int i = 0; i < pointset.PointCount(); i++) {
            SYY += (pointset.Data[i].getY() - MeanY) * (pointset.Data[i].getY() - MeanY);
        }
    }

    void ComputeSD()
    {
        equation.SDvalue = (float)std::sqrt((double)((((pointset.PointCount() * SigY2) - (SigY * SigY)) - equation.Mvalue * ((pointset.PointCount() * SigXY) - (SigX * SigY))) / pointset.PointCount()));
    }

    void ComputeR2()
    {
        ComputeSST();
        ComputeSSR();
        equation.R2value = (SST - SSR) / SST;
    }

    void ComputeSST()
    {
        SST = 0;
        for (int i = 0; i < pointset.PointCount(); i++) {
            SST += (pointset.Data[i].getY() - MeanY) * (pointset.Data[i].getY() - MeanY);
        }
    }

    void ComputeSSR()
    {
        SSR = 0;
        for (int i = 0; i < pointset.PointCount(); i++) {
            SSR += (pointset.Data[i].getY() - (GetY(pointset.Data[i].getX()))) * (pointset.Data[i].getY() - (GetY(pointset.Data[i].getX())));
        }
    }
};

struct PearsonCorr
{
    static float CalcCorrNeighborBin(const XYPointCollection& CollectionA, const XYPointCollection& CollectionB)
    {
        int num = (int)((std::min(CollectionA.Data.at(CollectionA.PointCount() - 1).getX(), CollectionB.Data.at(CollectionB.PointCount() - 1).getX()) - std::max(CollectionA.Data.at(0).getX(), CollectionB.Data.at(0).getX())) * 100);
        float timeinterval = 1 / (float) 100.0;

        vector<float> arrayA(num);
        vector<float> arrayB(num);

        float start = std::max(CollectionA.Data.at(0).getX(), CollectionB.Data.at(0).getX());

        for (int i = 0; i < num - 1; i++) {
            float low = start + i * timeinterval;
            float up = start + (i + 1) * timeinterval;

            for (int j = 0; j < CollectionA.PointCount(); j++) {
                if (CollectionA.Data[j].getX() >= low && CollectionA.Data[j].getX() < up) {
                    float intenlow = CollectionA.Data[j].getY() * (1 - (CollectionA.Data[j].getX() - low) / timeinterval);
                    float intenup = CollectionA.Data[j].getY() * (1 - (up - CollectionA.Data[j].getX()) / timeinterval);
                    if (intenlow > arrayA[i]) {
                        arrayA[i] = intenlow;
                    }
                    if (intenup > arrayA[i + 1]) {
                        arrayA[i + 1] = intenup;
                    }
                }
                else if (CollectionA.Data[j].getX() > up) {
                    break;
                }
            }

            for (int j = 0; j < CollectionB.PointCount(); j++) {
                if (CollectionB.Data[j].getX() >= low && CollectionB.Data[j].getX() < up) {
                    float intenlow = CollectionB.Data[j].getY() * (1 - (CollectionB.Data[j].getX() - low) / timeinterval);
                    float intenup = CollectionB.Data[j].getY() * (1 - (up - CollectionB.Data[j].getX()) / timeinterval);
                    if (intenlow > arrayB[i]) {
                        arrayB[i] = intenlow;
                    }
                    if (intenup > arrayB[i + 1]) {
                        arrayB[i + 1] = intenup;
                    }
                }
                else if (CollectionB.Data[j].getX() > up) {
                    break;
                }
            }
        }

        XYPointCollection pointset;
        for (int i = 0; i < num; i++)
        {
            if (arrayA[i] > 0 && arrayB[i] > 0) {
                pointset.AddPoint(arrayA[i], arrayB[i]);
            }
        }

        float R2 = 0;

        if (pointset.PointCount() > 5) {
            Regression regression(pointset);
            if (regression.equation.Mvalue > 0) {
                R2 = regression.GetR2();
            }
        }
        return R2;
    }

    static float CalcCorr(const XYPointCollection& CollectionA, const XYPointCollection& CollectionB, int NoPointPerInterval)
    {
        int num = std::max(CollectionA.PointCount(), CollectionB.PointCount()) / 2;
        float timeinterval = 2 / (float)NoPointPerInterval;
        if (num < 6) {
            return 0;
        }

        vector<float> arrayA(num);
        vector<float> arrayB(num);
        int size = 0;

        float start = std::max(CollectionA.Data.at(0).getX(), CollectionB.Data.at(0).getX());

        int i = 0;
        float low = start;
        float up = start + timeinterval;

        for (int j = 0; j < CollectionA.PointCount(); j++) {
            while (CollectionA.Data[j].getX() > up) {
                i++;
                low = up;
                up = low + timeinterval;
            }
            if (i >= num) {
                break;
            }
            if (CollectionA.Data[j].getX() >= low && CollectionA.Data[j].getX() < up) {
                if (CollectionA.Data[j].getY() > arrayA[i]) {
                    arrayA[i] = CollectionA.Data[j].getY();
                }
            }
        }
        i = 0;
        low = start;
        up = start + timeinterval;
        for (int j = 0; j < CollectionB.PointCount(); j++) {
            while (CollectionB.Data[j].getX() > up) {
                i++;
                low = up;
                up = low + timeinterval;
            }
            if (i >= num) {
                break;
            }
            if (CollectionB.Data[j].getX() >= low && CollectionB.Data[j].getX() < up) {
                if (CollectionB.Data[j].getY() > arrayB[i]) {
                    arrayB[i] = CollectionB.Data[j].getY();
                    if (arrayA[i] > 0 && arrayB[i] > 0)
                        ++size;
                }
            }
        }

        for (int idx = 1; idx < num - 1; idx++) {
            if (arrayA[idx] == 0) {
                arrayA[idx] = (arrayA[idx - 1] + arrayA[idx + 1]) / 2;
            }
            if (arrayB[idx] == 0) {
                arrayB[idx] = (arrayB[idx - 1] + arrayB[idx + 1]) / 2;
            }
        }

        XYPointCollection pointset;
        pointset.Data.reserve(size);
        for (int idx = 0; idx < num; idx++) {
            if (arrayA[idx] > 0 && arrayB[idx] > 0) {
                pointset.AddPoint(arrayA[idx], arrayB[idx]);
            }
        }

        float R2 = 0;
        if (pointset.PointCount() > 5) {
            Regression regression(pointset);
            if (regression.equation.Mvalue > 0) {
                R2 = regression.GetR2();
            }
        }
        return R2;
    }
};

class ChiSquareGOF
{
    using chi_squared = boost::math::chi_squared;

    std::vector<chi_squared> chimodels;

    public:

    ChiSquareGOF(int maxpeak)
    {
        for (int i = 1; i < maxpeak; i++)
            chimodels.emplace_back(chi_squared(i));
    }

    float GetGoodNessOfFitProb(const vector<float>& expected, const vector<float>& observed) const
    {
        float gof = 0;
        int nopeaks = 0;
        for (size_t i = 0; i < std::min(observed.size(), expected.size()); i++)
        {
            if (observed[i] > 0)
            {
                float error = expected[i] - observed[i];
                gof += (error * error) / (expected[i] * expected[i]);
                nopeaks++;
            }
        }

        if (std::isnan(gof) || nopeaks < 2)
            return 0;

        //if (chimodels[nopeaks - 2] == null)
        //    std::cout << std::endl;

        float prob = 1 - (float) boost::math::cdf(chimodels[nopeaks - 2], gof);
        return prob;
    }
};

} // namespace DiaUmpire

#endif // !_DIAUMPIRE_BSPLINE_HPP_


//  (C) Copyright Nick Thompson 2018.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_UNIVARIATE_STATISTICS_HPP
#define BOOST_MATH_TOOLS_UNIVARIATE_STATISTICS_HPP

#include <algorithm>
#include <iterator>
#include <tuple>
#include <boost/assert.hpp>
#include <boost/config/header_deprecated.hpp>

BOOST_HEADER_DEPRECATED("<boost/math/statistics/univariate_statistics.hpp>");

namespace boost::math::tools {

template<class ForwardIterator>
auto mean(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    if constexpr (std::is_integral<Real>::value)
    {
        double mu = 0;
        double i = 1;
        for(auto it = first; it != last; ++it) {
            mu = mu + (*it - mu)/i;
            i += 1;
        }
        return mu;
    }
    else if constexpr (std::is_same_v<typename std::iterator_traits<ForwardIterator>::iterator_category, std::random_access_iterator_tag>)
    {
        size_t elements = std::distance(first, last);
        Real mu0 = 0;
        Real mu1 = 0;
        Real mu2 = 0;
        Real mu3 = 0;
        Real i = 1;
        auto end = last - (elements % 4);
        for(auto it = first; it != end;  it += 4) {
            Real inv = Real(1)/i;
            Real tmp0 = (*it - mu0);
            Real tmp1 = (*(it+1) - mu1);
            Real tmp2 = (*(it+2) - mu2);
            Real tmp3 = (*(it+3) - mu3);
            // please generate a vectorized fma here
            mu0 += tmp0*inv;
            mu1 += tmp1*inv;
            mu2 += tmp2*inv;
            mu3 += tmp3*inv;
            i += 1;
        }
        Real num1 = Real(elements  - (elements %4))/Real(4);
        Real num2 = num1 + Real(elements % 4);

        for (auto it = end; it != last; ++it)
        {
            mu3 += (*it-mu3)/i;
            i += 1;
        }

        return (num1*(mu0+mu1+mu2) + num2*mu3)/Real(elements);
    }
    else
    {
        auto it = first;
        Real mu = *it;
        Real i = 2;
        while(++it != last)
        {
            mu += (*it - mu)/i;
            i += 1;
        }
        return mu;
    }
}

template<class Container>
inline auto mean(Container const & v)
{
    return mean(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto variance(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute mean and variance.");
    // Higham, Accuracy and Stability, equation 1.6a and 1.6b:
    if constexpr (std::is_integral<Real>::value)
    {
        double M = *first;
        double Q = 0;
        double k = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double tmp = *it - M;
            Q = Q + ((k-1)*tmp*tmp)/k;
            M = M + tmp/k;
            k += 1;
        }
        return Q/(k-1);
    }
    else
    {
        Real M = *first;
        Real Q = 0;
        Real k = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real tmp = (*it - M)/k;
            Q += k*(k-1)*tmp*tmp;
            M += tmp;
            k += 1;
        }
        return Q/(k-1);
    }
}

template<class Container>
inline auto variance(Container const & v)
{
    return variance(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto sample_variance(ForwardIterator first, ForwardIterator last)
{
    size_t n = std::distance(first, last);
    BOOST_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container>
inline auto sample_variance(Container const & v)
{
    return sample_variance(v.cbegin(), v.cend());
}

// Follows equation 1.5 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto skewness(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute skewness.");
    if constexpr (std::is_integral<Real>::value)
    {
        double M1 = *first;
        double M2 = 0;
        double M3 = 0;
        double n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double delta21 = *it - M1;
            double tmp = delta21/n;
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        double var = M2/(n-1);
        if (var == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            return double(0);
        }
        double skew = M3/(M2*sqrt(var));
        return skew;
    }
    else
    {
        Real M1 = *first;
        Real M2 = 0;
        Real M3 = 0;
        Real n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real delta21 = *it - M1;
            Real tmp = delta21/n;
            M3 += tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 += tmp*(n-1)*delta21;
            M1 += tmp;
            n += 1;
        }

        Real var = M2/(n-1);
        if (var == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            return Real(0);
        }
        Real skew = M3/(M2*sqrt(var));
        return skew;
    }
}

template<class Container>
inline auto skewness(Container const & v)
{
    return skewness(v.cbegin(), v.cend());
}

// Follows equation 1.5/1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto first_four_moments(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_ASSERT_MSG(first != last, "At least one sample is required to compute the first four moments.");
    if constexpr (std::is_integral<Real>::value)
    {
        double M1 = *first;
        double M2 = 0;
        double M3 = 0;
        double M4 = 0;
        double n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            double delta21 = *it - M1;
            double tmp = delta21/n;
            M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        return std::make_tuple(M1, M2/(n-1), M3/(n-1), M4/(n-1));
    }
    else
    {
        Real M1 = *first;
        Real M2 = 0;
        Real M3 = 0;
        Real M4 = 0;
        Real n = 2;
        for (auto it = std::next(first); it != last; ++it)
        {
            Real delta21 = *it - M1;
            Real tmp = delta21/n;
            M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
            M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
            M2 = M2 + tmp*(n-1)*delta21;
            M1 = M1 + tmp;
            n += 1;
        }

        return std::make_tuple(M1, M2/(n-1), M3/(n-1), M4/(n-1));
    }
}

template<class Container>
inline auto first_four_moments(Container const & v)
{
    return first_four_moments(v.cbegin(), v.cend());
}

// Follows equation 1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ForwardIterator>
auto kurtosis(ForwardIterator first, ForwardIterator last)
{
    auto [M1, M2, M3, M4] = first_four_moments(first, last);
    if (M2 == 0)
    {
        return M2;
    }
    return M4/(M2*M2);
}

template<class Container>
inline auto kurtosis(Container const & v)
{
    return kurtosis(v.cbegin(), v.cend());
}

template<class ForwardIterator>
auto excess_kurtosis(ForwardIterator first, ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container>
inline auto excess_kurtosis(Container const & v)
{
    return excess_kurtosis(v.cbegin(), v.cend());
}

template<class RandomAccessIterator>
auto median(RandomAccessIterator first, RandomAccessIterator last)
{
    size_t num_elems = std::distance(first, last);
    BOOST_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last);
        std::nth_element(middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class RandomAccessContainer>
inline auto median(RandomAccessContainer & v)
{
    return median(v.begin(), v.end());
}

template<class RandomAccessIterator>
auto gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    BOOST_ASSERT_MSG(first != last && std::next(first) != last, "Computation of the Gini coefficient requires at least two samples.");

    std::sort(first, last);
    if constexpr (std::is_integral<Real>::value)
    {
        double i = 1;
        double num = 0;
        double denom = 0;
        for (auto it = first; it != last; ++it)
        {
            num += *it*i;
            denom += *it;
            ++i;
        }

        // If the l1 norm is zero, all elements are zero, so every element is the same.
        if (denom == 0)
        {
            return double(0);
        }

        return ((2*num)/denom - i)/(i-1);
    }
    else
    {
        Real i = 1;
        Real num = 0;
        Real denom = 0;
        for (auto it = first; it != last; ++it)
        {
            num += *it*i;
            denom += *it;
            ++i;
        }

        // If the l1 norm is zero, all elements are zero, so every element is the same.
        if (denom == 0)
        {
            return Real(0);
        }

        return ((2*num)/denom - i)/(i-1);
    }
}

template<class RandomAccessContainer>
inline auto gini_coefficient(RandomAccessContainer & v)
{
    return gini_coefficient(v.begin(), v.end());
}

template<class RandomAccessIterator>
inline auto sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    size_t n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer>
inline auto sample_gini_coefficient(RandomAccessContainer & v)
{
    return sample_gini_coefficient(v.begin(), v.end());
}

template<class RandomAccessIterator>
auto median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last, typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::tools::median(first, last);
    }
    size_t num_elems = std::distance(first, last);
    BOOST_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last, comparator);
        std::nth_element(middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class RandomAccessContainer>
inline auto median_absolute_deviation(RandomAccessContainer & v, typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(v.begin(), v.end(), center);
}

}
#endif


#ifndef PARMCB_SVA_SIGNED_HPP_
#define PARMCB_SVA_SIGNED_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>
#include <boost/timer/timer.hpp>

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <parmcb/detail/signed_dijkstra.hpp>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>

namespace parmcb {

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_signed(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {

        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();

        /*
         * Initialize support vectors
         */
        std::vector<SpVecGF2<std::size_t>> support;
        for (std::size_t k = 0; k < csd; k++) {
            support.emplace_back(k);
        }

        boost::timer::cpu_timer cycle_timer;
        cycle_timer.stop();
        boost::timer::cpu_timer support_timer;
        support_timer.stop();

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            /*
             * Choose the sparsest support heuristic
             */
            auto min_support = k;
            for (auto r = k + 1; r < csd; ++r) {
                if (support[r].size() < support[min_support].size())
                    min_support = r;
                if (support[min_support].size() < 5) {
                    break;
                }
            }
            if (min_support != k) {  // swap
                std::swap(support[k], support[min_support]);
            }

            /*
             * Compute shortest odd cycle
             */
            cycle_timer.resume();
            std::less<WeightType> compare = std::less<WeightType>();
            std::tuple<std::set<Edge>, WeightType, bool> best = std::make_tuple(std::set<Edge>(),
                    (std::numeric_limits<WeightType>::max)(), false);
            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);

            if (signed_edges.size() >= boost::num_vertices(g)) {
                VertexIt vi, viend;
                for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
                    auto v = *vi;
                    const bool use_hidden_edges = false;
                    auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, std::set<Edge> { },
                            use_hidden_edges, v, true, v, false, std::get<2>(best), std::get<1>(best));
                    if (std::get<2>(res) && (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best)))) {
                        best = res;
                        assert(std::get<2>(best));
                    }
                }
            } else {
                /*
                 * Heuristic in case number of signed edges is small compared to the number of vertices.
                 */
                std::set<Edge> hidden_edges;
                std::copy(signed_edges.begin(), signed_edges.end(), std::inserter(hidden_edges, hidden_edges.begin()));
                for (auto sei = signed_edges.begin(); sei != signed_edges.end(); ++sei) {
                    auto se = *sei;
                    auto se_v = boost::source(se, g);
                    auto se_u = boost::target(se, g);
                    auto res = bidirectional_signed_dijkstra(g, weight_map, signed_edges, hidden_edges, true, se_v,
                            true, se_u, true, std::get<2>(best), std::get<1>(best));
                    hidden_edges.erase(hidden_edges.begin());
                    if (std::get<2>(res) && std::get<0>(res).find(se) == std::get<0>(res).end()) {
                        std::get<1>(res) += boost::get(weight_map, se);
                        if (!std::get<2>(best) || compare(std::get<1>(res), std::get<1>(best))) {
                            std::get<0>(res).insert(se);
                            best = res;
                            assert(std::get<2>(best));
                        }
                    }
                }
            }
            assert(std::get<2>(best));
            cycle_timer.stop();

            /*
             * Update support vectors
             */
            support_timer.resume();
            std::set<std::size_t> cyclek;
            convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
            for (std::size_t l = k + 1; l < csd; l++) {
                if (support[l] * cyclek == 1) {
                    support[l] += support[k];
                }
            }
            support_timer.stop();

            /*
             * Output new cycle
             */
            std::list<Edge> cyclek_edgelist;
            std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
            *out++ = cyclek_edgelist;
            mcb_weight += std::get<1>(best);
        }

        std::cout << "cycle   timer" << cycle_timer.format();
        std::cout << "support timer" << support_timer.format();

        return mcb_weight;
    }

} // parmcb

#endif


#ifndef STAN_MATH_REV_MAT_FUN_COV_EXP_QUAD_HPP
#define STAN_MATH_REV_MAT_FUN_COV_EXP_QUAD_HPP

#include <stan/math/rev/core.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/squared_distance.hpp>
#include <stan/math/prim/scal/fun/exp.hpp>
#include <stan/math/prim/scal/meta/scalar_type.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <vector>
#include <cmath>

namespace stan {
namespace math {

/**
 * This is a subclass of the vari class for precomputed
 * gradients of cov_exp_quad.
 *
 * The class stores the double values for the distance
 * matrix, pointers to the varis for the covariance
 * matrix, along with a pointer to the vari for sigma,
 * and the vari for l.
 *
 * @tparam T_x type of std::vector of elements
 * @tparam T_sigma type of sigma
 * @tparam T_l type of length scale
 */
template <typename T_x, typename T_sigma, typename T_l>
class cov_exp_quad_vari : public vari {
 public:
  const size_t size_;
  const size_t size_ltri_;
  const double l_d_;
  const double sigma_d_;
  const double sigma_sq_d_;
  double* dist_;
  vari* l_vari_;
  vari* sigma_vari_;
  vari** cov_lower_;
  vari** cov_diag_;

  /**
   * Constructor for cov_exp_quad.
   *
   * All memory allocated in
   * ChainableStack's stack_alloc arena.
   *
   * It is critical for the efficiency of this object
   * that the constructor create new varis that aren't
   * popped onto the var_stack_, but rather are
   * popped onto the var_nochain_stack_. This is
   * controlled to the second argument to
   * vari's constructor.
   *
   * @param x std::vector input that can be used in square distance
   *    Assumes each element of x is the same size
   * @param sigma standard deviation
   * @param l length scale
   */
  cov_exp_quad_vari(const std::vector<T_x>& x, const T_sigma& sigma,
                    const T_l& l)
      : vari(0.0),
        size_(x.size()),
        size_ltri_(size_ * (size_ - 1) / 2),
        l_d_(value_of(l)),
        sigma_d_(value_of(sigma)),
        sigma_sq_d_(sigma_d_ * sigma_d_),
        dist_(ChainableStack::instance().memalloc_.alloc_array<double>(
            size_ltri_)),
        l_vari_(l.vi_),
        sigma_vari_(sigma.vi_),
        cov_lower_(ChainableStack::instance().memalloc_.alloc_array<vari*>(
            size_ltri_)),
        cov_diag_(
            ChainableStack::instance().memalloc_.alloc_array<vari*>(size_)) {
    double inv_half_sq_l_d = 0.5 / (l_d_ * l_d_);
    size_t pos = 0;
    for (size_t j = 0; j < size_ - 1; ++j) {
      for (size_t i = j + 1; i < size_; ++i) {
        double dist_sq = squared_distance(x[i], x[j]);
        dist_[pos] = dist_sq;
        cov_lower_[pos] = new vari(
            sigma_sq_d_ * std::exp(-dist_sq * inv_half_sq_l_d), false);
        ++pos;
      }
    }
    for (size_t i = 0; i < size_; ++i)
      cov_diag_[i] = new vari(sigma_sq_d_, false);
  }

  virtual void chain() {
    double adjl = 0;
    double adjsigma = 0;

    for (size_t i = 0; i < size_ltri_; ++i) {
      vari* el_low = cov_lower_[i];
      double prod_add = el_low->adj_ * el_low->val_;
      adjl += prod_add * dist_[i];
      adjsigma += prod_add;
    }
    for (size_t i = 0; i < size_; ++i) {
      vari* el = cov_diag_[i];
      adjsigma += el->adj_ * el->val_;
    }
    l_vari_->adj_ += adjl / (l_d_ * l_d_ * l_d_);
    sigma_vari_->adj_ += adjsigma * 2 / sigma_d_;
  }
};

/**
 * This is a subclass of the vari class for precomputed
 * gradients of cov_exp_quad.
 *
 * The class stores the double values for the distance
 * matrix, pointers to the varis for the covariance
 * matrix, along with a pointer to the vari for sigma,
 * and the vari for l.
 *
 * @tparam T_x type of std::vector of elements
 * @tparam T_l type of length scale
 */
template <typename T_x, typename T_l>
class cov_exp_quad_vari<T_x, double, T_l> : public vari {
 public:
  const size_t size_;
  const size_t size_ltri_;
  const double l_d_;
  const double sigma_d_;
  const double sigma_sq_d_;
  double* dist_;
  vari* l_vari_;
  vari** cov_lower_;
  vari** cov_diag_;

  /**
   * Constructor for cov_exp_quad.
   *
   * All memory allocated in
   * ChainableStack's stack_alloc arena.
   *
   * It is critical for the efficiency of this object
   * that the constructor create new varis that aren't
   * popped onto the var_stack_, but rather are
   * popped onto the var_nochain_stack_. This is
   * controlled to the second argument to
   * vari's constructor.
   *
   * @param x std::vector input that can be used in square distance
   *    Assumes each element of x is the same size
   * @param sigma standard deviation
   * @param l length scale
   */
  cov_exp_quad_vari(const std::vector<T_x>& x, double sigma, const T_l& l)
      : vari(0.0),
        size_(x.size()),
        size_ltri_(size_ * (size_ - 1) / 2),
        l_d_(value_of(l)),
        sigma_d_(value_of(sigma)),
        sigma_sq_d_(sigma_d_ * sigma_d_),
        dist_(ChainableStack::instance().memalloc_.alloc_array<double>(
            size_ltri_)),
        l_vari_(l.vi_),
        cov_lower_(ChainableStack::instance().memalloc_.alloc_array<vari*>(
            size_ltri_)),
        cov_diag_(
            ChainableStack::instance().memalloc_.alloc_array<vari*>(size_)) {
    double inv_half_sq_l_d = 0.5 / (l_d_ * l_d_);
    size_t pos = 0;
    for (size_t j = 0; j < size_ - 1; ++j) {
      for (size_t i = j + 1; i < size_; ++i) {
        double dist_sq = squared_distance(x[i], x[j]);
        dist_[pos] = dist_sq;
        cov_lower_[pos] = new vari(
            sigma_sq_d_ * std::exp(-dist_sq * inv_half_sq_l_d), false);
        ++pos;
      }
    }
    for (size_t i = 0; i < size_; ++i)
      cov_diag_[i] = new vari(sigma_sq_d_, false);
  }

  virtual void chain() {
    double adjl = 0;

    for (size_t i = 0; i < size_ltri_; ++i) {
      vari* el_low = cov_lower_[i];
      adjl += el_low->adj_ * el_low->val_ * dist_[i];
    }
    l_vari_->adj_ += adjl / (l_d_ * l_d_ * l_d_);
  }
};

/**
 * Returns a squared exponential kernel.
 *
 * @param x std::vector input that can be used in square distance
 *    Assumes each element of x is the same size
 * @param sigma standard deviation
 * @param l length scale
 * @return squared distance
 * @throw std::domain_error if sigma <= 0, l <= 0, or
 *   x is nan or infinite
 */
template <typename T_x>
inline typename boost::enable_if_c<
    boost::is_same<typename scalar_type<T_x>::type, double>::value,
    Eigen::Matrix<var, -1, -1> >::type
cov_exp_quad(const std::vector<T_x>& x, const var& sigma, const var& l) {
  check_positive("cov_exp_quad", "sigma", sigma);
  check_positive("cov_exp_quad", "l", l);
  size_t x_size = x.size();
  for (size_t i = 0; i < x_size; ++i)
    check_not_nan("cov_exp_quad", "x", x[i]);

  Eigen::Matrix<var, -1, -1> cov(x_size, x_size);
  if (x_size == 0)
    return cov;

  cov_exp_quad_vari<T_x, var, var>* baseVari
      = new cov_exp_quad_vari<T_x, var, var>(x, sigma, l);

  size_t pos = 0;
  for (size_t j = 0; j < x_size - 1; ++j) {
    for (size_t i = (j + 1); i < x_size; ++i) {
      cov.coeffRef(i, j).vi_ = baseVari->cov_lower_[pos];
      cov.coeffRef(j, i).vi_ = cov.coeffRef(i, j).vi_;
      ++pos;
    }
    cov.coeffRef(j, j).vi_ = baseVari->cov_diag_[j];
  }
  cov.coeffRef(x_size - 1, x_size - 1).vi_ = baseVari->cov_diag_[x_size - 1];
  return cov;
}

/**
 * Returns a squared exponential kernel.
 *
 * @param x std::vector input that can be used in square distance
 *    Assumes each element of x is the same size
 * @param sigma standard deviation
 * @param l length scale
 * @return squared distance
 * @throw std::domain_error if sigma <= 0, l <= 0, or
 *   x is nan or infinite
 */
template <typename T_x>
inline typename boost::enable_if_c<
    boost::is_same<typename scalar_type<T_x>::type, double>::value,
    Eigen::Matrix<var, -1, -1> >::type
cov_exp_quad(const std::vector<T_x>& x, double sigma, const var& l) {
  check_positive("cov_exp_quad", "marginal variance", sigma);
  check_positive("cov_exp_quad", "length-scale", l);
  size_t x_size = x.size();
  for (size_t i = 0; i < x_size; ++i)
    check_not_nan("cov_exp_quad", "x", x[i]);

  Eigen::Matrix<var, -1, -1> cov(x_size, x_size);
  if (x_size == 0)
    return cov;

  cov_exp_quad_vari<T_x, double, var>* baseVari
      = new cov_exp_quad_vari<T_x, double, var>(x, sigma, l);

  size_t pos = 0;
  for (size_t j = 0; j < x_size - 1; ++j) {
    for (size_t i = (j + 1); i < x_size; ++i) {
      cov.coeffRef(i, j).vi_ = baseVari->cov_lower_[pos];
      cov.coeffRef(j, i).vi_ = cov.coeffRef(i, j).vi_;
      ++pos;
    }
    cov.coeffRef(j, j).vi_ = baseVari->cov_diag_[j];
  }
  cov.coeffRef(x_size - 1, x_size - 1).vi_ = baseVari->cov_diag_[x_size - 1];
  return cov;
}

}  // namespace math
}  // namespace stan
#endif


#include "wrapper.hpp"
#include "piano.hpp"
#include "python_common.hpp"
#include <Eigen/Dense>
#include <numpy/ndarrayobject.h>
#include <portaudio.h>

static PyModuleDef Module = {
    PyModuleDef_HEAD_INIT,
    "spectrum_analyzer",
    "",
    -1,
    nullptr,
    nullptr,
    nullptr,
    nullptr,
    nullptr
};

int PaInit(PyObject*, PyObject*, PyObject*) {
    // We don't acquire GIL here becuase portaudio won't access Python objects.
    Pa_Initialize();
    return 0;
}

void PaEnd(PyObject*) {
    Pa_Terminate();
}

PyTypeObject PaInitializerType {
    PyVarObject_HEAD_INIT(NULL, 0)
    "_PaInitializer",          /* tp_name */
    sizeof(PyObject),          /* tp_basicsize */
    0,                         /* tp_itemsize */
    PaEnd,                     /* tp_dealloc */
    0,                         /* tp_print */
    0,                         /* tp_getattr */
    0,                         /* tp_setattr */
    0,                         /* tp_reserved */
    0,                         /* tp_repr */
    0,                         /* tp_as_number */
    0,                         /* tp_as_sequence */
    0,                         /* tp_as_mapping */
    0,                         /* tp_hash  */
    0,                         /* tp_call */
    0,                         /* tp_str */
    0,                         /* tp_getattro */
    0,                         /* tp_setattro */
    0,                         /* tp_as_buffer */
    Py_TPFLAGS_DEFAULT,        /* tp_flags */
    0,                         /* tp_doc */
    0,                         /* tp_traverse */
    0,                         /* tp_clear */
    0,                         /* tp_richcompare */
    0,                         /* tp_weaklistoffset */
    0,                         /* tp_iter */
    0,                         /* tp_iternext */
    0,                         /* tp_methods */
    0,                         /* tp_members */
    0,                         /* tp_getset */
    0,                         /* tp_base */
    0,                         /* tp_dict */
    0,                         /* tp_descr_get */
    0,                         /* tp_descr_set */
    0,                         /* tp_dictoffset */
    PaInit,                    /* tp_init */
    0,                         /* tp_alloc */
    PyType_GenericNew          /* tp_new */
};

PyObject* get_pitch(PyObject*, PyObject* args) {
    PyObject* py_wav;
    unsigned interval;
    if (!PyArg_ParseTuple(args, "OI", &py_wav, &interval))
        return nullptr;
    wav_file& wav = *static_cast<py_wav_handle*>(py_wav);
    const std::size_t len = wav.size_of(interval);
    Eigen::VectorXd ref(len, 1);
    if (wav.get(len, ref.data())) {
        PyErr_SetString(PyExc_IndexError, "File length exceeded");
        return nullptr;
    }
    // Fitting form x[i] = sum(a[j] * cos(omega[j] * n) + b[j] * sin(omega[j] * n), j)
    // where i and n are time, namely n = i / sample_rate
    //Eigen::MatrixXd cos_sin_consts(len, 88 * 2);
    //for (int i = 0; i != len; ++i) {
    //    // first 88 cos, second 88 sin
    //    for (int j = 0; j != 88; ++j) {
    //        double tmp = i * piano_omegas[j] / wav.sample_rate;
    //        cos_sin_consts(i, j) = std::cos(tmp);
    //        cos_sin_consts(i, 88 + j) = std::sin(tmp);
    //    }
    //}
    Eigen::MatrixXd tmp(2 * 88, 2 * 88); // = cos_sin_consts.transpose() * cos_sin_consts
    //for (int i = 0; i < 88; ++i) {
    //    for (int j = 0; j < 88; ++j) {
    //        double acc1 = 0, acc2 = 0, acc3 = 0, acc4 = 0;
    //        for (int k = 0; k != len; ++k) {
    //            double tmp = k * piano_omegas[i] / wav.sample_rate;
    //            double tmp2 = k * piano_omegas[j] / wav.sample_rate;
    //            acc1 += std::cos(tmp) * std::cos(tmp2);
    //            acc2 += std::cos(tmp) * std::sin(tmp2);
    //            acc3 += std::sin(tmp) * std::cos(tmp2);
    //            acc4 += std::sin(tmp) * std::sin(tmp2);
    //        }
    //        tmp(i, j) = acc1;
    //        tmp(i, 88 + j) = acc2;
    //        tmp(88 + i, j) = acc3;
    //        tmp(88 + i, 88 + j) = acc4;
    //    }
    //}
    // Lagrange's formulae applied and optimized for symmetricity
    for (int i = 0; i != 88; ++i) {
        double htmp3 = piano_omegas[i] / wav.sample_rate;
        double tmp3 = 2 * htmp3;
        tmp(i, i) = len - 0.5 + std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        tmp(i, 88 + i) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3);
        tmp(88 + i, 88 + i) = len + 0.5 - std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        for (int j = i + 1; j != 88; ++j) {
            double tmp1 = piano_omegas[i] / wav.sample_rate;
            double tmp2 = piano_omegas[j] / wav.sample_rate;
            double tmp3 = tmp1 + tmp2, tmp4 = tmp1 - tmp2;
            double htmp3 = tmp3 * 0.5, htmp4 = tmp4 * 0.5;
            tmp(i, j) = std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3)) + std::sin(len * tmp4 + htmp4) / (2 * std::sin(htmp4)) - 1;
            tmp(i, 88 + j) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3) - 0.5 * (std::cos(htmp4) - std::cos(len * tmp4 + htmp4)) / std::sin(htmp4);
            /* tmp(88 + i, j) */ tmp(j, 88 + i) = 0.5 * (std::cos(htmp3) - std::cos(len * tmp3 + htmp3)) / std::sin(htmp3) + 0.5 * (std::cos(htmp4) - std::cos(len * tmp4 + htmp4)) / std::sin(htmp4);
            tmp(88 + i, 88 + j) = std::sin(len * tmp4 + htmp4) / (2 * std::sin(htmp4)) - std::sin(len * tmp3 + htmp3) / (2 * std::sin(htmp3));
        }
    }
    tmp /= 2;
    Eigen::Matrix<double, 88 * 2, 1> rhs; // = cos_sin_consts.tranpose() * ref
#pragma omp parallel for
    for (int i = 0; i < 88; ++i) {
        double acc0 = 0;
        double acc1 = 0, acc2 = 0;
        for (int j = 0; j != len; ++j) {
            acc1 += std::cos(acc0) * ref[j];
            acc2 += std::sin(acc0) * ref[j];
            acc0 += piano_omegas[i] / wav.sample_rate;
        }
        rhs[i] = acc1;
        rhs[88 + i] = acc2;
    }
    Eigen::Matrix<double, 88 * 2, 1> sol = tmp.selfadjointView<Eigen::Upper>().llt().solve(rhs);
    Eigen::Map<Eigen::Array<double, 88, 1>> a(sol.data());
    Eigen::Map<Eigen::Array<double, 88, 1>> b(sol.data() + 88);
    npy_intp dim[] = { 88 };
    PyObject* ret = PyArray_SimpleNew(1, dim, NPY_DOUBLE);
    if (!ret)
        return nullptr;
    Eigen::Map<Eigen::Array<double, 88, 1>> power(static_cast<double*>(PyArray_GETPTR1(ret, 0)));
    power = (a.cwiseAbs2() + b.cwiseAbs2());
    return ret;
}

static PyMethodDef Methods[] = {
    { "get_pitch", get_pitch, METH_VARARGS, "" },
    { nullptr }
};

PyMODINIT_FUNC
PyInit_spectrum_analyzer() {
    PyObject* m = PyExc(PyModule_Create(&Module), nullptr);
    import_array();
    PyModule_AddFunctions(m, Methods);
    PyOnly(PyType_Ready(&py_wav_handle_type), 0);
    Py_INCREF(&py_wav_handle_type);
    PyModule_AddObject(m, "wav_file", reinterpret_cast<PyObject*>(&py_wav_handle_type));
    PyOnly(PyType_Ready(&PaInitializerType), 0);
    Py_INCREF(&PaInitializerType);
    PyOnly(PyType_Ready(&pa_stream_handle_type), 0);
    Py_INCREF(&pa_stream_handle_type);
    auto empty_arg = PyTuple_New(0);
    auto init = PyObject_CallObject(reinterpret_cast<PyObject*>(&PaInitializerType), empty_arg);
    Py_DECREF(empty_arg);
    PyModule_AddObject(m, "__Do_NOT_TouchThis", init);
    return m;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
  Copyright (C) 2013 Peter Caspers

  This file is part of QuantLib, a free-software/open-source library
  for financial quantitative analysts and developers - http://quantlib.org/

  QuantLib is free software: you can redistribute it and/or modify it
  under the terms of the QuantLib license.  You should have received a
  copy of the license along with this program; if not, please email
  <quantlib-dev@lists.sf.net>. The license is also available online at
  <http://quantlib.org/license.shtml>.

  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 license for more details.
*/

/*! \file cmsreplicationpricer.cpp
  \brief
*/

#include <ql/cashflows/cmsreplicationpricer.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cmscoupon.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/time/schedule.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/pricingengines/blackformula.hpp>

#include <boost/make_shared.hpp>
#include <ql/termstructures/volatility/atmsmilesection.hpp>

namespace QuantLib {

    CmsReplicationPricer::CmsReplicationPricer(const Handle<SwaptionVolatilityStructure>& swaptionVol,
                                               const Handle<Quote>& meanReversion,
                                               const Handle<YieldTermStructure>& couponDiscountCurve,
                                               const Settings& settings) :
        CmsCouponPricer(swaptionVol), meanReversion_(meanReversion),
        couponDiscountCurve_(couponDiscountCurve), settings_(settings) {

        registerWith(meanReversion_);
        if(!couponDiscountCurve_.empty())
            registerWith(couponDiscountCurve_);
    }

    void CmsReplicationPricer::initialize(const FloatingRateCoupon& coupon){

        coupon_ =  dynamic_cast<const CmsCoupon*>(&coupon);
        QL_REQUIRE(coupon_, "CMS coupon needed");
        gearing_ = coupon_->gearing();
        spread_ = coupon_->spread();
        Time accrualPeriod = coupon_->accrualPeriod();
        QL_REQUIRE(accrualPeriod != 0.0, "null accrual period");

        fixingDate_ = coupon_->fixingDate();
        paymentDate_ = coupon_->date();
        swapIndex_ = coupon_->swapIndex();

        forwardCurve_ = swapIndex_->forwardingTermStructure();
        if(swapIndex_->exogenousDiscount())
            discountCurve_ = swapIndex_->discountingTermStructure();
        else
            discountCurve_ = forwardCurve_;

        // if no coupon discount curve is given just use the discounting curve from the swap index.
        // for rate calculation this curve cancels out in the computation, so e.g. the discounting
        // swap engine will produce correct results, even if the couponDiscountCurve is not set here.
        // only the price member function in this class will be dependent on the coupon discount curve.

        if(couponDiscountCurve_.empty())
            couponDiscountCurve_ = discountCurve_;

        today_ = QuantLib::Settings::instance().evaluationDate();

        if(paymentDate_ > today_)
            discount_ = couponDiscountCurve_->discount(paymentDate_);
        else discount_= 1.;

        spreadLegValue_ = spread_ * accrualPeriod * discount_;

        if (fixingDate_ > today_) {

            swapTenor_ = swapIndex_->tenor();
            swap_ = swapIndex_->underlyingSwap(fixingDate_);

            swapRateValue_ = swap_->fairRate();
            annuity_ = 1.0E4 * std::fabs(swap_->fixedLegBPS());

            boost::shared_ptr<SmileSection> sectionTmp =
                swaptionVolatility()->smileSection(fixingDate_, swapTenor_);

            // if the section does not provide an atm level, we enhance it to
            // have one, no need to exit with an exception ...

            if (sectionTmp->atmLevel() == Null<Real>())
                smileSection_ = boost::make_shared<AtmSmileSection>(
                    sectionTmp, swapRateValue_);
            else
                smileSection_ = sectionTmp;

            const Leg& fixedCoupons = swap_->fixedLeg();
            fixedLegPaymentDates_ = std::vector<Date>(fixedCoupons.size());
            fixedLegYearFractions_ = std::vector<Real>(fixedCoupons.size());
            for(Size i=0; i<fixedCoupons.size(); i++) {
                boost::shared_ptr<FixedRateCoupon> coupon =
                    boost::dynamic_pointer_cast<FixedRateCoupon>(fixedCoupons[i]);
                fixedLegPaymentDates_[i] = coupon->date();
                fixedLegYearFractions_[i] = coupon->accrualPeriod();
            }

            const Leg& floatingCoupons = swap_->floatingLeg();
            floatingLegStartDates_ = floatingLegEndDates_ = floatingLegPaymentDates_ =
                std::vector<Date>(floatingCoupons.size());
            for(Size i=0; i<floatingCoupons.size(); i++) {
                boost::shared_ptr<IborCoupon> coupon =
                    boost::dynamic_pointer_cast<IborCoupon>(floatingCoupons[i]);
                floatingLegStartDates_[i] = coupon->accrualStartDate();
                floatingLegEndDates_[i] = coupon->accrualEndDate();
                floatingLegPaymentDates_[i] = coupon->date();
            }

        }
    }

    Real CmsReplicationPricer::hullWhiteScenario(Real dt, Real h) const {

        Real e;

        if(close(meanReversion_->value(),0.0))
            e = dt;
        else
            e = (1.0 - std::exp( -dt * meanReversion_->value() ) ) / meanReversion_->value();

        return std::exp( -h * e );

    }

    Real CmsReplicationPricer::annuity(Real h) const {

        Real annuity=0.0;
        for(Size i=0; i<fixedLegYearFractions_.size(); i++) {
            Real dt = discountCurve_->dayCounter().yearFraction(fixingDate_,fixedLegPaymentDates_[i]);
            annuity += fixedLegYearFractions_[i] * discountCurve_->discount(fixedLegPaymentDates_[i])
                / discountCurve_->discount(fixingDate_) * hullWhiteScenario(dt,h);
        }

        return annuity;

    }

    Real CmsReplicationPricer::floatingLegNpv(Real h) const {

        if(settings_.simplifiedFloatingLeg_) {
            Real dt1 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegStartDates_.front());
            Real dt2 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegPaymentDates_.back());
            return (discountCurve_->discount(floatingLegStartDates_.front()) * hullWhiteScenario(dt1,h) -
                    discountCurve_->discount(floatingLegPaymentDates_.back()) * hullWhiteScenario(dt2,h)) /
                discountCurve_->discount(fixingDate_);
        }
        else {
            Real npv=0.0;
            for(Size i=0; i<floatingLegStartDates_.size();i++) {
                Real dt1 = forwardCurve_->dayCounter().yearFraction(fixingDate_,floatingLegStartDates_[i]);
                Real dt2 = forwardCurve_->dayCounter().yearFraction(fixingDate_,floatingLegEndDates_[i]);
                Real dt3 = discountCurve_->dayCounter().yearFraction(fixingDate_,floatingLegPaymentDates_[i]);
                // we use that the day counter of the floating leg is equal to that of the float index always for
                // swapIndex underlying swaps
                npv += (forwardCurve_->discount(floatingLegStartDates_[i]) *
                            hullWhiteScenario(dt1, h) /
                            (forwardCurve_->discount(floatingLegEndDates_[i]) *
                             hullWhiteScenario(dt2, h)) -
                        1.0) *
                       discountCurve_->discount(floatingLegPaymentDates_[i]) /
                       discountCurve_->discount(fixingDate_) *
                       hullWhiteScenario(dt3, h);
            }
            return npv;
        }

    }

    Real CmsReplicationPricer::swapRate(Real h) const {

        return floatingLegNpv(h) / annuity(h);

    }

    Real CmsReplicationPricer::h(Real rate) const {

        Real a = -10.0, b = 10.0;
        HHelper h(this,rate);
        Brent solver;

        Real c;

        try {
            c = solver.solve(h,1.0E-6,0.0,a,b);
        } catch(QuantLib::Error e) {
            QL_FAIL("can not imply h from rate (" << rate << "):" << e.what());
        }

        return c;

    }

    Real CmsReplicationPricer::strikeFromVegaRatio(Real ratio, Option::Type optionType, Real referenceStrike) const {

        Real a,b,min,max,k;
        if(optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k = std::min(smileSection_->maxStrike(), settings_.upperRateBound_);
        }
        else {
            a = min = k = std::max(smileSection_->minStrike(), settings_.lowerRateBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        VegaRatioHelper h(&*smileSection_,smileSection_->vega(swapRateValue_)*ratio);
        Brent solver;

        try {
            k = solver.solve(h,1.0E-5,(a+b)/2.0,a,b);
        } catch(...) {
            // use default value set above
        }

        return std::min(std::max(k,min),max);

    }

    Real CmsReplicationPricer::strikeFromPrice(Real price, Option::Type optionType, Real referenceStrike) const {

        Real a,b,min,max,k;
        if(optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k = std::min(smileSection_->maxStrike(), settings_.upperRateBound_);
        }
        else {
            a = min = k = std::max(smileSection_->minStrike(), settings_.lowerRateBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        PriceHelper h(&*smileSection_, optionType, price);
        Brent solver;

        try {
            k = solver.solve(h,1.0E-5,swapRateValue_,a,b);
        } catch(...) {
            // use default value set above
        }

        return std::min(std::max(k,min),max);

    }

    Real CmsReplicationPricer::optionletPrice(Option::Type optionType, Real strike) const {

        Real phi = optionType == Option::Call ? 1.0 : -1.0;

        if(optionType == Option::Call && strike >= settings_.upperRateBound_) return 0.0;
        if(optionType == Option::Put && strike <= settings_.lowerRateBound_) return 0.0;

        // determine vector of hs corresponding to scenarios;

        std::vector<Real> hs;
        switch(settings_.strategy_) {

        case Settings::DiscreteStrikeSpreads : {
            for(Size i=0;i<settings_.n_;i++) {
                Real k = strike + phi*settings_.discreteStrikeSpreads_[i];
                if(k>=smileSection_->minStrike() && k<=smileSection_->maxStrike())
                    hs.push_back( h(k) );
            }
            break;
        }

        case Settings::RateBound : {
            Real h1 = h(strike);
            Real h2 = h(optionType == Option::Call ? settings_.upperRateBound_ : settings_.lowerRateBound_);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        case Settings::VegaRatio : {
            // strikeFromVegaRatio ensures that returned strike is on the expected side of strike
            Real effectiveBound = optionType == Option::Call ?
                std::min(settings_.upperRateBound_ ,
                         strikeFromVegaRatio(settings_.vegaRatio_,optionType,strike)) :
                std::max(settings_.lowerRateBound_,
                         strikeFromVegaRatio(settings_.vegaRatio_,optionType,strike));
            if(close(fabs(effectiveBound-strike),0.0)) return 0.0;
            Real h1 = h(strike);
            Real h2 = h(effectiveBound);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        case Settings::PriceThreshold : {
            // strikeFromPrice ensures that returned strike is on the expected side of strike
            Real effectiveBound = optionType == Option::Call ?
                std::min(settings_.upperRateBound_,
                         strikeFromPrice(settings_.priceThreshold_,optionType,strike)) :
                std::max(settings_.lowerRateBound_,
                         strikeFromPrice(settings_.priceThreshold_,optionType,strike));
            if(close(fabs(effectiveBound-strike),0.0)) return 0.0;
            Real h1 = h(strike);
            Real h2 = h(effectiveBound);
            for(Size i=0;i<settings_.n_;i++) {
                hs.push_back(h1+((Real)(i+1)/(settings_.n_-1))*(h2-h1));
            }
            break;
        }

        default:
            QL_FAIL("Unknown strategy (" << settings_.strategy_ << ")");

        }

        // compute the hedge basket and price it
        std::vector<Real> weights, strikes;

        Real rate = strike, lastRate;
        Real price = QL_MAX_REAL, lastPrice, basketPrice = 0.0;

        for(Size i=0;i<hs.size();i++) {
            lastPrice = price;
            lastRate = rate;
            rate = swapRate(hs[i]);
            strikes.push_back(lastRate);
            Real ann = annuity(hs[i]);
            Real npv=0.0;
            for(Size j=0;j<i;j++) { // this can be done _much_ more efficiently ...
                npv += weights[j] * phi * (rate - strikes[j]) * ann;
            }
            Real dt = discountCurve_->dayCounter().yearFraction(fixingDate_,paymentDate_);
            weights.push_back( ( phi * discountCurve_->discount(paymentDate_) /
                                 discountCurve_->discount(fixingDate_) *
                                 hullWhiteScenario(dt,hs[i]) * (rate - strike) - npv ) /
                               ( phi * ann * (rate - lastRate) ) );
            price = blackFormula(optionType,lastRate,swapRateValue_,
                                 std::sqrt(smileSection_->variance(lastRate)),annuity_);
            if(settings_.enforceMonotonicPrices_)
                price = std::min( lastPrice, price );
            basketPrice += weights.back()*price;
        }

        // note that fixedLegBPS() is computed w.r.t. discountCurve_, but the coupon discount curve
        // may be different. We have to take this into account here.

        basketPrice *=  coupon_->accrualPeriod() * discount_ / discountCurve_->discount(paymentDate_);

        return basketPrice;

    }

    Real CmsReplicationPricer::meanReversion() const { return meanReversion_->value();}

    Rate CmsReplicationPricer::swapletRate() const {
        return swapletPrice()/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::capletPrice(Rate effectiveCap) const {
        // caplet is equivalent to call option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs =
                std::max(coupon_->swapIndex()->fixing(fixingDate_)-effectiveCap, 0.);
            Rate price = (gearing_*Rs)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real capletPrice = optionletPrice(Option::Call, effectiveCap);
            return gearing_ * capletPrice;
        }
    }

    Rate CmsReplicationPricer::capletRate(Rate effectiveCap) const {
        return capletPrice(effectiveCap)/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::floorletPrice(Rate effectiveFloor) const {
        // floorlet is equivalent to put option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs =
                std::max(effectiveFloor-coupon_->swapIndex()->fixing(fixingDate_),0.);
            Rate price = (gearing_*Rs)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real floorletPrice = optionletPrice(Option::Put, effectiveFloor);
            return gearing_ * floorletPrice;
        }
    }

    Rate CmsReplicationPricer::floorletRate(Rate effectiveFloor) const {
        return floorletPrice(effectiveFloor)/(coupon_->accrualPeriod()*discount_);
    }

    Real CmsReplicationPricer::swapletPrice() const {

        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = coupon_->swapIndex()->fixing(fixingDate_);
            Rate price = (gearing_*Rs + spread_)*(coupon_->accrualPeriod()*discount_);
            return price;
        } else {
            Real atmCapletPrice = optionletPrice(Option::Call, swapRateValue_);
            Real atmFloorletPrice = optionletPrice(Option::Put, swapRateValue_);
            return gearing_ *(coupon_->accrualPeriod()* discount_ * swapRateValue_
                              + atmCapletPrice - atmFloorletPrice)
                + spreadLegValue_;
        }
    }

}