#include "openbr_internal.h"
#include "openbr/core/opencvutils.h"
#include "openbr/core/common.h"
#include "openbr/core/qtutils.h"
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Dense>

using namespace cv;
using namespace Eigen;

namespace br
{

// Find avg aspect ratio
static float getAspectRatio(const TemplateList &data)
{
    double tempRatio = 0;
    int ratioCnt = 0;

    foreach (const Template &tmpl, data) {
        QList<Rect> posRects = OpenCVUtils::toRects(tmpl.file.rects());
        foreach (const Rect &posRect, posRects) {
            if (posRect.x + posRect.width >= tmpl.m().cols || posRect.y + posRect.height >= tmpl.m().rows || posRect.x < 0 || posRect.y < 0) {
                continue;
            }
            tempRatio += (float)posRect.width / (float)posRect.height;
            ratioCnt += 1;
        }
    }
    return tempRatio / (double)ratioCnt;
}

/*!
 * \ingroup transforms
 * \brief Applies a transform to a sliding window.
 *        Discards negative detections.
 * \author Austin Blanton \cite imaus10
 */
class SlidingWindowTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(br::Transform *transform READ get_transform WRITE set_transform RESET reset_transform STORED false)
    Q_PROPERTY(int windowWidth READ get_windowWidth WRITE set_windowWidth RESET reset_windowWidth STORED false)
    Q_PROPERTY(bool takeFirst READ get_takeFirst WRITE set_takeFirst RESET reset_takeFirst STORED false)
    Q_PROPERTY(float threshold READ get_threshold WRITE set_threshold RESET reset_threshold STORED false)
    Q_PROPERTY(float stepFraction READ get_stepFraction WRITE set_stepFraction RESET reset_stepFraction STORED false)
    Q_PROPERTY(int ignoreBorder READ get_ignoreBorder WRITE set_ignoreBorder RESET reset_ignoreBorder STORED true)
    BR_PROPERTY(br::Transform *, transform, NULL)
    BR_PROPERTY(int, windowWidth, 24)
    BR_PROPERTY(bool, takeFirst, false)
    BR_PROPERTY(float, threshold, 0)
    BR_PROPERTY(float, stepFraction, 0.25)
    BR_PROPERTY(int, ignoreBorder, 0)

private:
    int windowHeight;
    bool skipProject;

    void train(const TemplateList &data)
    {
        skipProject = true;
        float aspectRatio = data.first().file.get<float>("aspectRatio", -1);
        if (aspectRatio == -1)
            aspectRatio = getAspectRatio(data);
        windowHeight = qRound(windowWidth / aspectRatio);

        if (transform->trainable) {
            TemplateList dataOut = data;
            if (ignoreBorder > 0) {
                for (int i = 0; i < dataOut.size(); i++) {
                    Template t = dataOut[i];
                    Mat m = t.m();
                    dataOut.replace(i,Template(t.file, Mat(m,Rect(ignoreBorder,ignoreBorder,m.cols - ignoreBorder * 2, m.rows - ignoreBorder * 2))));
                }
            }
            transform->train(dataOut);
        }
    }

    void store(QDataStream &stream) const
    {
        transform->store(stream);
        stream << windowHeight;
    }

    void load(QDataStream &stream)
    {
        transform->load(stream);
        stream >> windowHeight;
    }

    void project(const Template &src, Template &dst) const
    {
        float scale = src.file.get<float>("scale", 1);
        projectHelp(src, dst, windowWidth, windowHeight, scale);
    }

 protected:
     void projectHelp(const Template &src, Template &dst, int windowWidth, int windowHeight, float scale = 1) const
     {

        dst = src;
        if (skipProject) {
            dst = src;
            return;
        }

        Template windowTemplate(src.file, src);
        QList<float> confidences = dst.file.getList<float>("Confidences", QList<float>());
        for (float y = 0; y + windowHeight < src.m().rows; y += windowHeight*stepFraction) {
            for (float x = 0; x + windowWidth < src.m().cols; x += windowWidth*stepFraction) {
                Mat windowMat(src, Rect(x + ignoreBorder, y + ignoreBorder, windowWidth - ignoreBorder * 2, windowHeight - ignoreBorder * 2));
                windowTemplate.replace(0,windowMat);
                Template detect;
                transform->project(windowTemplate, detect);
                float conf = detect.m().at<float>(0);

                // the result will be in the Label
                if (conf > threshold) {
                    dst.file.appendRect(QRectF(x*scale, y*scale, windowWidth*scale, windowHeight*scale));
                    confidences.append(conf);
                    if (takeFirst)
                        return;
                }
            }
        }
        dst.file.setList<float>("Confidences", confidences);
    }
};

BR_REGISTER(Transform, SlidingWindowTransform)

/*!
 * \ingroup transforms
 * \brief Overloads SlidingWindowTransform for integral images that should be
 *        sampled at multiple scales.
 * \author Josh Klontz \cite jklontz
 */
class IntegralSlidingWindowTransform : public SlidingWindowTransform
{
    Q_OBJECT

 private:
    void project(const Template &src, Template &dst) const
    {
        // TODO: call SlidingWindowTransform::project on multiple scales
        SlidingWindowTransform::projectHelp(src, dst, 24, 24);
    }
};

BR_REGISTER(Transform, IntegralSlidingWindowTransform)

static TemplateList cropTrainingSamples(const TemplateList &data, const float aspectRatio, const int minSize = 32, const float maxOverlap = 0.5, const int negToPosRatio = 1)
{
    TemplateList result;
    foreach (const Template &tmpl, data) {
        QList<Rect> posRects = OpenCVUtils::toRects(tmpl.file.rects());
        QList<Rect> negRects;
        for (int i=0; i<posRects.size(); i++) {
            Rect &posRect = posRects[i];

            // Adjust for training samples that have different aspect ratios
            const int diff = int(posRect.height * aspectRatio) - posRect.width;
            posRect.x -= diff / 2;
            posRect.width += diff;

            // Ignore samples larger than the image
            if ((posRect.x + posRect.width >= tmpl.m().cols) ||
                (posRect.y + posRect.height >= tmpl.m().rows) ||
                (posRect.x < 0) ||
                (posRect.y < 0))
                continue;

            result += Template(tmpl.file, Mat(tmpl, posRect));
            result.last().file.set("Label", QString("pos"));

            // Add random negative samples
            Mat m = tmpl.m();
            int sample = 0;
            while (sample < negToPosRatio) {
                const int x = rand() % m.cols;
                const int y = rand() % m.rows;
                const int maxWidth = m.cols - x;
                const int maxHeight = m.rows - y;
                if (maxWidth <= minSize || maxHeight <= minSize)
                    continue;

                int height;
                int width;
                if (aspectRatio > (float) maxWidth / (float) maxHeight) {
                    width = rand() % (maxWidth - minSize) + minSize;
                    height = qRound(width / aspectRatio);
                } else {
                    height = rand() % (maxHeight - minSize) + minSize;
                    width = qRound(height * aspectRatio);
                }
                Rect negRect(x, y, width, height);

                // The negative samples cannot overlap the positive samples at
                // all, but they may partially overlap with other negatives.
                if (OpenCVUtils::overlaps(posRects, negRect, 0) ||
                    OpenCVUtils::overlaps(negRects, negRect, maxOverlap))
                    continue;

                result += Template(tmpl.file, Mat(tmpl, negRect));
                result.last().file.set("Label", QString("neg"));
                sample++;
            }
        }
    }

    return result;
}

/*!
 * \ingroup transforms
 * \brief Document me
 * \author Austin Blanton \cite imaus10
 */
class BuildScalesTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(br::Transform *transform READ get_transform WRITE set_transform RESET reset_transform STORED false)
    Q_PROPERTY(double scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(bool takeLargestScale READ get_takeLargestScale WRITE set_takeLargestScale RESET reset_takeLargestScale STORED false)
    Q_PROPERTY(int windowWidth READ get_windowWidth WRITE set_windowWidth RESET reset_windowWidth STORED false)
    Q_PROPERTY(int negToPosRatio READ get_negToPosRatio WRITE set_negToPosRatio RESET reset_negToPosRatio STORED false)
    Q_PROPERTY(int minSize READ get_minSize WRITE set_minSize RESET reset_minSize STORED false)
    Q_PROPERTY(double maxOverlap READ get_maxOverlap WRITE set_maxOverlap RESET reset_maxOverlap STORED false)
    Q_PROPERTY(float minScale READ get_minScale WRITE set_minScale RESET reset_minScale STORED false)
    BR_PROPERTY(br::Transform *, transform, NULL)
    BR_PROPERTY(double, scaleFactor, 0.75)
    BR_PROPERTY(bool, takeLargestScale, false)
    BR_PROPERTY(int, windowWidth, 24)
    BR_PROPERTY(int, negToPosRatio, 1)
    BR_PROPERTY(int, minSize, 8)
    BR_PROPERTY(double, maxOverlap, 0)
    BR_PROPERTY(float, minScale, 1.0)

private:
    float aspectRatio;
    int windowHeight;
    bool skipProject;

    void train(const TemplateList &data)
    {
        skipProject = true;
        aspectRatio = getAspectRatio(data);
        windowHeight = qRound(windowWidth / aspectRatio);
        if (transform->trainable) {
            TemplateList full;
            foreach (const Template &roi, cropTrainingSamples(data, aspectRatio, minSize, maxOverlap, negToPosRatio)) {
                Mat resized;
                resize(roi, resized, Size(windowWidth, windowHeight));
                full += Template(roi.file, resized);
            }
            full.first().file.set("aspectRatio", aspectRatio);
            transform->train(full);
        }
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        if (skipProject) {
            dst = src;
            return;
        }

        int rows = src.m().rows;
        int cols = src.m().cols;
        int windowHeight = (int) qRound((float) windowWidth / aspectRatio);

        float startScale;
        if ((cols / rows) > aspectRatio)
            startScale = qRound((float) rows / (float) windowHeight);
        else
            startScale = qRound((float) cols / (float) windowWidth);

        for (float scale = startScale; scale >= minScale; scale -= (1.0 - scaleFactor)) {
            Template scaleImg(dst.file, Mat());
            scaleImg.file.set("scale", scale);
            resize(src, scaleImg, Size(qRound(cols / scale), qRound(rows / scale)));
            transform->project(scaleImg, dst);
            if (takeLargestScale && !dst.file.rects().empty())
                return;
        }
    }

    void store(QDataStream &stream) const
    {
        transform->store(stream);
        stream << aspectRatio << windowHeight;
    }
    void load(QDataStream &stream)
    {
        transform->load(stream);
        stream >> aspectRatio >> windowHeight;
    }
};

BR_REGISTER(Transform, BuildScalesTransform)

/*!
 * \ingroup transforms
 * \brief Sample detection bounding boxes from without resizing
 * \author Josh Klontz \cite jklontz
 */
