#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_


/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Author: Stuart Glaser
 */

#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include "robot_mechanism_controllers/joint_trajectory_action_controller.h"
#include <sstream>
#include "angles/angles.h"
#include "pluginlib/class_list_macros.h"

PLUGINLIB_EXPORT_CLASS( controller::JointTrajectoryActionController, pr2_controller_interface::Controller)

namespace controller {

// These functions are pulled from the spline_smoother package.
// They've been moved here to avoid depending on packages that aren't
// mature yet.

static inline void generatePowers(int n, double x, double* powers)
{
  powers[0] = 1.0;
  for (int i=1; i<=n; i++)
  {
    powers[i] = powers[i-1]*x;
  }
}

static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
    double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
{
  coefficients.resize(6);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.5*end_acc;
    coefficients[3] = 0.0;
    coefficients[4] = 0.0;
    coefficients[5] = 0.0;
  }
  else
  {
    double T[6];
    generatePowers(5, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = 0.5*start_acc;
    coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
                       12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
    coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
                       16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
    coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
                       6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
  }
}

/**
 * \brief Samples a quintic spline segment at a particular time
 */
static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
    double& position, double& velocity, double& acceleration)
{
  // create powers of time:
  double t[6];
  generatePowers(5, time, t);

  position = t[0]*coefficients[0] +
      t[1]*coefficients[1] +
      t[2]*coefficients[2] +
      t[3]*coefficients[3] +
      t[4]*coefficients[4] +
      t[5]*coefficients[5];

  velocity = t[0]*coefficients[1] +
      2.0*t[1]*coefficients[2] +
      3.0*t[2]*coefficients[3] +
      4.0*t[3]*coefficients[4] +
      5.0*t[4]*coefficients[5];

  acceleration = 2.0*t[0]*coefficients[2] +
      6.0*t[1]*coefficients[3] +
      12.0*t[2]*coefficients[4] +
      20.0*t[3]*coefficients[5];
}

static void getCubicSplineCoefficients(double start_pos, double start_vel,
    double end_pos, double end_vel, double time, std::vector<double>& coefficients)
{
  coefficients.resize(4);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.0;
    coefficients[3] = 0.0;
  }
  else
  {
    double T[4];
    generatePowers(3, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
    coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
  }
}

JointTrajectoryActionController::JointTrajectoryActionController()
  : loop_count_(0), robot_(NULL)
{
}

JointTrajectoryActionController::~JointTrajectoryActionController()
{
  sub_command_.shutdown();
  serve_query_state_.shutdown();
  action_server_.reset();
  action_server_follow_.reset();
}

bool JointTrajectoryActionController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
{
  using namespace XmlRpc;
  node_ = n;
  robot_ = robot;

  // Gets all of the joints
  XmlRpc::XmlRpcValue joint_names;
  if (!node_.getParam("joints", joint_names))
  {
    ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  for (int i = 0; i < joint_names.size(); ++i)
  {
    XmlRpcValue &name_value = joint_names[i];
    if (name_value.getType() != XmlRpcValue::TypeString)
    {
      ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
                node_.getNamespace().c_str());
      return false;
    }

    pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
    if (!j) {
      ROS_ERROR("Joint not found: %s. (namespace: %s)",
                ((std::string)name_value).c_str(), node_.getNamespace().c_str());
      return false;
    }
    joints_.push_back(j);
  }

  // Ensures that all the joints are calibrated.
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    if (!joints_[i]->calibrated_)
    {
      ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
                joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
      return false;
    }
  }

  // Sets up pid controllers for all of the joints
  std::string gains_ns;
  if (!node_.getParam("gains", gains_ns))
    gains_ns = node_.getNamespace() + "/gains";
  masses_.resize(joints_.size());
  pids_.resize(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
    joint_nh.param("mass", masses_[i], 0.0);
    if (!pids_[i].init(joint_nh))
      return false;
  }

  // Sets up the proxy controllers for each joint
  proxies_enabled_.resize(joints_.size());
  proxies_.resize(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    ros::NodeHandle joint_nh(gains_ns + "/" + joints_[i]->joint_->name);
    if (joint_nh.hasParam("proxy")) {
      proxies_enabled_[i] = true;
      joint_nh.param("proxy/lambda", proxies_[i].lambda_proxy_, 1.0);
      joint_nh.param("proxy/acc_converge", proxies_[i].acc_converge_, 0.0);
      joint_nh.param("proxy/vel_limit", proxies_[i].vel_limit_, 0.0);
      joint_nh.param("proxy/effort_limit", proxies_[i].effort_limit_, 0.0);
      proxies_[i].mass_ = masses_[i];
      double _;
      pids_[i].getGains(proxies_[i].Kp_, proxies_[i].Ki_, proxies_[i].Kd_, proxies_[i].Ficl_, _);
    }
    else {
      proxies_enabled_[i] = false;
    }
  }

  // Trajectory and goal tolerances
  default_trajectory_tolerance_.resize(joints_.size());
  default_goal_tolerance_.resize(joints_.size());
  node_.param("joint_trajectory_action_node/constraints/goal_time", default_goal_time_constraint_, 0.0);
  double stopped_velocity_tolerance;
  node_.param("joint_trajectory_action_node/constraints/stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
  for (size_t i = 0; i < default_goal_tolerance_.size(); ++i)
    default_goal_tolerance_[i].velocity = stopped_velocity_tolerance;
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    std::string ns = std::string("joint_trajectory_action_node/constraints/") + joints_[i]->joint_->name;
    node_.param(ns + "/goal", default_goal_tolerance_[i].position, 0.0);
    node_.param(ns + "/trajectory", default_trajectory_tolerance_[i].position, 0.0);
  }

  // Output filters
  output_filters_.resize(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    std::string p = "output_filters/" + joints_[i]->joint_->name;
    if (node_.hasParam(p))
    {
      output_filters_[i].reset(new filters::FilterChain<double>("double"));
      if (!output_filters_[i]->configure(node_.resolveName(p)))
        output_filters_[i].reset();
    }
  }

  // Creates a dummy trajectory
  boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &traj = *traj_ptr;
  traj[0].start_time = robot_->getTime().toSec();
  traj[0].duration = 0.0;
  traj[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    traj[0].splines[j].coef[0] = 0.0;
  current_trajectory_box_.set(traj_ptr);

  sub_command_ = node_.subscribe("command", 1, &JointTrajectoryActionController::commandCB, this);
  serve_query_state_ = node_.advertiseService(
    "query_state", &JointTrajectoryActionController::queryStateService, this);

  q.resize(joints_.size());
  qd.resize(joints_.size());
  qdd.resize(joints_.size());

  controller_state_publisher_.reset(
    new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
    (node_, "state", 1));
  controller_state_publisher_->lock();
  for (size_t j = 0; j < joints_.size(); ++j)
    controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
  controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
  controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
  controller_state_publisher_->msg_.desired.effort.resize(joints_.size());
  controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
  controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.actual.effort.resize(joints_.size());
  controller_state_publisher_->msg_.error.positions.resize(joints_.size());
  controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.error.effort.resize(joints_.size());
  controller_state_publisher_->unlock();

  action_server_.reset(new JTAS(node_, "joint_trajectory_action",
                                boost::bind(&JointTrajectoryActionController::goalCB, this, _1),
                                boost::bind(&JointTrajectoryActionController::cancelCB, this, _1),
                                false));
  action_server_follow_.reset(new FJTAS(node_, "follow_joint_trajectory",
                                        boost::bind(&JointTrajectoryActionController::goalCBFollow, this, _1),
                                        boost::bind(&JointTrajectoryActionController::cancelCBFollow, this, _1),
                                        false));
  action_server_->start();
  action_server_follow_->start();

  return true;
}