class Detector : public Transform
{
    Q_OBJECT
    Q_PROPERTY(br::Transform *transform READ get_transform WRITE set_transform RESET reset_transform)
    BR_PROPERTY(br::Transform*, transform, make("Identity"))

    void train(const TemplateList &data)
    {
        const float aspectRatio = getAspectRatio(data);
        TemplateList cropped = cropTrainingSamples(data, aspectRatio);
        qDebug("Detector using: %d training samples.", cropped.size());
        cropped.first().file.set("aspectRatio", aspectRatio);
        transform->train(cropped);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
    }
};

BR_REGISTER(Transform, Detector)

/*!
 * \ingroup transforms
 * \brief Detects objects with OpenCV's built-in HOG detection.
 * \author Austin Blanton \cite imaus10
 */
class HOGDetectTransform : public UntrainableTransform
{
    Q_OBJECT

    HOGDescriptor hog;

    void init()
    {
        hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        std::vector<Rect> objLocs;
        QList<Rect> rects;
        hog.detectMultiScale(src, objLocs);
        foreach (const Rect &obj, objLocs)
            rects.append(obj);
        dst.file.setRects(rects);
    }
};

BR_REGISTER(Transform, HOGDetectTransform)

/*!
 * \ingroup transforms
 * \brief Consolidate redundant/overlapping detections.
 * \author Brendan Klare \cite bklare
 */
class ConsolidateDetectionsTransform : public Transform
{
    Q_OBJECT

public:
    ConsolidateDetectionsTransform() : Transform(false, false) {}
private:

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        if (!dst.file.contains("Confidences"))
            return;

        //Compute overlap between rectangles and create discrete Laplacian matrix
        QList<Rect> rects = OpenCVUtils::toRects(src.file.rects());
        int n = rects.size();
        if (n == 0)
            return;
        MatrixXf laplace(n,n);
        for (int i = 0; i < n; i++) {
            laplace(i,i) = 0;
        }
        for (int i = 0; i < n; i++){
            for (int j = i + 1; j < n; j++) {
                float overlap = (float)((rects[i] & rects[j]).area()) / (float)max(rects[i].area(), rects[j].area());
                if (overlap > 0.5) {
                    laplace(i,j) = -1.0;
                    laplace(j,i) = -1.0;
                    laplace(i,i) = laplace(i,i) + 1.0;
                    laplace(j,j) = laplace(j,j) + 1.0;
                } else {
                    laplace(i,j) = 0;
                    laplace(j,i) = 0;
                }
            }
        }

        // Compute eigendecomposition
        SelfAdjointEigenSolver<Eigen::MatrixXf> eSolver(laplace);
        MatrixXf allEVals = eSolver.eigenvalues();
        MatrixXf allEVecs = eSolver.eigenvectors();

        //Keep eigenvectors with zero eigenvalues
        int nRegions = 0;
        for (int i = 0; i < n; i++) {
            if (fabs(allEVals(i)) < 1e-4) {
                nRegions++;
            }
        }
        MatrixXf regionVecs(n, nRegions);
        for (int i = 0, cnt = 0; i < n; i++) {
            if (fabs(allEVals(i)) < 1e-4)
                regionVecs.col(cnt++) = allEVecs.col(i);
        }

        //Determine membership for each consolidated location
        // and compute average of regions. This is determined by
        // finding which eigenvector has the highest magnitude for
        // each input dimension. Each input dimension corresponds to
        // one of the input rect region. Thus, each eigenvector represents
        // a set of overlaping regions.
        float  * midX = new float[nRegions];
        float * midY = new float[nRegions];
        float * avgWidth = new float[nRegions];
        float *avgHeight = new float[nRegions];
        float *confs = new float[nRegions];
        int *cnts = new int[nRegions];
        int mx;
        int mxIdx;
        for (int i = 0 ; i < nRegions; i++) {
            midX[i] = 0;
            midY[i] = 0;
            avgWidth[i] = 0;
            avgHeight[i] = 0;
            confs[i] = 0;
            cnts[i] = 0;
        }

        QList<float> confidences = dst.file.getList<float>("Confidences");
        for (int i = 0; i < n; i++) {
            mx = 0.0;
            mxIdx = -1;

            for (int j = 0; j < nRegions; j++) {
                if (fabs(regionVecs(i,j)) > mx) {
                    mx = fabs(regionVecs(i,j));
                    mxIdx = j;
                }
            }

            Rect curRect = rects[i];
            midX[mxIdx] += ((float)curRect.x + (float)curRect.width  / 2.0);
            midY[mxIdx] += ((float)curRect.y + (float)curRect.height / 2.0);
            avgWidth[mxIdx]  += (float) curRect.width;
            avgHeight[mxIdx] += (float) curRect.height;
            confs[mxIdx] += confidences[i];
            cnts[mxIdx]++;
        }

        QList<Rect> consolidatedRects;
        QList<float> consolidatedConfidences;
        for (int i = 0; i < nRegions; i++) {
            float cntF = (float) cnts[i];
            if (cntF > 0) {
                int x = qRound((midX[i] / cntF) - (avgWidth[i] / cntF) / 2.0);
                int y = qRound((midY[i] / cntF) - (avgHeight[i] / cntF) / 2.0);
                int w = qRound(avgWidth[i] / cntF);
                int h = qRound(avgHeight[i] / cntF);
                consolidatedRects.append(Rect(x,y,w,h));
                consolidatedConfidences.append(confs[i] / cntF);
            }
        }

        delete [] midX;
        delete [] midY;
        delete [] avgWidth;
        delete [] avgHeight;
        delete [] confs;
        delete [] cnts;

        dst.file.setRects(consolidatedRects);
        dst.file.setList<float>("Confidences", consolidatedConfidences);
    }
};

BR_REGISTER(Transform, ConsolidateDetectionsTransform)

/*!
 * \ingroup transforms
 * \brief For each rectangle bounding box in src, a new
 *      template is created.
 * \author Brendan Klare \cite bklare
 */
class RectsToTemplatesTransform : public UntrainableMetaTransform
{
    Q_OBJECT

private:
    void project(const Template &src, Template &dst) const
    {
        Template tOut(src.file);
        QList<float> confidences = src.file.getList<float>("Confidences");
        QList<QRectF> rects = src.file.rects();
        for (int i = 0; i < rects.size(); i++) {
            Mat m(src, OpenCVUtils::toRect(rects[i]));
            Template t(src.file, m);
            t.file.set("Confidence", confidences[i]);
            t.file.clearRects();
            tOut << t;
        }
        dst = tOut;
    }
};

BR_REGISTER(Transform, RectsToTemplatesTransform)

} // namespace br

#include "slidingwindow.moc"


#ifndef slic3r_Polygon_hpp_
#define slic3r_Polygon_hpp_

#include <myinit.h>
#include <vector>
#include "Line.hpp"
#include "MultiPoint.hpp"
#include "Polyline.hpp"

namespace Slic3r {

class Polygon;
typedef std::vector<Polygon> Polygons;

class Polygon : public MultiPoint {
    public:
    operator Polygons() const;
    operator Polyline() const;
    Point& operator[](Points::size_type idx);
    const Point& operator[](Points::size_type idx) const;
    Point last_point() const;
    Lines lines() const;
    void lines(Lines* lines) const;
    void split_at_vertex(const Point &point, Polyline* polyline) const;
    void split_at_index(int index, Polyline* polyline) const;
    void split_at_first_point(Polyline* polyline) const;
    void equally_spaced_points(double distance, Points* points) const;
    double area() const;
    bool is_counter_clockwise() const;
    bool is_clockwise() const;
    bool make_counter_clockwise();
    bool make_clockwise();
    bool is_valid() const;
    bool contains_point(const Point &point) const;
    Polygons simplify(double tolerance) const;
    void simplify(double tolerance, Polygons &polygons) const;
    void triangulate_convex(Polygons* polygons) const;
    Point centroid() const;

    #ifdef SLIC3RXS
    void from_SV_check(SV* poly_sv);
    #endif
};

}

// start Boost
#include <boost/polygon/polygon.hpp>
namespace boost { namespace polygon {
    template <>
    struct geometry_concept<Polygon>{ typedef polygon_concept type; };

    template <>
    struct polygon_traits<Polygon> {
        typedef coord_t coordinate_type;
        typedef Points::const_iterator iterator_type;
        typedef Point point_type;

        // Get the begin iterator
        static inline iterator_type begin_points(const Polygon& t) {
            return t.points.begin();
        }

        // Get the end iterator
        static inline iterator_type end_points(const Polygon& t) {
            return t.points.end();
        }

        // Get the number of sides of the polygon
        static inline std::size_t size(const Polygon& t) {
            return t.points.size();
        }

        // Get the winding direction of the polygon
        static inline winding_direction winding(const Polygon& t) {
            return unknown_winding;
        }
    };

    template <>
    struct polygon_mutable_traits<Polygon> {
        // expects stl style iterators
        template <typename iT>
        static inline Polygon& set_points(Polygon& polygon, iT input_begin, iT input_end) {
            polygon.points.clear();
            while (input_begin != input_end) {
                polygon.points.push_back(Point());
                boost::polygon::assign(polygon.points.back(), *input_begin);
                ++input_begin;
            }
            // skip last point since Boost will set last point = first point
            polygon.points.pop_back();
            return polygon;
        }
    };

    template <>
    struct geometry_concept<Polygons> { typedef polygon_set_concept type; };

    //next we map to the concept through traits
    template <>
    struct polygon_set_traits<Polygons> {
        typedef coord_t coordinate_type;
        typedef Polygons::const_iterator iterator_type;
        typedef Polygons operator_arg_type;

        static inline iterator_type begin(const Polygons& polygon_set) {
            return polygon_set.begin();
        }

        static inline iterator_type end(const Polygons& polygon_set) {
            return polygon_set.end();
        }

        //don't worry about these, just return false from them
        static inline bool clean(const Polygons& polygon_set) { return false; }
        static inline bool sorted(const Polygons& polygon_set) { return false; }
    };

    template <>
    struct polygon_set_mutable_traits<Polygons> {
        template <typename input_iterator_type>
        static inline void set(Polygons& polygons, input_iterator_type input_begin, input_iterator_type input_end) {
          polygons.assign(input_begin, input_end);
        }
    };
} }
// end Boost

#endif


// File: matrix1.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Matrix>
void fill_and_print(Matrix& A, char name)
{
    // Set values in traditional way
    A= 1.2, 3.4,
       5.6, 7.8;

    // Just print them
    std::cout << name << " is \n" << A << "\n";
}