void JointTrajectoryActionController::starting()
{
  last_time_ = robot_->getTime();

  for (size_t i = 0; i < pids_.size(); ++i) {
    pids_[i].reset();
    proxies_[i].reset(joints_[i]->position_, joints_[i]->velocity_);
  }

  // Creates a "hold current position" trajectory.
  boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &hold = *hold_ptr;
  hold[0].start_time = last_time_.toSec() - 0.001;
  hold[0].duration = 0.0;
  hold[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    hold[0].splines[j].coef[0] = joints_[j]->position_;

  current_trajectory_box_.set(hold_ptr);
}

void JointTrajectoryActionController::update()
{
  ros::Time time = robot_->getTime();
  ros::Duration dt = time - last_time_;
  last_time_ = time;

  boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
  boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);

  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
    ROS_FATAL("The current trajectory can never be null");

  // Only because this is what the code originally looked like.
  const SpecifiedTrajectory &traj = *traj_ptr;

  // ------ Finds the current segment

  // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < time.toSec())
  {
    ++seg;
  }

  if (seg == -1)
  {
    if (traj.size() == 0)
      ROS_ERROR("No segments in the trajectory");
    else
      ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
    return;
  }

  // ------ Trajectory Sampling

  for (size_t i = 0; i < q.size(); ++i)
  {
    sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
                               time.toSec() - traj[seg].start_time,
                               q[i], qd[i], qdd[i]);
  }

  // ------ Trajectory Following

  std::vector<double> error(joints_.size());
  std::vector<double> v_error(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    double effort;

    // Compute the errors with respect to the desired trajectory
    // (whether or not using the proxy controller).  They are also
    // used later to determine reaching the goal.
    error[i] = q[i] - joints_[i]->position_;
    v_error[i] = qd[i] - joints_[i]->velocity_;

    // Use the proxy controller (if enabled)
    if (proxies_enabled_[i]) {
      effort = proxies_[i].update(q[i], qd[i], qdd[i],
                                  joints_[i]->position_, joints_[i]->velocity_,
                                  dt.toSec());
    }
    else {
      effort = pids_[i].computeCommand(error[i], v_error[i], dt);

      double effort_unfiltered = effort;
      if (output_filters_[i])
        output_filters_[i]->update(effort_unfiltered, effort);
    }

    // Apply the effort.  WHY IS THIS ADDITIVE?
    joints_[i]->commanded_effort_ = effort;
  }

  // ------ Determines if the goal has failed or succeeded

  if ((traj[seg].gh && traj[seg].gh == current_active_goal) ||
      (traj[seg].gh_follow && traj[seg].gh_follow == current_active_goal_follow))
  {
    ros::Time end_time(traj[seg].start_time + traj[seg].duration);
    if (time <= end_time)
    {
      // Verifies trajectory tolerances
      for (size_t j = 0; j < joints_.size(); ++j)
      {
        if (traj[seg].trajectory_tolerance[j].violated(error[j], v_error[j]))
        {
          if (traj[seg].gh)
            traj[seg].gh->setAborted();
          else if (traj[seg].gh_follow) {
            traj[seg].gh_follow->preallocated_result_->error_code =
              control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
            traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
          }
          break;
        }
      }
    }
    else if (seg == (int)traj.size() - 1)
    {
      // Checks that we have ended inside the goal tolerances
      bool inside_goal_constraints = true;
      for (size_t i = 0; i < joints_.size() && inside_goal_constraints; ++i)
      {
        if (traj[seg].goal_tolerance[i].violated(error[i], v_error[i]))
          inside_goal_constraints = false;
      }

      if (inside_goal_constraints)
      {
        rt_active_goal_.reset();
        rt_active_goal_follow_.reset();
        if (traj[seg].gh)
          traj[seg].gh->setSucceeded();
        else if (traj[seg].gh_follow) {
          traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
          traj[seg].gh_follow->setSucceeded(traj[seg].gh_follow->preallocated_result_);
        }
        //ROS_ERROR("Success!  (%s)", traj[seg].gh->gh_.getGoalID().id.c_str());
      }
      else if (time < end_time + ros::Duration(traj[seg].goal_time_tolerance))
      {
        // Still have some time left to make it.
      }
      else
      {
        //ROS_WARN("Aborting because we wound up outside the goal constraints");
        rt_active_goal_.reset();
        rt_active_goal_follow_.reset();
        if (traj[seg].gh)
          traj[seg].gh->setAborted();
        else if (traj[seg].gh_follow) {
          traj[seg].gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
          traj[seg].gh_follow->setAborted(traj[seg].gh_follow->preallocated_result_);
        }
      }
    }
  }

  // ------ State publishing

  if (loop_count_ % 10 == 0)
  {
    if (controller_state_publisher_ && controller_state_publisher_->trylock())
    {
      controller_state_publisher_->msg_.header.stamp = time;
      for (size_t j = 0; j < joints_.size(); ++j)
      {
        controller_state_publisher_->msg_.desired.positions[j] = q[j];
        controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
        controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
        controller_state_publisher_->msg_.desired.effort[j] = joints_[j]->commanded_effort_;
        controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
        controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
        controller_state_publisher_->msg_.actual.effort[j] = joints_[j]->measured_effort_;
        controller_state_publisher_->msg_.error.positions[j] = error[j];
        controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
        controller_state_publisher_->msg_.error.effort[j] = joints_[j]->measured_effort_ - joints_[j]->commanded_effort_;
      }
      controller_state_publisher_->unlockAndPublish();
    }
    if (current_active_goal_follow)
    {
      current_active_goal_follow->preallocated_feedback_->header.stamp = time;
      current_active_goal_follow->preallocated_feedback_->joint_names.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->desired.positions.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->desired.velocities.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->desired.accelerations.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->desired.effort.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->actual.positions.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->actual.velocities.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->actual.effort.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->error.positions.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->error.velocities.resize(joints_.size());
      current_active_goal_follow->preallocated_feedback_->error.effort.resize(joints_.size());
      for (size_t j = 0; j < joints_.size(); ++j)
      {
        current_active_goal_follow->preallocated_feedback_->joint_names[j] = joints_[j]->joint_->name;
        current_active_goal_follow->preallocated_feedback_->desired.positions[j] = q[j];
        current_active_goal_follow->preallocated_feedback_->desired.velocities[j] = qd[j];
        current_active_goal_follow->preallocated_feedback_->desired.accelerations[j] = qdd[j];
        current_active_goal_follow->preallocated_feedback_->desired.effort[j] = joints_[j]->commanded_effort_;
        current_active_goal_follow->preallocated_feedback_->actual.positions[j] = joints_[j]->position_;
        current_active_goal_follow->preallocated_feedback_->actual.velocities[j] = joints_[j]->velocity_;
        current_active_goal_follow->preallocated_feedback_->actual.effort[j] = joints_[j]->measured_effort_;
        current_active_goal_follow->preallocated_feedback_->error.positions[j] = error[j];
        current_active_goal_follow->preallocated_feedback_->error.velocities[j] = joints_[j]->velocity_ - qd[j];
        current_active_goal_follow->preallocated_feedback_->error.effort[j] = joints_[j]->measured_effort_ - joints_[j]->commanded_effort_;
      }
      const actionlib_msgs::GoalID goalID = current_active_goal_follow->gh_.getGoalID();
      ros::Duration time_from_start = time - goalID.stamp;
      current_active_goal_follow->preallocated_feedback_->desired.time_from_start = time_from_start;
      current_active_goal_follow->preallocated_feedback_->actual.time_from_start = time_from_start;
      current_active_goal_follow->preallocated_feedback_->error.time_from_start = time_from_start;
      current_active_goal_follow->gh_.publishFeedback(*current_active_goal_follow->preallocated_feedback_);

    }
  }

  ++loop_count_;
}

void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
  preemptActiveGoal();
  commandTrajectory(msg);
}

void JointTrajectoryActionController::commandTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &msg,
                                                        boost::shared_ptr<RTGoalHandle> gh,
                                                        boost::shared_ptr<RTGoalHandleFollow> gh_follow)
{
  assert(!gh || !gh_follow);
  ros::Time time = last_time_ + ros::Duration(0.01);
  ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
            time.toSec(), msg->header.stamp.toSec());

  boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
  SpecifiedTrajectory &new_traj = *new_traj_ptr;

  // ------ If requested, performs a stop

  if (msg->points.empty())
  {
    starting();
    return;
  }

  // ------ Computes the tolerances for following the trajectory

  std::vector<JointTolerance> trajectory_tolerance = default_trajectory_tolerance_;
  std::vector<JointTolerance> goal_tolerance = default_goal_tolerance_;
  double goal_time_tolerance = default_goal_time_constraint_;

  if (gh_follow)
  {
    for (size_t j = 0; j < joints_.size(); ++j)
    {
      // Extracts the path tolerance from the command
      for (size_t k = 0; k < gh_follow->gh_.getGoal()->path_tolerance.size(); ++k)
      {
        const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->path_tolerance[k];
        if (joints_[j]->joint_->name == tol.name)
        {
          // If the commanded tolerances are positive, overwrite the
          // existing tolerances.  If they are -1, remove any existing
          // tolerance.  If they are 0, leave the default tolerance in
          // place.

          if (tol.position > 0)
            trajectory_tolerance[j].position = tol.position;
          else if (tol.position < 0)
            trajectory_tolerance[j].position = 0.0;

          if (tol.velocity > 0)
            trajectory_tolerance[j].velocity = tol.velocity;
          else if (tol.velocity < 0)
            trajectory_tolerance[j].velocity = 0.0;

          if (tol.acceleration > 0)
            trajectory_tolerance[j].acceleration = tol.acceleration;
          else if (tol.acceleration < 0)
            trajectory_tolerance[j].acceleration = 0.0;

          break;
        }
      }

      // Extracts the goal tolerance from the command
      for (size_t k = 0; k < gh_follow->gh_.getGoal()->goal_tolerance.size(); ++k)
      {
        const control_msgs::JointTolerance &tol = gh_follow->gh_.getGoal()->goal_tolerance[k];
        if (joints_[j]->joint_->name == tol.name)
        {
          // If the commanded tolerances are positive, overwrite the
          // existing tolerances.  If they are -1, remove any existing
          // tolerance.  If they are 0, leave the default tolerance in
          // place.

          if (tol.position > 0)
            goal_tolerance[j].position = tol.position;
          else if (tol.position < 0)
            goal_tolerance[j].position = 0.0;

          if (tol.velocity > 0)
            goal_tolerance[j].velocity = tol.velocity;
          else if (tol.velocity < 0)
            goal_tolerance[j].velocity = 0.0;

          if (tol.acceleration > 0)
            goal_tolerance[j].acceleration = tol.acceleration;
          else if (tol.acceleration < 0)
            goal_tolerance[j].acceleration = 0.0;

          break;
        }
      }
    }

    const ros::Duration &tol = gh_follow->gh_.getGoal()->goal_time_tolerance;
    if (tol < ros::Duration(0.0))
      goal_time_tolerance = 0.0;
    else if (tol > ros::Duration(0.0))
      goal_time_tolerance = tol.toSec();
  }

  // ------ Correlates the joints we're commanding to the joints in the message

  std::vector<int> lookup(joints_.size(), -1);  // Maps from an index in joints_ to an index in the msg
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    for (size_t k = 0; k < msg->joint_names.size(); ++k)
    {
      if (msg->joint_names[k] == joints_[j]->joint_->name)
      {
        lookup[j] = k;
        break;
      }
    }

    if (lookup[j] == -1)
    {
      ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
      if (gh)
        gh->setAborted();
      else if (gh_follow) {
        gh_follow->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
        gh_follow->setAborted(gh_follow->preallocated_result_);
      }
      return;
    }
  }

  // ------ Grabs the trajectory that we're currently following.

  boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
  current_trajectory_box_.get(prev_traj_ptr);
  if (!prev_traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return;
  }
  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;

  // ------ Copies over the segments from the previous trajectory that are still useful.

  // Useful segments are still relevant after the current time.
  int first_useful = -1;
  while (first_useful + 1 < (int)prev_traj.size() &&
         prev_traj[first_useful + 1].start_time <= time.toSec())
  {
    ++first_useful;
  }

  // Useful segments are not going to be completely overwritten by the message's splines.
  int last_useful = -1;
  double msg_start_time;
  if (msg->header.stamp == ros::Time(0.0))
    msg_start_time = time.toSec();
  else
    msg_start_time = msg->header.stamp.toSec();
  /*
  if (msg->points.size() > 0)
    msg_start_time += msg->points[0].time_from_start.toSec();
  */

  while (last_useful + 1 < (int)prev_traj.size() &&
         prev_traj[last_useful + 1].start_time < msg_start_time)
  {
    ++last_useful;
  }

  if (last_useful < first_useful)
    first_useful = last_useful;

  // Copies over the old segments that were determined to be useful.
  for (int i = std::max(first_useful,0); i <= last_useful; ++i)
  {
    new_traj.push_back(prev_traj[i]);
  }

  // We always save the last segment so that we know where to stop if
  // there are no new segments.
  if (new_traj.size() == 0)
    new_traj.push_back(prev_traj[prev_traj.size() - 1]);

  // ------ Determines when and where the new segments start

  // Finds the end conditions of the final segment
  Segment &last = new_traj[new_traj.size() - 1];
  std::vector<double> prev_positions(joints_.size());
  std::vector<double> prev_velocities(joints_.size());
  std::vector<double> prev_accelerations(joints_.size());

  double t = (msg->header.stamp == ros::Time(0.0) ? time.toSec() : msg->header.stamp.toSec())
    - last.start_time;
  ROS_DEBUG("Initial conditions at %.3f for new set of splines:", t);
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
                               t,
                               prev_positions[i], prev_velocities[i], prev_accelerations[i]);
    ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
              prev_accelerations[i], joints_[i]->joint_->name.c_str());
  }

  // ------ Tacks on the new segments

  std::vector<double> positions;
  std::vector<double> velocities;
  std::vector<double> accelerations;

  std::vector<double> durations(msg->points.size());
  if (msg->points.size() > 0)
    durations[0] = msg->points[0].time_from_start.toSec();
  for (size_t i = 1; i < msg->points.size(); ++i)
    durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();

  // Checks if we should wrap
  std::vector<double> wrap(joints_.size(), 0.0);
  assert(!msg->points[0].positions.empty());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
    {
      double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[lookup[j]]);
      wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[lookup[j]];
    }
  }

  for (size_t i = 0; i < msg->points.size(); ++i)
  {
    Segment seg;

    if (msg->header.stamp == ros::Time(0.0))
      seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
    else
      seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
    seg.duration = durations[i];
    seg.gh = gh;
    seg.gh_follow = gh_follow;
    seg.splines.resize(joints_.size());

    // Checks that the incoming segment has the right number of elements.

    if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
      return;
    }
    if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
      return;
    }
    if (msg->points[i].positions.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
      return;
    }

    // Re-orders the joints in the command to match the internal joint order.

    accelerations.resize(msg->points[i].accelerations.size());
    velocities.resize(msg->points[i].velocities.size());
    positions.resize(msg->points[i].positions.size());
    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
      if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
      if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
    }

    // Converts the boundary conditions to splines.

    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (prev_accelerations.size() > 0 && accelerations.size() > 0)
      {
        getQuinticSplineCoefficients(
          prev_positions[j], prev_velocities[j], prev_accelerations[j],
          positions[j], velocities[j], accelerations[j],
          durations[i],
          seg.splines[j].coef);
      }
      else if (prev_velocities.size() > 0 && velocities.size() > 0)
      {
        getCubicSplineCoefficients(
          prev_positions[j], prev_velocities[j],
          positions[j], velocities[j],
          durations[i],
          seg.splines[j].coef);
        seg.splines[j].coef.resize(6, 0.0);
      }
      else
      {
        seg.splines[j].coef[0] = prev_positions[j];
        if (durations[i] == 0.0)
          seg.splines[j].coef[1] = 0.0;
        else
          seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
        seg.splines[j].coef[2] = 0.0;
        seg.splines[j].coef[3] = 0.0;
        seg.splines[j].coef[4] = 0.0;
        seg.splines[j].coef[5] = 0.0;
      }
    }

    // Writes the tolerances into the segment
    seg.trajectory_tolerance = trajectory_tolerance;
    seg.goal_tolerance = goal_tolerance;
    seg.goal_time_tolerance = goal_time_tolerance;

    // Pushes the splines onto the end of the new trajectory.

    new_traj.push_back(seg);

    // Computes the starting conditions for the next segment

    prev_positions = positions;
    prev_velocities = velocities;
    prev_accelerations = accelerations;
  }

  //ROS_ERROR("Last segment goal id: %s", new_traj[new_traj.size()-1].gh->gh_.getGoalID().id.c_str());

  // ------ Commits the new trajectory

  if (!new_traj_ptr)
  {
    ROS_ERROR("The new trajectory was null!");
    return;
  }

  current_trajectory_box_.set(new_traj_ptr);
  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