int main(int, char**)
{
#if defined(MTL_WITH_VARIADIC_TEMPLATE) && defined(MTL_WITH_TEMPLATE_ALIAS)
    using namespace mtl;

    // Create a 2 by 2 dense matrix
    matrix<double>                            A(2, 2);
    fill_and_print(A, 'A');

    // Now a sparse matrix
    matrix<double, sparse>                    B(2, 2);
    fill_and_print(B, 'B');

    // A column-major sparse matrix (CCS by default)
    matrix<double, sparse, column_major>      C(2, 2);
    fill_and_print(C, 'C');

    // A Fortran-like matrix
    matrix<double, col_major>                 D(2, 2);
    fill_and_print(D, 'D');

    // A sparse matrix with a shorter size type
    matrix<float, sparse, as_size_type<int> > E(2, 2);
    fill_and_print(E, 'E');

    // A (dense) matrix with compile-time size
    matrix<double, dim<2, 2> >                F;
    fill_and_print(F, 'F');
#endif
    return 0;
}


#define BOOST_TEST_NO_LIB
#define BOOST_TEST_MAIN

#include <gp/node/node.hpp>
#include <gp/node/array_nodes.hpp>
#include <gp/tree/tree.hpp>
#include <boost/test/unit_test.hpp>

using namespace gp;
using namespace gp::node;