#if 0
  for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
  {
    ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
    for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
    {
      ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf , %.2lf  %.2lf(%s)",
                new_traj[i].splines[j].coef[0],
                new_traj[i].splines[j].coef[1],
                new_traj[i].splines[j].coef[2],
                new_traj[i].splines[j].coef[3],
                new_traj[i].splines[j].coef[4],
                new_traj[i].splines[j].coef[5],
                joints_[j]->joint_->name_.c_str());
    }
  }
#endif
}

bool JointTrajectoryActionController::queryStateService(
  pr2_controllers_msgs::QueryTrajectoryState::Request &req,
  pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
{
  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return false;
  }
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < req.time.toSec())
  {
    ++seg;
  }
  if (seg == -1)
    return false;

  for (size_t i = 0; i < q.size(); ++i)
  {
  }

  resp.name.resize(joints_.size());
  resp.position.resize(joints_.size());
  resp.velocity.resize(joints_.size());
  resp.acceleration.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    resp.name[j] = joints_[j]->joint_->name;
    sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
                               req.time.toSec() - traj[seg].start_time,
                               resp.position[j], resp.velocity[j], resp.acceleration[j]);
  }

  return true;
}

void JointTrajectoryActionController::sampleSplineWithTimeBounds(
  const std::vector<double>& coefficients, double duration, double time,
  double& position, double& velocity, double& acceleration)
{
  if (time < 0)
  {
    double _;
    sampleQuinticSpline(coefficients, 0.0, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else if (time > duration)
  {
    double _;
    sampleQuinticSpline(coefficients, duration, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else
  {
    sampleQuinticSpline(coefficients, time,
                        position, velocity, acceleration);
  }
}

static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
{
  if (a.size() != b.size())
    return false;

  for (size_t i = 0; i < a.size(); ++i)
  {
    if (count(b.begin(), b.end(), a[i]) != 1)
      return false;
  }
  for (size_t i = 0; i < b.size(); ++i)
  {
    if (count(a.begin(), a.end(), b[i]) != 1)
      return false;
  }

  return true;
}

void JointTrajectoryActionController::preemptActiveGoal()
{
  boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
  boost::shared_ptr<RTGoalHandleFollow> current_active_goal_follow(rt_active_goal_follow_);

  // Cancels the currently active goal.
  if (current_active_goal)
  {
    // Marks the current goal as canceled.
    rt_active_goal_.reset();
    current_active_goal->gh_.setCanceled();
  }
  if (current_active_goal_follow)
  {
    rt_active_goal_follow_.reset();
    current_active_goal_follow->gh_.setCanceled();
  }
}

template <class Enclosure, class Member>
static boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
{
  actionlib::EnclosureDeleter<Enclosure> d(enclosure);
  boost::shared_ptr<Member> p(&member, d);
  return p;
}

void JointTrajectoryActionController::goalCB(GoalHandle gh)
{
  std::vector<std::string> joint_names(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    joint_names[j] = joints_[j]->joint_->name;

  // Ensures that the joints in the goal match the joints we are commanding.
  if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
  {
    ROS_ERROR("Joints on incoming goal don't match our joints");
    gh.setRejected();
    return;
  }

  preemptActiveGoal();

  gh.setAccepted();
  boost::shared_ptr<RTGoalHandle> rt_gh(new RTGoalHandle(gh));

  // Sends the trajectory along to the controller
  goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandle::runNonRT, rt_gh);
  commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory), rt_gh);
  rt_active_goal_ = rt_gh;
  goal_handle_timer_.start();
}

void JointTrajectoryActionController::goalCBFollow(GoalHandleFollow gh)
{
  std::vector<std::string> joint_names(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    joint_names[j] = joints_[j]->joint_->name;

  // Ensures that the joints in the goal match the joints we are commanding.
  if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
  {
    ROS_ERROR("Joints on incoming goal don't match our joints");
    control_msgs::FollowJointTrajectoryResult result;
    result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
    gh.setRejected(result);
    return;
  }

  preemptActiveGoal();

  gh.setAccepted();
  boost::shared_ptr<RTGoalHandleFollow> rt_gh(new RTGoalHandleFollow(gh));

  // Sends the trajectory along to the controller
  goal_handle_timer_ = node_.createTimer(ros::Duration(0.01), &RTGoalHandleFollow::runNonRT, rt_gh);
  commandTrajectory(share_member(gh.getGoal(), gh.getGoal()->trajectory),
                    boost::shared_ptr<RTGoalHandle>((RTGoalHandle*)NULL),
                    rt_gh);
  rt_active_goal_follow_ = rt_gh;
  goal_handle_timer_.start();
}

void JointTrajectoryActionController::cancelCB(GoalHandle gh)
{
  boost::shared_ptr<RTGoalHandle> current_active_goal(rt_active_goal_);
  if (current_active_goal && current_active_goal->gh_ == gh)
  {
    rt_active_goal_.reset();

    trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
    empty->joint_names.resize(joints_.size());
    for (size_t j = 0; j < joints_.size(); ++j)
      empty->joint_names[j] = joints_[j]->joint_->name;
    commandTrajectory(empty);

    // Marks the current goal as canceled.
    current_active_goal->gh_.setCanceled();
  }
}

void JointTrajectoryActionController::cancelCBFollow(GoalHandleFollow gh)
{
  boost::shared_ptr<RTGoalHandleFollow> current_active_goal(rt_active_goal_follow_);
  if (current_active_goal && current_active_goal->gh_ == gh)
  {
    rt_active_goal_follow_.reset();

    trajectory_msgs::JointTrajectory::Ptr empty(new trajectory_msgs::JointTrajectory);
    empty->joint_names.resize(joints_.size());
    for (size_t j = 0; j < joints_.size(); ++j)
      empty->joint_names[j] = joints_[j]->joint_->name;
    commandTrajectory(empty);

    // Marks the current goal as canceled.
    current_active_goal->gh_.setCanceled();
  }
}
}


#include <iostream>

#include <bs/ewma.hpp>
#include <bs/frame_range.hpp>
#include <bs/fuzzy_choquet.hpp>
#include <bs/utils.hpp>

#include <boost/make_shared.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <range/v3/front.hpp>
#include <range/v3/action/take.hpp>
#include <range/v3/view/take.hpp>

#include <options.hpp>
#include <run.hpp>

namespace std {

template< typename T >
inline std::ostream&
operator<< (std::ostream& s, const std::vector< T >& v) {
    copy (v.begin (), v.end (), ostream_iterator< T > (s, ","));
    return s;
}

}

//
// options_t::options_t is specific to each example:
//
options_t::options_t (int argc, char** argv) {
    {
        auto tmp = std::make_pair (
            "program", po::variable_value (std::string (argv [0]), false));
        map_.insert (tmp);
    }

    po::options_description generic ("Generic options");
    po::options_description config ("Configuration options");

    generic.add_options ()
    ("version", "version")
    ("help", "this");

    config.add_options ()
    ("display,d", "display frames.")

    ("input,i",   po::value< std::string > ()->default_value ("0"),
     "input (file or stream index).")

    ("measure,m", po::value< std::vector< double > > ()->multitoken ()
     ->default_value (std::vector< double > { .6, .3, .1 }),
     "lambda measure, sum must be 1")

    ("alpha,a", po::value< double > ()->default_value (.01),
     "alpha")

    ("background-alpha,b", po::value< double > ()->default_value (.1),
     "alpha for background learning")

    ("learning-frames,n", po::value< size_t > ()->default_value (10UL),
     "number of learning frames")

    ("threshold,t", po::value< double > ()->default_value (.67),
     "threshold value");

    desc_ = boost::make_shared< po::options_description > ();

    desc_->add (generic);
    desc_->add (config);

    store (po::command_line_parser (argc, argv).options (*desc_).run (), map_);

    notify (map_);
}

////////////////////////////////////////////////////////////////////////

static void
program_options_from (int& argc, char** argv) {
    bool complete_invocation = false;

    options_t program_options (argc, argv);

    if (program_options.have ("version")) {
        std::cout << "OpenCV v3.1\n";
        complete_invocation = true;
    }

    if (program_options.have ("help")) {
        std::cout << program_options.description () << std::endl;
        complete_invocation = true;
    }

    if (complete_invocation)
        exit (0);

    global_options (program_options);
}

////////////////////////////////////////////////////////////////////////

static cv::Mat
bootstrap (cv::VideoCapture& cap) {
    using namespace ranges;

    //
    // The bootstrapping function:
    //
    bs::ewma_t f;

    //
    // Learn the background over 15 frames:
    //
    auto frames = bs::getframes_from (cap);

    for (auto frame : (frames | view::take (15))) {
        f (bs::float_from (frame));
    }

    return f.value ();
}

static void
process_fuzzy_choquet (cv::VideoCapture& cap, const options_t& opts) {
    const bool display = opts.have ("display");

    auto background_model = bootstrap (cap);

    bs::fuzzy_choquet_t fuzzy_choquet (
        background_model,
        opts ["alpha"].as< double > (),
        opts ["threshold"].as< double > (),
        opts ["measure"].as< std::vector< double > > ());

    for (auto& frame : bs::getframes_from (cap)) {
        bs::frame_delay temp { 10 };

        const auto mask = fuzzy_choquet (frame);

        if (display)
            imshow ("Fuzzy Choquet filter", mask);

        if (temp.wait_for_key (27))
            break;
    }
}

////////////////////////////////////////////////////////////////////////

int main (int argc, char** argv) {
    program_options_from (argc, argv);
    return run_with (process_fuzzy_choquet, global_options ()), 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "list_to_matrix.h"

#include <cassert>
#include <cstdio>

#include <Eigen/Dense>

#include "max_size.h"
#include "min_size.h"

template <typename T, typename Derived>
IGL_INLINE bool igl::list_to_matrix(const std::vector<std::vector<T > > & V,Eigen::PlainObjectBase<Derived>& M)
{
  // number of rows
  int m = V.size();
  if(m == 0)
  {
    M.resize(0,0);
    return true;
  }
  // number of columns
  int n = igl::min_size(V);
  if(n != igl::max_size(V))
  {
    return false;
  }
  assert(n != -1);
  // Resize output
  M.resize(m,n);

  // Loop over rows
  for(int i = 0;i<m;i++)
  {
    // Loop over cols
    for(int j = 0;j<n;j++)
    {
      M(i,j) = V[i][j];
    }
  }

  return true;
}

template <typename T, typename Derived>
IGL_INLINE bool igl::list_to_matrix(
  const std::vector<std::vector<T > > & V,
  const int n,
  const T & padding,
  Eigen::PlainObjectBase<Derived>& M)
{
  const int m = V.size();
  M.resize(m,n);
  for(int i = 0;i<m;i++)
  {
    const auto & row = V[i];
    if(row.size()>n)
    {
      return false;
    }
    int j = 0;
    for(;j<row.size();j++)
    {
      M(i,j) = row[j];
    }
    for(;j<n;j++)
    {
      M(i,j) = padding;
    }
  }
  return true;
}

template <typename T, typename Derived>
IGL_INLINE bool igl::list_to_matrix(const std::vector<T > & V,Eigen::PlainObjectBase<Derived>& M)
{
  // number of rows
  int m = V.size();
  if(m == 0)
  {
    //fprintf(stderr,"Error: list_to_matrix() list is empty()\n");
    //return false;
    if(Derived::ColsAtCompileTime == 1)
    {
      M.resize(0,1);
    }else if(Derived::RowsAtCompileTime == 1)
    {
      M.resize(1,0);
    }else
    {
      M.resize(0,0);
    }
    return true;
  }
  // Resize output
  if(Derived::RowsAtCompileTime == 1)
  {
    M.resize(1,m);
  }else
  {
    M.resize(m,1);
  }

  // Loop over rows
  for(int i = 0;i<m;i++)
  {
    M(i) = V[i];
  }

  return true;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
// generated by autoexplicit.sh
// generated by autoexplicit.sh
template bool igl::list_to_matrix<long, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<std::vector<long, std::allocator<long> >, std::allocator<std::vector<long, std::allocator<long> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<bool, Eigen::Array<bool, -1, 3, 0, -1, 3> >(std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > > const&, Eigen::PlainObjectBase<Eigen::Array<bool, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 1, -1, -1> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 1, -1, -1> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, 2, 0, -1, 2> >(std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, 2, 0, -1, 2> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<double, 4, 1, 0, 4, 1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 4, 1, 0, 4, 1> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> >&);
// generated by autoexplicit.sh
template bool igl::list_to_matrix<double, Eigen::Matrix<double, 2, 1, 0, 2, 1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 1, 0, 2, 1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, 3, 1, -1, 3> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> >(std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template bool igl::list_to_matrix<float, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(std::vector<std::vector<float, std::allocator<float> >, std::allocator<std::vector<float, std::allocator<float> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template bool igl::list_to_matrix<unsigned int, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> >(std::vector<std::vector<unsigned int, std::allocator<unsigned int> >, std::allocator<std::vector<unsigned int, std::allocator<unsigned int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template bool igl::list_to_matrix<unsigned long, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(std::vector<unsigned long, std::allocator<unsigned long> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template bool igl::list_to_matrix<float, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(std::vector<std::vector<float, std::allocator<float> >, std::allocator<std::vector<float, std::allocator<float> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<bool, Eigen::Array<bool, -1, 1, 0, -1, 1> >(std::vector<bool, std::allocator<bool> > const&, Eigen::PlainObjectBase<Eigen::Array<bool, -1, 1, 0, -1, 1> >&);
template bool igl::list_to_matrix<bool, Eigen::Array<bool, -1, -1, 0, -1, -1> >(std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > > const&, Eigen::PlainObjectBase<Eigen::Array<bool, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<bool, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<bool, std::allocator<bool> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<bool, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<std::vector<bool, std::allocator<bool> >, std::allocator<std::vector<bool, std::allocator<bool> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<int, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<unsigned long, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<unsigned long, std::allocator<unsigned long> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<int, Eigen::Matrix<int, -1, 3, 1, -1, 3> >(std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> >&);
template bool igl::list_to_matrix<unsigned long, Eigen::Matrix<long, -1, 1, 0, -1, 1> >(std::vector<unsigned long, std::allocator<unsigned long> > const&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> >&);

#ifdef WIN32
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(std::vector<double, std::allocator<double> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::list_to_matrix<unsigned __int64, class Eigen::Matrix<int, -1, -1, 0, -1, -1> >(class std::vector<unsigned __int64, class std::allocator<unsigned __int64> > const &, class Eigen::PlainObjectBase<class Eigen::Matrix<int, -1, -1, 0, -1, -1> > &);
template bool igl::list_to_matrix<unsigned __int64,class Eigen::Matrix<__int64,-1,1,0,-1,1> >(class std::vector<unsigned __int64,class std::allocator<unsigned __int64> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<__int64,-1,1,0,-1,1> > &);
template bool igl::list_to_matrix<unsigned __int64,class Eigen::Matrix<long,-1,1,0,-1,1> >(class std::vector<unsigned __int64,class std::allocator<unsigned __int64> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<long,-1,1,0,-1,1> > &);
template bool igl::list_to_matrix<unsigned long long,class Eigen::Matrix<int,-1,1,0,-1,1> >(class std::vector<unsigned long long,class std::allocator<unsigned long long> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<int,-1,1,0,-1,1> > &);
template bool igl::list_to_matrix<unsigned long, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(std::vector<unsigned long, std::allocator<unsigned long> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
#endif
#endif


#ifndef _COCONUT_PULP_PRIMITIVE_POSITION_HPP_
#define _COCONUT_PULP_PRIMITIVE_POSITION_HPP_

#include <boost/operators.hpp>

#include <coconut-tools/serialisation.hpp>

#include "detail/VectorType.hpp"
#include "Vector.hpp"

namespace coconut {
namespace pulp {
namespace primitive {

class Position :
	boost::additive<Position, Vector>,
	detail::VectorType<4>
{
public:

	Position() = default;

	Position(const pulp::math::Vec3& vector) :
		detail::VectorType<4>(vector.x(), vector.y(), vector.z(), 1.0f)
	{
	}

	Position(const pulp::math::Vec4& vector) :
		detail::VectorType<4>(vector.x(), vector.y(), vector.z(), 1.0f)
	{
	}

	Position(float x, float y, float z) :
		detail::VectorType<4>(x, y, z, 1.0f)
	{
	}

	Position& operator+=(const primitive::Vector& rhs) {
		static_cast<detail::VectorType<4>&>(*this) += rhs;
		return *this;
	}

	Position& operator-=(const primitive::Vector& rhs) {
		static_cast<detail::VectorType<4>&>(*this) -= rhs;
		return *this;
	}

	float& x() {
		return get<0>();
	}

	float x() const {
		return get<0>();
	}

	float& y() {
		return get<1>();
	}

	float y() const {
		return get<1>();
	}

	float& z() {
		return get<2>();
	}

	float z() const {
		return get<2>();
	}

	using detail::VectorType<4>::storeAs;

private:

	friend primitive::Vector operator-(const Position& lhs, const Position& rhs) {
		return primitive::Vector(lhs.x() - rhs.x(), lhs.y() - rhs.y(), lhs.z() - rhs.z());
	}

	friend std::ostream& operator<<(std::ostream& os, const Position& position) {
		return os << static_cast<const detail::VectorType<4>&>(position);
	}

	friend class primitive::Vector;

};

CT_MAKE_SERIALISABLE(SerialiserType, serialiser, Position, position) {
	serialiser(SerialiserType::Label("x"), position.x());
	serialiser(SerialiserType::Label("y"), position.y());
	serialiser(SerialiserType::Label("z"), position.z());
}

} // namespace primitive

using primitive::Position;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_PRIMITIVE_POSITION_HPP_ */


#include "tree.h"

#include "pthread.h"
#include <boost/qvm/vec_access.hpp>
#include <iostream>
#include <omp.h>

using namespace boost::qvm;

OctreeNode::OctreeNode(Particles p, BoundingBox bbox)
    : particles(p), bounding_box(bbox) {
  children.reserve(8);
}

boost::qvm::vec<float, 3> OctreeNode::get_min_bounds() {
  return bounding_box.get_min();
}

boost::qvm::vec<float, 3> OctreeNode::get_max_bounds() {
  return bounding_box.get_max();
}

BoundingBox &OctreeNode::get_bounding_box() { return bounding_box; }

void OctreeNode::add_particle(Particle particle) {
  particles.add_particle(particle);
}

Particle *OctreeNode::get_particle(size_t index) {
  return &particles.get(index);
}

size_t OctreeNode::get_particle_count() { return particles.get_count(); }

OctreeNode *OctreeNode::get_child(size_t n) { return children[n]; }

void OctreeNode::set_parent(OctreeNode *p) { this->parent = p; }

OctreeNode *OctreeNode::get_parent() { return this->parent; }

void OctreeNode::build_children() {
  // Split the BoundingBox into octants
  vec<float, 3> p_min = bounding_box.get_min();
  vec<float, 3> p_max = bounding_box.get_max();
  vec<float, 3> center = bounding_box.get_center();

  // Create OctreeNodes for each octant and add them to the children
  Particles empty_particles;
  // Top-Left Front
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(p_min), A<1>(center), A<2>(center),
                                   A<0>(center), A<2>(p_max), A<2>(p_max))));

  // Top-Right Front
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(center), A<1>(center), A<2>(center),
                                   A<0>(p_max), A<1>(p_max), A<2>(p_max))));

  // Bottom-Left Front
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(p_min), A<1>(p_min), A<2>(center),
                                   A<0>(center), A<1>(center), A<2>(p_max))));

  // Bottom-Right Front
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(center), A<1>(p_min), A<2>(center),
                                   A<0>(p_max), A<1>(center), A<2>(p_max))));

  // Top-Left Back
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(p_min), A<1>(center), A<2>(p_min),
                                   A<0>(center), A<1>(p_max), A<2>(center))));

  // Top-Right Back
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(center), A<1>(center), A<2>(p_min),
                                   A<0>(p_max), A<1>(p_max), A<2>(center))));

  // Bottom-Left Back
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(p_min), A<1>(p_min), A<2>(p_min),
                                   A<0>(center), A<1>(center), A<2>(center))));

  // Bottom-Right Back
  children.push_back(new OctreeNode(
      empty_particles, BoundingBox(A<0>(center), A<1>(p_min), A<2>(p_min),
                                   A<0>(p_max), A<1>(center), A<2>(center))));

  // Assign parent node as this for each child
  for (int i = 0; i < 8; i++) {
    children[i]->set_parent(this);

    // Loop through all the particles and assign them to the
    // correct OctreeNode
    for (int it = 0; it < this->get_particle_count(); it++) {
      if (children[i]->get_bounding_box().contains(
              particles.get(it).get_position()))
        children[i]->add_particle(particles.get(it));
    }
  }
}

// Recursive function to generate oct tree called using
void *make_tree_unbounded(void *node) {
  pthread_t child_threads[8];
  bool pthread_active[8] = {false};
  // Create the children of the root vector
  OctreeNode *root = static_cast<OctreeNode *>(node);
  if (root->get_particle_count() > 1) {
    root->build_children();

    // Now build chidren for each of that root's children as well
    for (size_t i = 0; i < 8; i++) {
      OctreeNode *child = root->get_child(i);
      // Only spawn a new thread if we have more than one particle in the area
      if (child->get_particle_count() > 1) {
        pthread_create(&child_threads[i], NULL, make_tree_unbounded, (void *)child);
        pthread_active[i] = true;
      }
    }

    // Wait for the children to finish
    for (int i = 0; i < 8; i++) {
      if (pthread_active[i])
        pthread_join(child_threads[i], NULL);
    }
  }
}

// Initialize the semaphore
void init_semaphore(int max_threads) {
  sem_init(&thread_availible, 0, max_threads);
}

// Destroy the semaphore
void deinit_semaphore() {
  sem_destroy(&thread_availible);
}

// Recursive function to generate oct tree called using
void *make_tree_bounded(void *node) {
  pthread_t child_threads[8];
  bool pthread_active[8] = {false};
  // Create the children of the root vector
  OctreeNode *root = static_cast<OctreeNode *>(node);
  if (root->get_particle_count() > 1) {
    root->build_children();

    // Now build children for each of that root's children as well
    for (size_t i = 0; i < 8; i++) {
      OctreeNode *child = root->get_child(i);
      // Only spawn a new thread if we have more than one particle in the area
      if (child->get_particle_count() > 1) {
        // When this passes fewer than 8 threads are running
        sem_wait(&thread_availible);
        pthread_create(&child_threads[i], NULL, make_tree_bounded, (void *)child);
        pthread_active[i] = true;
      }
    }

    // Wait for the children to finish
    for (int i = 0; i < 8; i++) {
      if(pthread_active[i])
        pthread_join(child_threads[i], NULL);
    }
  }
  // Increment the semaphore since this thread is done
  sem_post(&thread_availible);
}