BOOST_AUTO_TEST_SUITE(array_node)
    BOOST_AUTO_TEST_CASE(array_index_access) {
        auto argNode = NodeInterface::createInstance<ArgumentNode<GpArray<int>, 0>>();
        auto atNode = NodeInterface::createInstance<AtNode<GpArray<int>>>();
        auto idxNode = NodeInterface::createInstance<ConstNode<int>>(1);

        atNode->setChild(0, std::move(argNode));
        atNode->setChild(1, std::move(idxNode));

        utility::EvaluationContext evaluationContext1(std::make_tuple(GpArray{1, 2, 3}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans1 = atNode->evaluateByAny(evaluationContext1);
        BOOST_CHECK(evaluationContext1.getEvaluationStatus() == utility::EvaluationStatus::Evaluating);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans1), 2);

        utility::EvaluationContext evaluationContext2(std::make_tuple(GpArray<int>{1}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans2 = atNode->evaluateByAny(evaluationContext2);
        BOOST_CHECK(evaluationContext2.getEvaluationStatus() != utility::EvaluationStatus::Evaluating);
    }
    BOOST_AUTO_TEST_CASE(lvalue_array_index_access) {
        auto argNode = NodeInterface::createInstance<ArgumentNode<utility::LeftHandValue<GpArray<int>>, 0>>();
        auto atNode = NodeInterface::createInstance<AtNode<utility::LeftHandValue<GpArray<int>>>>();
        auto idxNode = NodeInterface::createInstance<ConstNode<int>>(1);

        atNode->setChild(0, std::move(argNode));
        atNode->setChild(1, std::move(idxNode));

        utility::EvaluationContext evaluationContext1(std::make_tuple(GpArray{1, 2, 3}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans1 = atNode->evaluateByAny(evaluationContext1);
        BOOST_CHECK(evaluationContext1.getEvaluationStatus() == utility::EvaluationStatus::Evaluating);
        BOOST_CHECK(std::any_cast<utility::LeftHandValue<int>>(ans1));
        BOOST_CHECK_EQUAL(std::any_cast<utility::LeftHandValue<int>>(ans1).getRef(), 2);

        utility::EvaluationContext evaluationContext2(std::make_tuple(GpArray<int>{1}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans2 = atNode->evaluateByAny(evaluationContext2);
        BOOST_CHECK(evaluationContext2.getEvaluationStatus() != utility::EvaluationStatus::Evaluating);
    }
    BOOST_AUTO_TEST_CASE(array_element_substitution) {
        auto prognNode = NodeInterface::createInstance<PrognNode<int, 2>>();

        auto atNode0 = NodeInterface::createInstance<AtNode<GpArray<int>>>();
        auto idxNode0 = NodeInterface::createInstance<ConstNode<int>>(1);
        auto argNode0 = NodeInterface::createInstance<ArgumentNode<GpArray<int>, 0>>();

        auto substNode = NodeInterface::createInstance<SubstitutionNode<int>>();
        auto argNode = NodeInterface::createInstance<ArgumentNode<utility::LeftHandValue<GpArray<int>>, 0>>();
        auto atNode = NodeInterface::createInstance<AtNode<utility::LeftHandValue<GpArray<int>>>>();
        auto idxNode = NodeInterface::createInstance<ConstNode<int>>(1);
        auto constNode = NodeInterface::createInstance<ConstNode<int>>(-2);

        atNode0->setChild(0, std::move(argNode0));
        atNode0->setChild(1, std::move(idxNode0));

        prognNode->setChild(1, std::move(atNode0));

        atNode->setChild(0, std::move(argNode));
        atNode->setChild(1, std::move(idxNode));

        substNode->setChild(0, std::move(atNode));
        substNode->setChild(1, std::move(constNode));

        prognNode->setChild(0, std::move(substNode));

        utility::EvaluationContext evaluationContext1(std::make_tuple(GpArray{1, 2, 3}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans1 = prognNode->evaluateByAny(evaluationContext1);
        BOOST_CHECK(evaluationContext1.getEvaluationStatus() == utility::EvaluationStatus::Evaluating);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans1), -2);

        utility::EvaluationContext evaluationContext2(std::make_tuple(GpArray<int>{1}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans2 = prognNode->evaluateByAny(evaluationContext2);
        BOOST_CHECK(evaluationContext2.getEvaluationStatus() != utility::EvaluationStatus::Evaluating);
    }
    BOOST_AUTO_TEST_CASE(array_size) {
        auto argNode = NodeInterface::createInstance<ArgumentNode<GpArray<int>, 0>>();
        auto sizeOfArrNode = NodeInterface::createInstance<SizeOfArray<GpArray<int>>>();

        sizeOfArrNode->setChild(0, std::move(argNode));

        utility::EvaluationContext evaluationContext1(std::make_tuple(GpArray{1, 2, 3}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans1 = sizeOfArrNode->evaluateByAny(evaluationContext1);
        BOOST_CHECK(evaluationContext1.getEvaluationStatus() == utility::EvaluationStatus::Evaluating);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans1), 3);

        utility::EvaluationContext evaluationContext2(std::make_tuple(GpArray<int>{}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans2 = sizeOfArrNode->evaluateByAny(evaluationContext2);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans2), 0);
    }
    BOOST_AUTO_TEST_CASE(lvalue_array_size) {
        auto argNode = NodeInterface::createInstance<ArgumentNode<utility::LeftHandValue<GpArray<int>>, 0>>();
        auto sizeOfArrNode = NodeInterface::createInstance<SizeOfArray<utility::LeftHandValue<GpArray<int>>>>();

        sizeOfArrNode->setChild(0, std::move(argNode));

        utility::EvaluationContext evaluationContext1(std::make_tuple(GpArray{1, 2, 3}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans1 = sizeOfArrNode->evaluateByAny(evaluationContext1);
        BOOST_CHECK(evaluationContext1.getEvaluationStatus() == utility::EvaluationStatus::Evaluating);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans1), 3);

        utility::EvaluationContext evaluationContext2(std::make_tuple(GpArray<int>{}),
                                                      utility::EvaluationContext::VariableTable{},
                                                      100, 100);
        auto ans2 = sizeOfArrNode->evaluateByAny(evaluationContext2);
        BOOST_CHECK_EQUAL(std::any_cast<int>(ans2), 0);
    }
BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/U.B.P.
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ. Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SQR_ABS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_arithmetic
 * \defgroup boost_simd_arithmetic_sqr_abs sqr_abs
 *
 * \par Description
 * The function always returns a value of the same type than the entry.
 * \par
 * This is the saturated square modulus of the input.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/sqr_abs.hpp>
 * \endcode
 *
 * \par Alias
 * \arg sqr_modulus
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::sqr_abs_(A0)>::type
 *     sqr_abs(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of sqr_abs
 *
 * \return a value of the same type as the parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag sqr_abs_ of functor sqr_abs
     *        in namespace boost::simd::tag for toolbox boost.simd.arithmetic
    **/
    struct sqr_abs_ : ext::elementwise_<sqr_abs_> { typedef ext::elementwise_<sqr_abs_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::sqr_abs_, sqr_abs, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::sqr_abs_, sqr_modulus, 1)
} }

#endif

// modified by jt the 25/12/2010


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_INT_SPLAT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_INT_SPLAT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/meta/int_c.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Int_splat generic tag

     Represents the Int_splat constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    template<boost::simd::int64_t I>
    struct Intpattern : ext::pure_constant_< Intpattern<I> >
    {
      typedef ext::pure_constant_< Intpattern<I> > parent;
      template<class Target, class Dummy=void>
      /// INTERNAL ONLY
      struct apply : meta::int_c< typename Target::type
                                , typename Target::type(I)
                                > {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_HEADER( dispatching( ext::adl_helper(), *(Intpattern*)0, static_cast<Args&&>(args)... ) )
      {
        return dispatching( ext::adl_helper(), Intpattern(), static_cast<Args&&>(args)... );
      }
    };
  }

  template<class Target, typename boost::dispatch::meta::scalar_of<Target>::type N>
  inline Target integral_constant()
  {
    typedef tag::Intpattern<boost::simd::int64_t(N)> tag_t;
    typename dispatch::make_functor< tag_t, Target >::type callee;
    return callee( dispatch::meta::as_<Target>() );
  }
} }

#include <boost/simd/constant/common.hpp>

#endif


#include "truncated-svd-solver/linear-algebra-helpers.h"

#include <algorithm>
#include <cmath>

#include <cholmod.h>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <spqr.hpp>
#include <SuiteSparseQR.hpp>

namespace truncated_svd_solver {

cholmod_sparse* columnSubmatrix(cholmod_sparse* A, std::ptrdiff_t
    col_start_idx, std::ptrdiff_t col_end_idx, cholmod_common* cholmod) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(cholmod);

  CHECK_GE(col_end_idx, col_start_idx);
  CHECK_LT(col_end_idx, static_cast<std::ptrdiff_t>(A->ncol));
  CHECK_GE(col_start_idx, 0);

  const std::ptrdiff_t num_indices = col_end_idx - col_start_idx + 1;
  std::ptrdiff_t* col_indices = new std::ptrdiff_t[num_indices];
  for (std::ptrdiff_t j = col_start_idx; j <= col_end_idx; ++j) {
    col_indices[j - col_start_idx] = j;
  }

  cholmod_sparse* A_sub = cholmod_l_submatrix(A, nullptr, -1, col_indices,
    num_indices, 1, 1, cholmod);
  delete [] col_indices;
  CHECK(A_sub != nullptr) << "cholmod_l_submatrix failed.";

  return A_sub;
}

cholmod_sparse* rowSubmatrix(cholmod_sparse* A, std::ptrdiff_t row_start_idx,
    std::ptrdiff_t row_end_idx, cholmod_common* cholmod) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(cholmod);

  CHECK_GE(row_end_idx, row_start_idx);
  CHECK_LT(row_end_idx, static_cast<std::ptrdiff_t>(A->nrow));
  CHECK_GE(row_start_idx, 0);

  const std::ptrdiff_t num_indices = row_end_idx - row_start_idx + 1;
  std::ptrdiff_t* row_indices = new std::ptrdiff_t[num_indices];
  for (std::ptrdiff_t i = row_start_idx; i <= row_end_idx; ++i){
    row_indices[i - row_start_idx] = i;
  }

  cholmod_sparse* A_sub = cholmod_l_submatrix(A, row_indices, num_indices,
    nullptr, -1, 1, 1, cholmod);
  delete [] row_indices;
  CHECK(A_sub != nullptr) << "cholmod_l_submatrix failed.";

  return A_sub;
}

double colNorm(const cholmod_sparse* A, std::ptrdiff_t col_idx) {
  CHECK_NOTNULL(A);
  CHECK_GE(col_idx, 0);
  CHECK_LT(col_idx, static_cast<std::ptrdiff_t>(A->ncol));

  const std::ptrdiff_t* col_ptr =
    reinterpret_cast<const std::ptrdiff_t*>(A->p);
  const double* values = reinterpret_cast<const double*>(A->x);
  const std::ptrdiff_t p = col_ptr[col_idx];
  const std::ptrdiff_t num_elements = col_ptr[col_idx + 1] - p;

  double norm = 0.0;
  for (std::ptrdiff_t i = 0; i < num_elements; ++i) {
    norm += values[p + i] * values[p + i];
  }
  return std::sqrt(norm);
}

cholmod_dense* columnScalingMatrix(cholmod_sparse* A, cholmod_common*
    cholmod, double eps) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(cholmod);
  CHECK_GT(eps, 0.0);

  cholmod_dense* G = cholmod_l_allocate_dense(A->ncol, 1, A->ncol,
    CHOLMOD_REAL, cholmod);
  CHECK(G != nullptr) << "cholmod_l_allocate_dense failed.";

  const double norm_tolerance = std::sqrt(A->nrow * eps);
  double* values = reinterpret_cast<double*>(G->x);
  for (std::ptrdiff_t col_idx = 0;
      col_idx < static_cast<std::ptrdiff_t>(A->ncol);
      ++col_idx) {
    const double norm = colNorm(A, col_idx);
    if (norm < norm_tolerance) {
      values[col_idx] = 0.0;
    } else {
      values[col_idx] = 1.0 / norm;
    }
  }
  return G;
}

void cholmodSparseToEigenDenseCopy(const cholmod_sparse* in,
    Eigen::MatrixXd& out) {
  CHECK_NOTNULL(in);
  out.setZero(in->nrow, in->ncol);

  const std::ptrdiff_t* row_ind =
    reinterpret_cast<const std::ptrdiff_t*>(in->i);
  const std::ptrdiff_t* col_ptr =
    reinterpret_cast<const std::ptrdiff_t*>(in->p);
  const double* values = reinterpret_cast<const double*>(in->x);
  for (std::ptrdiff_t col_idx = 0;
       col_idx < static_cast<std::ptrdiff_t>(in->ncol); ++col_idx)
    for (std::ptrdiff_t val_idx = col_ptr[col_idx];
         val_idx < col_ptr[col_idx + 1]; ++val_idx) {
      out(row_ind[val_idx], col_idx) = values[val_idx];
      if (in->stype && col_idx != row_ind[val_idx]) {
        out(col_idx, row_ind[val_idx]) = values[val_idx];
      }
    }
}

void eigenDenseToCholmodDenseView(const Eigen::VectorXd& in, cholmod_dense*
    out) {
  CHECK_NOTNULL(out);

  out->nrow = in.size();
  out->ncol = 1;
  out->nzmax = in.size();
  out->d = in.size();
  out->x = reinterpret_cast<void*>(const_cast<double*>(in.data()));
  out->z = nullptr;
  out->xtype = CHOLMOD_REAL;
  out->dtype = CHOLMOD_DOUBLE;
}

cholmod_dense* eigenDenseToCholmodDenseCopy(const Eigen::VectorXd& in,
    cholmod_common* cholmod) {
  CHECK_NOTNULL(cholmod);
  cholmod_dense* out = cholmod_l_allocate_dense(in.size(), 1, in.size(),
    CHOLMOD_REAL, cholmod);
  CHECK(out != nullptr) << "cholmod_l_allocate_dense failed.";

  double* out_val = reinterpret_cast<double*>(out->x);
  const double* in_val = in.data();
  std::copy(in_val, in_val + in.size(), out_val);
  return out;
}

void cholmodDenseToEigenDenseCopy(const cholmod_dense* in, Eigen::VectorXd&
    out) {
  CHECK_NOTNULL(in);
  out.resize(in->nrow);
  const double* in_val = reinterpret_cast<const double*>(in->x);
  std::copy(in_val, in_val + in->nrow, out.data());
}

cholmod_sparse* eigenDenseToCholmodSparseCopy(const Eigen::MatrixXd& A,
    cholmod_common* cholmod, double eps) {
  CHECK_NOTNULL(cholmod);
  CHECK_GT(eps, 0.0);

  size_t nzmax = 0;
  for (std::ptrdiff_t i = 0; i < A.rows(); ++i) {
    for (std::ptrdiff_t j = 0; j < A.cols(); ++j) {
      if (std::fabs(A(i, j)) > eps) {
        nzmax++;
      }
    }
  }

  cholmod_sparse* A_cholmod = cholmod_l_allocate_sparse(A.rows(), A.cols(),
    nzmax, 1, 1, 0, CHOLMOD_REAL, cholmod);
  CHECK(A_cholmod != nullptr) << "cholmod_l_allocate_sparse failed.";

  std::ptrdiff_t* row_ind = reinterpret_cast<std::ptrdiff_t*>(A_cholmod->i);
  std::ptrdiff_t* col_ptr = reinterpret_cast<std::ptrdiff_t*>(A_cholmod->p);
  double* values = reinterpret_cast<double*>(A_cholmod->x);
  std::ptrdiff_t row_it = 0;
  std::ptrdiff_t col_it = 1;
  for (std::ptrdiff_t c = 0; c < A.cols(); ++c) {
    for (std::ptrdiff_t r = 0; r < A.rows(); ++r)
      if (std::fabs(A(r, c)) > eps) {
        values[row_it] = A(r, c);
        row_ind[row_it] = r;
        row_it++;
      }
    col_ptr[col_it] = row_it;
    col_it++;
  }
  return A_cholmod;
}

std::ptrdiff_t estimateNumericalRank(const Eigen::VectorXd& singular_values,
                                     double tolerance) {
  CHECK_GE(tolerance, 0.0);

  std::ptrdiff_t numerical_rank = singular_values.size();
  for (std::ptrdiff_t i = singular_values.size() - 1; i >= 0; --i) {
    if (singular_values(i) > tolerance) {
      // Assumes that the singular values are ordered.
      break;
    } else {
      --numerical_rank;
    }
  }
  return numerical_rank;
}

double rankTol(const Eigen::VectorXd& singular_values, double eps) {
  CHECK_GT(singular_values.rows(), 0) << "Empty singular values vector.";
  CHECK_GT(eps, 0.0);
  return singular_values(0) * eps * singular_values.size();
}

double qrTol(cholmod_sparse* A, cholmod_common* cholmod, double eps) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(cholmod);
  CHECK_GT(eps, 0.0);
  return 20.0 * static_cast<double>(A->nrow + A->ncol) * eps *
    spqr_maxcolnorm<double>(A, cholmod);
}

double svGap(const Eigen::VectorXd& singular_values, std::ptrdiff_t rank) {
  CHECK_LE(rank, singular_values.rows());
  CHECK_GE(rank, 0);

  if (rank == 0) {
    return 0.0;
  } else if (rank < singular_values.size()) {
    return singular_values(rank - 1) / singular_values(rank);
  }
  return std::numeric_limits<double>::infinity();
}

void reduceLeftHandSide(SuiteSparseQR_factorization<double>* factor,
    cholmod_sparse* A_rt, cholmod_sparse** Omega, cholmod_sparse** A_rtQ,
    cholmod_common* cholmod) {
  CHECK_NOTNULL(A_rt);
  CHECK_NOTNULL(cholmod);
  CHECK_NOTNULL(Omega);

  // Handle the special case where the QR part is empty.
  if (factor == nullptr) {
    // NO QR part!
    *Omega = cholmod_l_aat(A_rt, nullptr, 0, 1, cholmod);
    return;
  } else {
    CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
    CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
  }
  CHECK_NOTNULL(factor);
  CHECK_NOTNULL(A_rtQ);

  cholmod_sparse* A_rtQFull = SuiteSparseQR_qmult<double>(SPQR_XQ, factor,
    A_rt, cholmod);
  CHECK(A_rtQFull != nullptr) << "SuiteSparseQR_qmult failed.";

  *A_rtQ = columnSubmatrix(A_rtQFull, 0, factor->QRsym->n - 1, cholmod);
  cholmod_l_free_sparse(&A_rtQFull, cholmod);

  cholmod_sparse* A_rtQ2 = cholmod_l_aat(*A_rtQ, nullptr, 0, 1, cholmod);
  CHECK(A_rtQ2 != nullptr) << "cholmod_l_aat failed.";
  A_rtQ2->stype = 1;

  cholmod_sparse* A_rt2 = cholmod_l_aat(A_rt, nullptr, 0, 1, cholmod);
  CHECK(A_rt2 != nullptr) << "cholmod_l_aat failed.";
  A_rt2->stype = 1;

  double alpha[2];
  alpha[0] = 1.0;
  double beta[2];
  beta[0] = -1.0;

  *Omega = cholmod_l_add(A_rt2, A_rtQ2, alpha, beta, 1, 1, cholmod);
  CHECK(*Omega != nullptr) << "cholmod_l_add failed.";

  cholmod_l_free_sparse(&A_rt2, cholmod);
  cholmod_l_free_sparse(&A_rtQ2, cholmod);
}

cholmod_dense* reduceRightHandSide(SuiteSparseQR_factorization<double>*
    factor, cholmod_sparse* A_rt, cholmod_sparse* A_rtQ, cholmod_dense* b,
    cholmod_common* cholmod) {
  CHECK_NOTNULL(A_rt);
  CHECK_NOTNULL(b);
  CHECK_NOTNULL(cholmod);

  // Handle the special case where the QR part is empty.
  if (factor == nullptr) {
    // NO QR part!
    double one = 1, zero = 0;
    cholmod_dense* b_reduced = cholmod_l_allocate_dense(
        A_rt->nrow, 1, A_rt->nrow, CHOLMOD_REAL, cholmod);
    CHECK(b_reduced != nullptr) << "cholmod_l_allocate_dense failed.";

    const int status = cholmod_l_sdmult(A_rt, 0, &one, &zero, b, b_reduced,
                                        cholmod);
    CHECK(status) << "cholmod_l_sdmult failed";
    return b_reduced;
  } else {
    CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
    CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
  }
  CHECK_NOTNULL(factor);
  CHECK_NOTNULL(A_rtQ);

  cholmod_sparse* b_sparse = cholmod_l_dense_to_sparse(b, 1, cholmod);
  CHECK(b_sparse != nullptr) << "cholmod_l_dense_to_sparse failed.";

  cholmod_sparse* A_rtb = cholmod_l_ssmult(A_rt, b_sparse, 0, 1, 1, cholmod);
  CHECK(A_rtb != nullptr) << "cholmod_l_ssmult failed.";

  cholmod_sparse* QtbFull = SuiteSparseQR_qmult<double>(SPQR_QTX, factor,
    b_sparse, cholmod);
  CHECK(QtbFull != nullptr) << "SuiteSparseQR_qmult failed.";
  cholmod_l_free_sparse(&b_sparse, cholmod);

  cholmod_sparse* Qtb;
  Qtb = rowSubmatrix(QtbFull, 0, factor->QRsym->n - 1, cholmod);
  cholmod_l_free_sparse(&QtbFull, cholmod);

  cholmod_sparse* A_rtQQtb = cholmod_l_ssmult(A_rtQ, Qtb, 0, 1, 1, cholmod);
  CHECK(A_rtQQtb != nullptr) << "cholmod_l_ssmult failed.";
  cholmod_l_free_sparse(&Qtb, cholmod);

  double alpha[2];
  alpha[0] = 1.0;
  double beta[2];
  beta[0] = -1.0;

  cholmod_sparse* b_reduced_sparse = cholmod_l_add(A_rtb, A_rtQQtb, alpha,
    beta, 1, 1, cholmod);
  CHECK(b_reduced_sparse != nullptr) << "cholmod_l_add failed.";
  cholmod_l_free_sparse(&A_rtb, cholmod);
  cholmod_l_free_sparse(&A_rtQQtb, cholmod);

  cholmod_dense* b_reduced = cholmod_l_sparse_to_dense(b_reduced_sparse,
    cholmod);
  CHECK(b_reduced != nullptr) << "cholmod_l_sparse_to_dense failed.";
  cholmod_l_free_sparse(&b_reduced_sparse, cholmod);
  return b_reduced;
}

void analyzeSVD(const cholmod_sparse* Omega, Eigen::VectorXd& sv,
    Eigen::MatrixXd& U, Eigen::MatrixXd& V) {
  CHECK_NOTNULL(Omega);

  Eigen::MatrixXd OmegaDense;
  cholmodSparseToEigenDenseCopy(Omega, OmegaDense);
  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(OmegaDense,
    Eigen::ComputeThinU | Eigen::ComputeThinV);
  U = svd.matrixU();
  V = svd.matrixV();
  sv = svd.singularValues();
}

void solveSVD(const cholmod_dense* b, const Eigen::VectorXd& sv, const
    Eigen::MatrixXd& U, const Eigen::MatrixXd& V, std::ptrdiff_t rank,
    Eigen::VectorXd& x) {
  CHECK_NOTNULL(b);
  CHECK_LE(rank, V.cols());
  CHECK_EQ(V.cols(), sv.rows());
  CHECK_EQ(U.rows(), static_cast<std::ptrdiff_t>(b->nrow));

  Eigen::Map<const Eigen::VectorXd> b_eigen(
    reinterpret_cast<const double*>(b->x), b->nrow);
  x = V.leftCols(rank) * sv.head(rank).asDiagonal().inverse() *
    U.leftCols(rank).adjoint() * b_eigen;
}

cholmod_dense* solveQR(SuiteSparseQR_factorization<double>* factor,
    cholmod_dense* b, cholmod_sparse* A_r, const Eigen::VectorXd& x_r,
    cholmod_common* cholmod) {
  CHECK_NOTNULL(factor);
  CHECK(factor->QRsym != nullptr) << "Run symbolic factorization first.";
  CHECK(factor->QRnum != nullptr) << "Run QR decomposition first.";
  CHECK_NOTNULL(b);
  CHECK_NOTNULL(cholmod);

  cholmod_dense* QtbmA_rx_r = nullptr;
  if (A_r != nullptr) {
    CHECK_EQ(x_r.size(), static_cast<int>(A_r->ncol))
      << "Dimension mismatch between x_r and A_r.";

    cholmod_dense x_r_cholmod;
    eigenDenseToCholmodDenseView(x_r, &x_r_cholmod);
    cholmod_dense* A_rx_r = cholmod_l_allocate_dense(A_r->nrow, 1, A_r->nrow,
      CHOLMOD_REAL, cholmod);
    CHECK(A_rx_r != nullptr) << "cholmod_l_allocate_dense failed.";

    double alpha[2];
    alpha[0] = 1.0;
    double beta[2];
    beta[0] = 0.0;
    const bool success = cholmod_l_sdmult(A_r, 0, alpha, beta, &x_r_cholmod,
                                          A_rx_r, cholmod);
    CHECK(success) << "cholmod_l_sdmult failed.";

    Eigen::Map<const Eigen::VectorXd> bEigen(
      reinterpret_cast<const double*>(b->x), b->nrow);
    Eigen::Map<const Eigen::VectorXd> A_rx_rEigen(
      reinterpret_cast<const double*>(A_rx_r->x), A_rx_r->nrow);
    const Eigen::VectorXd bmA_rx_rEigen = bEigen - A_rx_rEigen;
    cholmod_l_free_dense(&A_rx_r, cholmod);
    cholmod_dense bmA_rx_r;
    eigenDenseToCholmodDenseView(bmA_rx_rEigen, &bmA_rx_r);
    QtbmA_rx_r = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, &bmA_rx_r,
                                             cholmod);
  } else {
    QtbmA_rx_r = SuiteSparseQR_qmult<double>(SPQR_QTX, factor, b, cholmod);
  }
  CHECK(QtbmA_rx_r != nullptr) << "SuiteSparseQR_qmult failed.";

  cholmod_dense* x_l = SuiteSparseQR_solve<double>(SPQR_RETX_EQUALS_B,
    factor, QtbmA_rx_r, cholmod);
  CHECK(x_l != nullptr) << "SuiteSparseQR_solve failed.";
  cholmod_l_free_dense(&QtbmA_rx_r, cholmod);
  return x_l;
}

}  // namespace truncated_svd_solver


/*!
@file
Defines `boost::hana::Integral`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_INTEGRAL_HPP
#define BOOST_HANA_INTEGRAL_HPP

#include <boost/hana/comparable/equal_mcd.hpp>
#include <boost/hana/constant/mcd.hpp>
#include <boost/hana/detail/integral_fwd.hpp>
#include <boost/hana/detail/std/size_t.hpp>
#include <boost/hana/detail/std/type_traits.hpp>
#include <boost/hana/group/minus_mcd.hpp>
#include <boost/hana/logical/mcd.hpp>
#include <boost/hana/monoid/mcd.hpp>
#include <boost/hana/orderable/less_mcd.hpp>

namespace boost { namespace hana {
    //! @details
    //! `Integral`s can be compared with objects of any arithmetic type; the
    //! comparison is done by comparing their underlying integral value.
    template <typename T>
    struct Comparable::instance<Integral, T, when<detail::std::is_arithmetic<T>{}>>
        : Comparable::equal_mcd
    {
        template <typename X, typename Y>
        static constexpr auto equal_impl(X x, Y y)
        { return x() == y; }
    };

    template <typename T>
    struct Comparable::instance<T, Integral, when<detail::std::is_arithmetic<T>{}>>
        : Comparable::equal_mcd
    {
        template <typename X, typename Y>
        static constexpr auto equal_impl(X x, Y y)
        { return x == y(); }
    };

    //! @details
    //! An `Integral` is less than another if and only if its underlying
    //! integral value is less than the other's.
    //!
    //! @snippet example/integral/orderable.cpp main
    template <>
    struct Orderable::instance<Integral, Integral> : Orderable::less_mcd {
        template <typename T, typename U>
        static constexpr auto less_impl(T t, U u)
        { return bool_<(t() < u())>; }
    };

    //! @details
    //! `Integral`s can be ordered with objects of any arithmetic type; the
    //! ordering is done by ordering their underlying integral value.
    template <typename T>
    struct Orderable::instance<Integral, T, when<detail::std::is_arithmetic<T>{}>>
        : Orderable::less_mcd
    {
        template <typename X, typename Y>
        static constexpr auto less_impl(X x, Y y)
        { return x() < y; }
    };

    template <typename T>
    struct Orderable::instance<T, Integral, when<detail::std::is_arithmetic<T>{}>>
        : Orderable::less_mcd
    {
        template <typename X, typename Y>
        static constexpr auto less_impl(X x, Y y)
        { return x < y(); }
    };

    template <>
    struct Constant::instance<Integral> : Constant::mcd {
        template <typename C>
        static constexpr auto value_impl(C const& c)
        { return c(); }
    };

    //! @details
    //! An `Integral` is true-valued if and only if its integral value is
    //! true-valued, i.e. if
    //! @code
    //!     static_cast<bool>(n()) == true
    //! @endcode
    template <>
    struct Logical::instance<Integral> : Logical::mcd {
        template <typename Cond, typename Then, typename Else>
        static constexpr auto eval_if_impl(Cond c, Then t, Else e)
        { return eval_if_impl(bool_<static_cast<bool>(c())>, t, e); }

        template <typename Then, typename Else>
        static constexpr auto eval_if_impl(decltype(true_), Then t, Else)
        { return t([](auto x) { return x; }); }

        template <typename Then, typename Else>
        static constexpr auto eval_if_impl(decltype(false_), Then, Else e)
        { return e([](auto x) { return x; }); }

        template <typename Cond>
        static constexpr auto not_impl(Cond c)
        { return bool_<!c()>; }
    };

    //! Additive `Monoid` of `Integral`s.
    template <>
    struct Monoid::instance<Integral, Integral> : Monoid::mcd {
        template <typename X, typename Y>
        static constexpr auto plus_impl(X x, Y y)
        { return integral<decltype(value(x) + value(y)), value(x) + value(y)>; }

        static constexpr auto zero_impl()
        { return int_<0>; }
    };

    template <typename T>
    struct Monoid::instance<Integral, T, when<detail::std::is_arithmetic<T>{}>>
        : Monoid::mcd
    {
        template <typename X, typename Y>
        static constexpr auto plus_impl(X x, Y y)
        { return value(x) + y; }
    };

    template <typename T>
    struct Monoid::instance<T, Integral, when<detail::std::is_arithmetic<T>{}>>
        : Monoid::mcd
    {
        template <typename X, typename Y>
        static constexpr auto plus_impl(X x, Y y)
        { return x + value(y); }
    };

    //! Additive `Group` of `Integral`s.
    template <>
    struct Group::instance<Integral, Integral>
        : Group::minus_mcd<Integral, Integral>
    {
        template <typename X, typename Y>
        static constexpr auto minus_impl(X x, Y y)
        { return integral<decltype(value(x) - value(y)), value(x) - value(y)>; }
    };

    template <typename T>
    struct Group::instance<Integral, T, when<detail::std::is_arithmetic<T>{}>>
        : Group::minus_mcd<Integral, T>
    {
        template <typename X, typename Y>
        static constexpr auto minus_impl(X x, Y y)
        { return value(x) - y; }
    };

    template <typename T>
    struct Group::instance<T, Integral, when<detail::std::is_arithmetic<T>{}>>
        : Group::minus_mcd<T, Integral>
    {
        template <typename X, typename Y>
        static constexpr auto minus_impl(X x, Y y)
        { return x - value(y); }
    };

    namespace integral_detail {
        constexpr int to_int(char c)
        { return static_cast<int>(c) - 48; }

        template <detail::std::size_t N>
        constexpr long long parse(const char (&arr)[N]) {
            long long number = 0, base = 1;
            for (decltype(N) i = 0; i < N; ++i) {
                number += to_int(arr[N - 1 - i]) * base;
                base *= 10;
            }
            return number;
        }
    }

    namespace literals {
        //! Creates an `Integral` from a literal.
        //! @relates boost::hana::Integral
        //!
        //! The literal is parsed at compile-time and the result is returned
        //! as an `llong<...>`.
        //!
        //! @note
        //! We use `llong<...>` instead of `ullong<...>` because using an
        //! unsigned type leads to unexpected behavior when doing stuff like
        //! `-1_c`. If we used an unsigned type, `-1_c` would be something
        //! like `ullong<-1>` which is actually `ullong<something huge>`.
        //!
        //! ### Example
        //! @snippet example/integral/literals.cpp main
        //!
        //! @todo Add support for stuff like `0x1234_c`.
        template <char ...c>
        constexpr auto operator"" _c()
        { return llong<integral_detail::parse<sizeof...(c)>({c...})>; }
    }
}} // end namespace boost::hana

#endif // !BOOST_HANA_INTEGRAL_HPP


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>
#include <iostream> //needed in order to use cout
#include <mutex>
#include <unistd.h>
#include <ctime>

using namespace std;

namespace gazebo
{
    struct Utils{
        //Utility Functions
        double normalize(double angle){
            math::Angle dAngle(angle);
            dAngle.Normalize();
            return dAngle.Radian();
        }
        double dxy(math::Vector3 A, math::Vector3 B) {
            A.z = 0;
            B.z = 0;
            return A.Distance(B);
        }
    };

    struct RobotState {
        Utils utils;
        double baseProb;
        double turnAmount;
        double desiredHeading;
        bool crashed;
        double xCrash;
        double yCrash;
        std::string action;//stop,forward,turn-right,turn-left,?reverse
        std::string state;//search,acquire,obstacle-avoidance,go-home

        gazebo::physics::ModelPtr nearestLitter;
        int seenLitter;
        int allLitterPicked;
        int maxCapacity;
        int currLitterPicked;
        double litterSensingRange;
        double fieldOfView;

        RobotState() {
            //default constructor
        }
        RobotState(double baseProb, double yaw, int maxCapacity,
                    double litterSensingRange, double fieldOfView) {
            this->baseProb = baseProb;
            this->desiredHeading = yaw;
            this->maxCapacity = maxCapacity;
            this->litterSensingRange = litterSensingRange;
            this->fieldOfView = fieldOfView;

            this->turnAmount = 0;
            this->crashed = false;
            this->xCrash = 0;
            this->yCrash = 0;
            this->action = "stop";
            this->state = "search";

            this->nearestLitter = nullptr;
            this->seenLitter = 0;
            this->allLitterPicked = 0;
            this->currLitterPicked = 0;

        }
        std::string state2string() {
            std::stringstream robotData;
            robotData <<"baseProb: "<<this->baseProb<<", "
                    <<"turnAmount: "<<this->turnAmount<<", "
                    <<"desiredHeading: "<<this->desiredHeading<<", "
                    <<"crashed: "<<this->crashed<<", "
                    <<"xCrash: "<<this->xCrash<<", "
                    <<"yCrash: "<<this->yCrash<<", "
                    <<"action: "<<this->action<<", "
                    <<"state: "<<this->state<<", "
                    <<"seenLitter: "<<this->seenLitter<<", "
                    <<"allLitterPicked: "<<this->allLitterPicked<<", "
                    <<"maxCapacity: "<<this->maxCapacity<<", "
                    <<"currLitterPicked: "<<this->currLitterPicked<<", "
                    <<"litterSensingRange: "<<this->litterSensingRange<<", "
                    <<"fieldOfView: "<<this->fieldOfView//<<", "
                    ;
            return robotData.str();
        }

    };
    struct RW {
        Utils utils;
        std::default_random_engine generator;
        std::uniform_real_distribution<double> uformRand;
        std::normal_distribution<double> normalRandHeading;
        RW(std::default_random_engine generator,
            std::uniform_real_distribution<double> uformRand,
            std::normal_distribution<double> normalRandHeading) {

                this->generator = generator;
                this->uformRand = uformRand;
                this->normalRandHeading = normalRandHeading;
        }
        RW() {
            //default constructor
        }

        void updateHeading (RobotState* currState){

            if(this->uformRand(this->generator) < currState->baseProb)
            {//randomly change desired direction
                double tempTurnAmount = this->normalRandHeading(this->generator);
                if(tempTurnAmount > 2 * M_PI) tempTurnAmount = 2 * M_PI;

                if(tempTurnAmount < 0.0) tempTurnAmount = 0.0;

                tempTurnAmount = (this->utils).normalize(tempTurnAmount);
                currState->desiredHeading = currState->desiredHeading + tempTurnAmount;
            }

        }

    };

    // struct CPFA {
    //     /* This class implements the CPFA algorithm */
    // };
	class MP_CPFA : public ModelPlugin
	{
		private:
            std::mutex mutex;
            physics::ModelPtr model;
            physics::JointPtr rwheel;
            physics::JointPtr lwheel;

            double rVel;
            double Kp;
            double wheel_separation;
            RobotState myState;
            RW randomWalk;
            Utils utils;

            transport::NodePtr node;

            transport::SubscriberPtr contactSub;
            transport::PublisherPtr pubRobotData;
            // when robot has to go in a particular direction, it needs to make a detour to avoid obstacle

            //WORKING WITH RANDOM NUMBERS
            std::default_random_engine generator;
            std::uniform_real_distribution<double> uformRand;
            double uformMin, uformMax;
            std::normal_distribution<double> normalRandHeading;
            double normalStddev, normalMean;

            event::ConnectionPtr updateConnection;

        public : void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
        {
            this->model = _parent;

            this->rwheel = this->model->GetJoint("R_joint");
            this->lwheel = this->model->GetJoint("L_joint");
            this->wheel_separation = this->rwheel->GetAnchor(0).Distance(
                                        this->lwheel->GetAnchor(0));

            this->node = transport::NodePtr(new transport::Node());
            this->node->Init();

            std:: string contactMsg = "/gazebo/default/" + this->model->GetName() +
                "/chassis/chassis_contact/contacts";
            this->contactSub = this->node->Subscribe(contactMsg, &MP_CPFA::ContactCB,this);
            this->pubRobotData = this->node->Advertise<msgs::Any>(this->model->GetName() + "/data");

            //Initialize Parameters
            this->initParams();

            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                boost::bind(&MP_CPFA::OnUpdate, this, _1)
            );
        }
        public: void ContactCB(ConstContactsPtr &c)
        {
            std::lock_guard<std::mutex> lock(this->mutex);
            math::Quaternion myRot = (this->model->GetWorldPose()).rot;
            double headingError = (this->myState).desiredHeading - myRot.GetYaw();
            headingError = (this->utils).normalize(headingError);

            if( !((this->myState).crashed) && abs(headingError) < 0.09)
            {
                msgs::Contacts contacts = *c;
                for(int i = 0; i < (int) contacts.contact_size(); i++)
                {
                    std::string col1 = contacts.contact(i).collision1();
                    std::string col2 = contacts.contact(i).collision2();
                    std::string collision = col1 + " " + col2;

                    bool _me = (collision.find(this->model->GetName()) != std::string::npos);
                    bool _obstacle = collision.find("chassis::collision") != std::string::npos;
                    bool _litter = collision.find("litter") != std::string::npos;

                    if(_me && _obstacle && not _litter)
                    {
                        for (int j = 0; j < (int) contacts.contact(i).position_size(); j++)
                        {
                            if (contacts.contact(i).position(j).z() > 0.004)
                            {
                                (this->myState).xCrash = contacts.contact(i).position(j).x();
                                (this->myState).yCrash = contacts.contact(i).position(j).y();
                                (this->myState).turnAmount = this->avoidObstacle((this->myState).xCrash,
                                    (this->myState).yCrash, this->model->GetWorldPose());
                                (this->myState).crashed = true;
                                return;
                            }
                        }
                    }
                }

            }
        }
        public: double avoidObstacle(double x, double y, math::Pose myPos)
        {
            double turnRequired = 0;
            double yaw = myPos.rot.GetYaw();
            double xc = myPos.pos.x;
            double yc = myPos.pos.y;
            double obstacleLoc = atan2(y - yc, x - xc);
            double dtheta = yaw - obstacleLoc;
            dtheta = (this->utils).normalize(dtheta);

            if(abs(dtheta) < M_PI / 2.0)
            {
                if(dtheta < - M_PI / 6.0)
                {
                    turnRequired = - M_PI / 4.0;
                }
                else if(dtheta > M_PI / 6.0)
                {
                    turnRequired = M_PI / 4.0;
                }
                else
                {
                    double x = this->uformRand(this->generator) * M_PI + M_PI / 2.0;
                    turnRequired = (this->utils).normalize(x);
                }

            }
            return turnRequired;
        }

        public: void litterSensor(RobotState* myState,
                                gazebo::physics::ModelPtr myModel,
                                gazebo::physics::WorldPtr myWorld) {
            //sense presence of litter and update robotState
            gazebo::physics::Model_V models = myWorld->GetModels();

            gazebo::math::Pose my_pose = myModel->GetWorldPose();
            gazebo::math::Vector3 my_pos = my_pose.pos;
            double my_yaw = my_pose.rot.GetYaw();
            int seenLitter = 0;

            for (auto m : models) {//search through models in world
                std::string m_name = m->GetName();
                if(m_name.find("litter") != std::string::npos) {
                    gazebo::math::Vector3 m_pos = m->GetWorldPose().pos;
                    double dist = (this->utils).dxy(my_pos,m_pos);//linear distance
                    double lit_or = (this->utils).normalize(atan2(my_pos.y - m_pos.y, my_pos.x - m_pos.x))
                                    - my_yaw; //orientation of litter wrt robot
                    if((dist <= myState->litterSensingRange) and //linear distance within sensing range
                        (lit_or >= -myState->fieldOfView/2 and lit_or <= myState->fieldOfView/2)){//and within field of view
                        //litter within sensing range
                        seenLitter += 1;//increment litter
                    }
                }
            }
            myState->seenLitter = seenLitter;//update litter count

        }
        public : void initParams()
        {
            this->rVel = 10.0;
            // (this->myState).baseProb = 0.0025;
            // (this->myState).turnAmount = 0;

            math::Quaternion myRot = (this->model->GetWorldPose()).rot;
            // (this->myState).desiredHeading = myRot.GetYaw();

            // (this->myState).crashed = false;

            this->Kp = 10 * this->rVel;

            // (this->myState).xCrash;
            // (this->myState).yCrash;

            // the seed for the random number generation
            std::string rName = this->model->GetName();
            int robNum = stoi(rName.substr(9));
            std::time_t currTime = std::time(nullptr);
            robNum = (int)  currTime / robNum;
            // int xPos = (int) abs((this->model->GetWorldPose()).pos.x);
            // int yPos = (int) abs((this->model->GetWorldPose()).pos.y);
            this->generator = std::default_random_engine(robNum);

            this->uformMin = 0;
            this->uformMax = 1;
            this->uformRand = std::uniform_real_distribution<double>(this->uformMin,this->uformMax);

            this->normalStddev = 1.5707;
            this->normalMean = 3.142;
            this->normalRandHeading = std::normal_distribution<double>(this->normalMean,this->normalStddev);

            //initialize state object
            this->myState = RobotState(0.0025,myRot.GetYaw(),1000,15,M_PI * 2);
            //update algorithm object
            this->randomWalk = RW(this->generator,this->uformRand,this->normalRandHeading);

        }
        public: std::string rotate(double headingError)
        {
            std::string dxn;
            if(headingError > 0)
            {
                this->lwheel->SetVelocity(0,-(this->rVel/2.0));
                this->rwheel->SetVelocity(0,this->rVel/2.0);
                dxn = "turn-left";
            }
            else if(headingError < 0)
            {
                this->lwheel->SetVelocity(0,(this->rVel/2.0));
                this->rwheel->SetVelocity(0,-(this->rVel/2.0));
                dxn = "turn-right";
            }
            return dxn;
        }

        public: void moveForward(double headingError)
        {
            double va = headingError * this->Kp;
            double r = this->rVel + va * this->wheel_separation / 2.0;
            double l = this->rVel - va * this->wheel_separation / 2.0;

            if (r > this->rVel)
                r = this->rVel;
            if (r < 0)
                r = 0;

            if (l > this->rVel)
                l = this->rVel;
            if (l < 0)
                l = 0;

            this->rwheel->SetVelocity(0,r);
            this->lwheel->SetVelocity(0,l);
        }

        public: void OnUpdate(const common::UpdateInfo & _info)
        {
            std::lock_guard<std::mutex> lock(this->mutex);
            math::Quaternion myRot = (this->model->GetWorldPose()).rot;
            // (this->myState).desiredHeading = myRot.GetYaw();
            // std::stringstream robotData;

            //sense presence of litter
            this->litterSensor(&(this->myState), this->model, this->model->GetWorld());

            if((this->myState).crashed)
            {//avoid obstacle: change direction; state=obstacle-avoidance
                (this->myState).desiredHeading = (this->myState).desiredHeading + (this->myState).turnAmount;
                (this->myState).crashed = false;
                (this->myState).state = "obstacle-avoidance";

                //robotData<<"crashed: "<<(this->myState).desiredHeading<<",";
            }
            // else if() {
            //     //capacity is full, to toward home: state=go-home
            // }
            // else if() {
            //     //litter found, go toward closest litter: state=acquire
            // }
            else {
                //Execute Control Algorithm: state=search; RW,
                (this->randomWalk).updateHeading(&(this->myState));

                (this->myState).state = "search";
                //robotData<<"Random turn: "<<(this->myState).desiredHeading<<",";
            }

            (this->myState).desiredHeading = (this->utils).normalize((this->myState).desiredHeading);
            double headingError = (this->myState).desiredHeading - myRot.GetYaw();
            headingError = (this->utils).normalize(headingError);
            if(abs(headingError) < 0.09)
            {//move straight
                this->moveForward(headingError);
                (this->myState).action = "forward";
                //robotData<<"move straight,";
            }
            else
            {
                //turn in desired direction
                (this->myState).action = this->rotate(headingError);

            }
            // robotData<<(this->myState).state<<","<<(this->myState).action<<": "
            //     <<(this->myState).desiredHeading<<" - "<<myRot.GetYaw()<<" = "
            //     <<headingError<<std::endl;
            std::string robotDataStr = (this->myState).state2string();
            msgs::Any robotDataMsg;
            robotDataMsg.set_type(msgs::Any::STRING);
            robotDataMsg.set_string_value(robotDataStr);
            // robotDataMsg.set_string_value(robotData.str());
            this->pubRobotData->Publish(robotDataMsg);
            std::string myName  = this->model->GetName();
            if (myName.find("robot18") != std::string::npos) {
                std::cout<<"seen litter: "<<(this->myState).seenLitter<<std::endl;
            }
        }
    };

    // Register this plugin with the simulator
    GZ_REGISTER_MODEL_PLUGIN(MP_CPFA)
}