// Recursive function to serially generate oct tree called using
void make_tree_serial(OctreeNode *node) {
  // Create the children of the root vector
  OctreeNode *root = node;
  if(root->get_particle_count() > 1) {
    root->build_children();

    // Now build children for each of that root's children as well
    for(size_t i = 0; i < 8; i++) {
      OctreeNode *child = root->get_child(i);
      // Only recurse down if we have more than one particle in the area
      if(child->get_particle_count() > 1) {
        make_tree_serial(child);
      }
    }
  }
}

// Recursive function to generate oct tree called using
void make_tree_openmp(OctreeNode *root) {
  // Create the children of the root vector
  omp_set_num_threads(8);
  if (root->get_particle_count() > 1) {
    root->build_children();

    // Now build chidren for each of that root's children as well
    #pragma omp parallel for
    for (size_t i = 0; i < 8; i++) {
      OctreeNode *child = root->get_child(i);
      // Only spawn a new thread if we have more than one particle in the area
      if (child->get_particle_count() > 1)
        make_tree_openmp(child);
    }
  }
}


#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include "MaxFlow.hpp"
using namespace std;
using namespace boost;

Network::Network(): g(), rev(get(edge_reverse, g)) {}

Network::Vertex Network::AddVertex() {
  return add_vertex(g);
}

void Network::AddEdge(Vertex &v1, Vertex &v2, const long capacity) {
  Traits::edge_descriptor e1 = add_edge(v1, v2, g).first;
  Traits::edge_descriptor e2 = add_edge(v2, v1, g).first;
  put(edge_capacity, g, e1, capacity);
  rev[e1] = e2;
  rev[e2] = e1;
}

unsigned long long Network::MaxFlow(Vertex &s, Vertex &t) {
  return push_relabel_max_flow(g, s, t);
}


#ifndef __PTSS_PKMIN
#define __PTSS_PKMIN

#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <math.h>
#include <chrono>
#include <exception>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include "ptss_dse.hpp"
#include "ptss_config.hpp"

double ptss_pkmin(alloc2_t init, unsigned int deadline, alloc2_t *opt_dggd);
#endif