// libgp - Gaussian process library for Machine Learning
// Copyright (c) 2013, Manuel Blum <mblum@informatik.uni-freiburg.de>
// All rights reserved.

#include "gp.h"
#include "gp_utils.h"

#include "cg.h"
#include "rprop.h"
#include <Eigen/Dense>

using namespace libgp;

int main(int argc, char const *argv[]) {
  int n = 1000, m = 1000;
  double tss = 0, error, f, y;
  // initialize Gaussian process for 2-D input using the squared exponential
  // covariance function with additive white noise.
  GaussianProcess gp(2, "CovSum ( CovRQiso, CovNoise)");
  // initialize hyper parameter vector
  Eigen::VectorXd params(gp.covf().get_param_dim());
  params << 2.0, 2.0, 2.0, -2.0;
  // set parameters of covariance function
  gp.covf().set_loghyper(params);

  // add training patterns
  for (int i = 0; i < n; ++i) {
    double x[] = {drand48() * 4 - 2, drand48() * 4 - 2};
    y = Utils::hill(x[0], x[1]) + Utils::randn() * 0.5;
    gp.add_pattern(x, y);
  }
  libgp::RProp rprop;
  rprop.init();
  rprop.maximize(&gp, 50, true);
  // gp.log_likelihood();
  gp.write("model.txt");
  // total squared error
  for (int i = 0; i < m; ++i) {
    double x[] = {drand48() * 4 - 2, drand48() * 4 - 2};
    f = gp.f(x);
    y = Utils::hill(x[0], x[1]);
    error = f - y;
    tss += error * error;
  }
  std::cout << "mse = " << tss / m << std::endl;
  return EXIT_SUCCESS;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines helper functions for use with Variables class

#pragma once

#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstddef>
#include <ostream>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/SliceIterator.hpp"
#include "ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TypeTraits.hpp"

/// \cond
template <size_t VolumeDim>
class OrientationMap;
template <size_t>
class Index;
template <typename TagsList>
class Variables;

// clang-tidy: redundant declarations
template <typename Tag, typename TagList>
constexpr typename Tag::type& get(Variables<TagList>& v) noexcept;  // NOLINT
template <typename Tag, typename TagList>
constexpr const typename Tag::type& get(  // NOLINT
    const Variables<TagList>& v) noexcept;
/// \endcond

/*!
 * \ingroup DataStructuresGroup
 * \brief Slices the data within `vars` to a codimension 1 slice. The
 * slice has a constant logical coordinate in direction `sliced_dim`,
 * slicing the volume at `fixed_index` in that dimension.  For
 * example, to get the lower boundary of `sliced_dim`, pass `0` for
 * `fixed_index`; to get the upper boundary, pass
 * `extents[sliced_dim] - 1`.
 *
 * \see add_slice_to_data
 *
 * \return Variables class sliced to a hypersurface.
 */
template <std::size_t VolumeDim, typename TagsList>
Variables<TagsList> data_on_slice(const Variables<TagsList>& vars,
                                  const Index<VolumeDim>& element_extents,
                                  const size_t sliced_dim,
                                  const size_t fixed_index) noexcept {
  const size_t interface_grid_points =
      element_extents.slice_away(sliced_dim).product();
  const size_t volume_grid_points = vars.number_of_grid_points();
  constexpr const size_t number_of_independent_components =
      Variables<TagsList>::number_of_independent_components;
  Variables<TagsList> interface_vars(interface_grid_points);
  const double* vars_data = vars.data();
  double* interface_vars_data = interface_vars.data();
  for (SliceIterator si(element_extents, sliced_dim, fixed_index); si; ++si) {
    for (size_t i = 0; i < number_of_independent_components; ++i) {
      // clang-tidy: do not use pointer arithmetic
      interface_vars_data[si.slice_offset() +                      // NOLINT
                          i * interface_grid_points] =             // NOLINT
          vars_data[si.volume_offset() + i * volume_grid_points];  // NOLINT
    }
  }
  return interface_vars;
}

/*!
 * \ingroup DataStructuresGroup
 * \brief Slices volume `Tensor`s into a `Variables`
 *
 * The slice has a constant logical coordinate in direction `sliced_dim`,
 * slicing the volume at `fixed_index` in that dimension.  For
 * example, to get the lower boundary of `sliced_dim`, pass `0` for
 * `fixed_index`; to get the upper boundary, pass
 * `extents[sliced_dim] - 1`.
 */
template <typename... TagsToSlice, size_t VolumeDim>
Variables<tmpl::list<TagsToSlice...>> data_on_slice(
    const Index<VolumeDim>& element_extents,
    const size_t sliced_dim, const size_t fixed_index,
    const typename TagsToSlice::type&... tensors) noexcept {
  const size_t interface_grid_points =
      element_extents.slice_away(sliced_dim).product();
  Variables<tmpl::list<TagsToSlice...>> interface_vars(interface_grid_points);
  for (SliceIterator si(element_extents, sliced_dim, fixed_index); si; ++si) {
    const auto lambda = [&si](auto& interface_tensor,
                              const auto& volume_tensor) noexcept {
      for (decltype(auto) interface_and_volume_tensor_components :
           boost::combine(interface_tensor, volume_tensor)) {
        boost::get<0>(
            interface_and_volume_tensor_components)[si.slice_offset()] =
            boost::get<1>(
                interface_and_volume_tensor_components)[si.volume_offset()];
      }
    };
    expand_pack((lambda(get<TagsToSlice>(interface_vars), tensors),
                 cpp17::void_type{})...);
  }
  return interface_vars;
}

/*!
 * \ingroup DataStructuresGroup
 * \brief Adds data on a codimension 1 slice to a volume quantity. The
 * slice has a constant logical coordinate in direction `sliced_dim`,
 * slicing the volume at `fixed_index` in that dimension.  For
 * example, to add to the lower boundary of `sliced_dim`, pass `0` for
 * `fixed_index`; to add to the upper boundary, pass
 * `extents[sliced_dim] - 1`.
 *
 * \see data_on_slice
 */
template <std::size_t VolumeDim, typename TagsList>
void add_slice_to_data(const gsl::not_null<Variables<TagsList>*> volume_vars,
                       const Variables<TagsList>& vars_on_slice,
                       const Index<VolumeDim>& extents,
                       const size_t sliced_dim,
                       const size_t fixed_index) noexcept {
  constexpr const size_t number_of_independent_components =
      Variables<TagsList>::number_of_independent_components;
  const size_t volume_grid_points = extents.product();
  const size_t slice_grid_points = extents.slice_away(sliced_dim).product();
  ASSERT(volume_vars->number_of_grid_points() == volume_grid_points,
         "volume_vars has wrong number of grid points.  Expected "
         << volume_grid_points << ", got "
         << volume_vars->number_of_grid_points());
  ASSERT(vars_on_slice.number_of_grid_points() == slice_grid_points,
         "vars_on_slice has wrong number of grid points.  Expected "
         << slice_grid_points << ", got "
         << vars_on_slice.number_of_grid_points());
  double* const volume_data = volume_vars->data();
  const double* const slice_data = vars_on_slice.data();
  for (SliceIterator si(extents, sliced_dim, fixed_index); si; ++si) {
    for (size_t i = 0; i < number_of_independent_components; ++i) {
      // clang-tidy: do not use pointer arithmetic
      volume_data[si.volume_offset() + i * volume_grid_points] +=  // NOLINT
          slice_data[si.slice_offset() + i * slice_grid_points];  // NOLINT
    }
  }
}

namespace OrientVariablesOnSlice_detail {

inline std::vector<size_t> oriented_offset(
    const Index<0>& /*slice_extents*/, const size_t /*sliced_dim*/,
    const OrientationMap<1>& /*orientation_of_neighbor*/) noexcept {
  // There is only one point on a slice of a 1D mesh
  return {0};
}

std::vector<size_t> oriented_offset(
    const Index<1>& slice_extents, size_t sliced_dim,
    const OrientationMap<2>& orientation_of_neighbor) noexcept;

std::vector<size_t> oriented_offset(
    const Index<2>& slice_extents, size_t sliced_dim,
    const OrientationMap<3>& orientation_of_neighbor) noexcept;
}  // namespace OrientVariablesOnSlice_detail

/// \ingroup DataStructuresGroup
/// Orients variables on a slice to the data-storage order of a neighbor with
/// the given orientation.
template <size_t VolumeDim, typename TagsList>
Variables<TagsList> orient_variables_on_slice(
    const Variables<TagsList>& variables_on_slice,
    const Index<VolumeDim - 1>& slice_extents, const size_t sliced_dim,
    const OrientationMap<VolumeDim>& orientation_of_neighbor) noexcept {
  const size_t number_of_grid_points = slice_extents.product();

  Variables<TagsList> oriented_variables(number_of_grid_points);
  const auto oriented_offset = OrientVariablesOnSlice_detail::oriented_offset(
      slice_extents, sliced_dim, orientation_of_neighbor);

  tmpl::for_each<TagsList>([&oriented_variables, &variables_on_slice,
                            &oriented_offset,
                            &number_of_grid_points](auto tag) {
    using Tag = tmpl::type_from<decltype(tag)>;
    auto& oriented_tensor = get<Tag>(oriented_variables);
    const auto& tensor_on_slice = get<Tag>(variables_on_slice);
    for (decltype(auto) oriented_and_slice_tensor_components :
         boost::combine(oriented_tensor, tensor_on_slice)) {
      DataVector& oriented_tensor_component =
          boost::get<0>(oriented_and_slice_tensor_components);
      const DataVector& tensor_component_on_slice =
          boost::get<1>(oriented_and_slice_tensor_components);
      for (size_t s = 0; s < number_of_grid_points; ++s) {
        oriented_tensor_component[oriented_offset[s]] =
            tensor_component_on_slice[s];
      }
    }
  });

  return oriented_variables;
}


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

/**
 * \file gps_vertex_prior.hpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#pragma once
#if 0
#include <Eigen/Core>

#include <vtr_pose_graph/index/rc_graph.hpp>
#include <vtr_pose_graph/relaxation/graph_optimization_problem.hpp>
//#include <asrl/messages/GPSMeasurement.pb.h>
#include <babelfish_robochunk_sensor_msgs/NavSatFix.pb.h>
#endif
namespace vtr {
namespace pose_graph {
#if 0
struct GpsPoint {
  GpsPoint(const Eigen::Vector3d& point) : point_(point), covSet_(false) {}
  GpsPoint(const Eigen::Vector3d& point, const Eigen::Matrix3d& cov)
      : point_(point), cov_(cov), covSet_(true) {}

  Eigen::Vector3d point_;
  Eigen::Matrix3d cov_;
  bool covSet_;
};

class GpsVertexPrior : public OptimizationTypeBase<RCGraph> {
 public:
  typedef OptimizationTypeBase<RCGraph> Base;

  // Explicity import things from the dependent scope
  typedef RCGraph::Ptr GraphPtr;
  typedef RCGraph::VertexIdType VertexIdType;
  typedef RCGraph::TransformType TransformType;
  typedef typename Base::TfMapType TfMapType;
  typedef typename Base::StateMapType StateMapType;
  typedef typename Base::CostTermPtr CostTermPtr;

  typedef babelfish::robochunk::sensor_msgs::NavSatFix MsgType;

  typedef steam::LossFunctionBase::Ptr LossFuncPtr;
  typedef steam::StaticNoiseModel<3> NoiseModel;
  typedef steam::BaseNoiseModel<3>::Ptr NoiseModelPtr;

  static constexpr char gpsStream[] = "/DGPS/fix";

  PTR_TYPEDEFS(GpsVertexPrior)
  DEFAULT_COPY_MOVE(GpsVertexPrior)

  /////////////////////////////////////////////////////////////////////////////
  /// @brief MakeShared forwarding template to catch all constructors
  /////////////////////////////////////////////////////////////////////////////
  template <typename... Args>
  static Ptr MakeShared(Args&&... args) {
    return Ptr(new GpsVertexPrior(std::forward<Args>(args)...));
  }

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Build state variables/cost terms and add them to the problem
  /////////////////////////////////////////////////////////////////////////////
  GpsVertexPrior(
      const Eigen::Matrix3d& cov = Eigen::Matrix3d::Identity(),
      const TransformType& T_gps_base = TransformType(), bool useAdj = false,
      const LossFuncPtr& lossFunc = LossFuncPtr(new steam::L2LossFunc()));

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Build state variables/cost terms and add them to the problem
  /////////////////////////////////////////////////////////////////////////////
  GpsVertexPrior(
      const NoiseModelPtr& noiseModel,
      const TransformType& T_gps_base = TransformType(), bool useAdj = false,
      const LossFuncPtr& lossFunc = LossFuncPtr(new steam::L2LossFunc()));

  virtual ~GpsVertexPrior() {}

  /////////////////////////////////////////////////////////////////////////////
  /// @brief Build state variables/cost terms and add them to the problem
  /////////////////////////////////////////////////////////////////////////////
  virtual void addCostTerms(const GraphPtr& graph, const VertexIdType& root,
                            StateMapType& stateMap, CostTermPtr& costTerms,
                            const Eval::Mask::Ptr& mask);

 private:
  NoiseModelPtr noiseModel_;

  TransformType T_gps_base_;

  LossFuncPtr lossFunc_;

  bool useAdj_;
};

#endif
}  // namespace pose_graph
}  // namespace vtr

//#include <asrl/pose_graph/relaxation/implementation/GpsVertexPrior-impl.hpp>


#pragma once

#include <boost/optional.hpp>

#include "GNAObject.hh"
#include "UncertainParameter.hh"

class CovariatedPrediction: public GNAObject,
                            public TransformationBind<CovariatedPrediction> {
public:

    /**
   * @brief Defines a segment in covariance
   */
  struct Segment {
    size_t i, n;

    void dump() const {
      printf("i=%zu, n=%zu, i+n=%zu", i, n, i+n);
    }
  };

  CovariatedPrediction();
  //CovariatedPrediction(const CovariatedPrediction &other);

  //CovariatedPrediction &operator=(const CovariatedPrediction &other);

  void append(SingleOutput &obs);
  void finalize();
  void prediction_ready();

  size_t blockOffset(OutputDescriptor inp);
  size_t blocksCount() const noexcept;
  void covariate(SingleOutput &cov,
                 SingleOutput &obs1, size_t n1,
                 SingleOutput &obs2, size_t n2);
  void rank1(SingleOutput &vec);

  Segment resolveSegment(Atypes args, const Segment &iseg);
  void resolveCovarianceActions(Atypes args);
  void calculateTypes(TypesFunctionArgs fargs);
  void calculatePrediction(FunctionArgs fargs);
  void calculateCovbaseTypes(TypesFunctionArgs fargs);
  void calculateCovbase(FunctionArgs fargs);
  void calculateCovTypes(TypesFunctionArgs fargs);
  void calculateCov(FunctionArgs fargs);
  void addSystematicCovMatrix(SingleOutput& sys_covmat);

  size_t size() const;

  void update() const;
protected:

    /**
   * @brief Defines an action to perform on a given segment
   * @param Action -- Either CovarianceAction::Diagonal or CovarianceAction::Block
   */
  struct CovarianceAction {
    enum Action {
      Diagonal, Block
    };

    explicit CovarianceAction(Action act) : action(act) { }

    Action action;

    boost::optional<Segment> a, b;

    bool resolved = false;
    boost::optional<Segment> x, y;

    void dump() const {
      printf("Covariance action on %s:", action==Diagonal ? "diag" : "block");
      if(a) { printf("a "); a->dump(); }
      if(x) { printf(", x "); x->dump(); }
      if(b) { printf(", b "); b->dump(); }
      if(y) { printf(", y "); y->dump(); }
      printf("\n");
    }
  };

/**
   * @brief Stores LLT decomposition matrix and provides access to it
   * @param size -- size of a matrix to be allocated.
*/
 /* TODO: Also would need change to play along with floats. */
  class LLT: public Eigen::LLT<Eigen::MatrixXd> {
  public:
    explicit LLT(): Eigen::LLT<Eigen::MatrixXd>() { }
    explicit LLT(size_t size): Eigen::LLT<Eigen::MatrixXd>(size) { }
    Eigen::MatrixXd& matrixRef() { return this->m_matrix; }
  };

  std::vector<OutputDescriptor> m_inputs;

  std::vector<CovarianceAction> m_covactions;

  bool m_finalized;
  bool m_prediction_ready;
  bool m_diagonal_covbase=false;
  bool m_diagonal_cov=false;
  size_t m_size=0u;
  LLT m_llt;
};