//  (C) Copyright Raffi Enficiaud 2017.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

// See ticket #13011

#define BOOST_TEST_MODULE fp_check_relational_op
#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;
namespace tt = boost::test_tools;

//
// with zeros
//

BOOST_AUTO_TEST_CASE(EQ_zero_on_left, * utf::expected_failures(2))
{
    BOOST_TEST(0.0 == 1.0);
    BOOST_TEST(0.0 == 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(NE_zero_on_left)
{
    BOOST_TEST(0.0 != 1.0);
    BOOST_TEST(0.0 != 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(EQ_zero_on_right, * utf::expected_failures(2))
{
    BOOST_TEST(1.0 == 0.0);
    BOOST_TEST(1.0 == 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(NE_zero_on_right)
{
    BOOST_TEST(1.0 != 0.0);
    BOOST_TEST(1.0 != 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(EQ_zero_on_left_right)
{
    BOOST_TEST(0.0 == 0.0);
    BOOST_TEST(0.0 == 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(NE_zero_on_left_right, * utf::expected_failures(2))
{
    BOOST_TEST(0.0 != 0.0);
    BOOST_TEST(0.0 != 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(LT_zero_on_left)
{
    BOOST_TEST(0.0 < 1.0);
    BOOST_TEST(0.0 < 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GT_zero_on_left, * utf::expected_failures(2) )
{
    BOOST_TEST(0.0 > 1.0);
    BOOST_TEST(0.0 > 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GT_zero_on_right)
{
    BOOST_TEST(1.0 > 0.0);
    BOOST_TEST(1.0 > 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(LT_zero_on_right, * utf::expected_failures(2) )
{
    BOOST_TEST(1.0 < 0.0);
    BOOST_TEST(1.0 < 0.0, tt::tolerance(0.001));
}

// with equality

BOOST_AUTO_TEST_CASE(LE_zero_on_left)
{
    BOOST_TEST(0.0 <= 1.0);
    BOOST_TEST(0.0 <= 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GE_zero_on_left, * utf::expected_failures(2) )
{
    BOOST_TEST(0.0 >= 1.0);
    BOOST_TEST(0.0 >= 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GE_zero_on_right)
{
    BOOST_TEST(1.0 >= 0.0);
    BOOST_TEST(1.0 >= 0.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(LE_zero_on_right, * utf::expected_failures(2) )
{
    BOOST_TEST(1.0 <= 0.0);
    BOOST_TEST(1.0 <= 0.0, tt::tolerance(0.001));
}

//
// with numbers
//

BOOST_AUTO_TEST_CASE(LT_smaller_on_left)
{
    BOOST_TEST(1.0 < 2.0);
    BOOST_TEST(1.0 < 2.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GT_smaller_on_left, * utf::expected_failures(2) )
{
    BOOST_TEST(1.0 > 2.0);
    BOOST_TEST(1.0 > 2.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GT_smaller_on_right)
{
    BOOST_TEST(2.0 > 1.0);
    BOOST_TEST(2.0 > 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(LT_smaller_on_right, * utf::expected_failures(2) )
{
    BOOST_TEST(2.0 < 1.0);
    BOOST_TEST(2.0 < 1.0, tt::tolerance(0.001));
}

// with equality

BOOST_AUTO_TEST_CASE(LE_smaller_on_left)
{
    BOOST_TEST(1.0 <= 2.0);
    BOOST_TEST(1.0 <= 2.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GE_smaller_on_left, * utf::expected_failures(2) )
{
    BOOST_TEST(1.0 >= 2.0);
    BOOST_TEST(1.0 >= 2.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(GE_smaller_on_right)
{
    BOOST_TEST(2.0 >= 1.0);
    BOOST_TEST(2.0 >= 1.0, tt::tolerance(0.001));
}

BOOST_AUTO_TEST_CASE(LE_smaller_on_right, * utf::expected_failures(2) )
{
    BOOST_TEST(2.0 <= 1.0);
    BOOST_TEST(2.0 <= 1.0, tt::tolerance(0.001));
}

// infinity
BOOST_AUTO_TEST_CASE(infinity_comparison)
{
  //BOOST_TEST(2.0 <= 1.0);
    BOOST_TEST(0 < std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() == std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() >= std::numeric_limits<float>::infinity());
  //BOOST_TEST(2.0 <= 1.0);

    float a = std::numeric_limits<float>::infinity();
    BOOST_TEST(a == std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() == a);

    float b = a;
    BOOST_TEST(b == a);
    BOOST_TEST(a == b);
}

BOOST_AUTO_TEST_CASE(infinity_comparison_with_tolerance,
  *utf::tolerance<float>(boost::math::fpc::percent_tolerance<float>(0.01))
) {
    BOOST_TEST(0 < std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() == std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() >= std::numeric_limits<float>::infinity());

    float a = std::numeric_limits<float>::infinity();
    BOOST_TEST(a == std::numeric_limits<float>::infinity());
    BOOST_TEST(std::numeric_limits<float>::infinity() == a);

    float b = a;
    BOOST_TEST(b == a);
    BOOST_TEST(a == b);
    BOOST_TEST((b == a));
    BOOST_TEST((a == b));

    BOOST_TEST(-a <= b);
    BOOST_TEST(-a < b);

    BOOST_TEST(a == b, tt::tolerance(0.f));
    BOOST_TEST(a == b, tt::tolerance(10.f));
    BOOST_TEST(a == b, tt::tolerance(1E10f));
}


#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <Eigen/Core>
#include "openbr_internal.h"

#include "openbr/core/opencvutils.h"

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Computes integral image.
 * \author Josh Klontz \cite jklontz
 */
class IntegralTransform : public UntrainableTransform
{
    Q_OBJECT

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

BR_REGISTER(Transform, IntegralTransform)

/*!
 * \ingroup transforms
 * \brief Sliding window feature extraction from a multi-channel integral image.
 * \author Josh Klontz \cite jklontz
 */
class IntegralSamplerTransform : public UntrainableTransform
{
    Q_OBJECT
    Q_PROPERTY(int scales READ get_scales WRITE set_scales RESET reset_scales STORED false)
    Q_PROPERTY(float scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(float stepFactor READ get_stepFactor WRITE set_stepFactor RESET reset_stepFactor STORED false)
    Q_PROPERTY(int minSize READ get_minSize WRITE set_minSize RESET reset_minSize STORED false)
    Q_PROPERTY(bool secondOrder READ get_secondOrder WRITE set_secondOrder RESET reset_secondOrder STORED false)
    BR_PROPERTY(int, scales, 6)
    BR_PROPERTY(float, scaleFactor, 2)
    BR_PROPERTY(float, stepFactor, 0.75)
    BR_PROPERTY(int, minSize, 8)
    BR_PROPERTY(bool, secondOrder, false)

    void project(const Template &src, Template &dst) const
    {
        typedef Eigen::Map< const Eigen::Matrix<qint32,Eigen::Dynamic,1> > InputDescriptor;
        typedef Eigen::Map< const Eigen::Matrix<float,Eigen::Dynamic,1> > SecondOrderInputDescriptor;
        typedef Eigen::Map< Eigen::Matrix<float,Eigen::Dynamic,1> > OutputDescriptor;

        const Mat &m = src.m();
        if (m.depth() != CV_32S) qFatal("Expected CV_32S matrix depth.");
        const int channels = m.channels();
        const int rowStep = channels * m.cols;

        int descriptors = 0;
        float idealSize = min(m.rows, m.cols)-1;
        for (int scale=0; scale<scales; scale++) {
            const int currentSize(idealSize);
            const int numDown = 1+(m.rows-currentSize-1)/int(idealSize*stepFactor);
            const int numAcross = 1+(m.cols-currentSize-1)/int(idealSize*stepFactor);
            descriptors += numDown*numAcross;
            if (secondOrder) descriptors += numDown*(numAcross-1) + (numDown-1)*numAcross;
            idealSize /= scaleFactor;
            if (idealSize < minSize) break;
        }
        Mat n(descriptors, channels, CV_32FC1);

        const qint32 *dataIn = (qint32*)m.data;
        float *dataOut = (float*)n.data;
        idealSize = min(m.rows, m.cols)-1;
        int index = 0;
        for (int scale=0; scale<scales; scale++) {
            const int currentSize(idealSize);
            const int currentStep(idealSize*stepFactor);
            for (int i=currentSize; i<m.rows; i+=currentStep) {
                for (int j=currentSize; j<m.cols; j+=currentStep) {
                    InputDescriptor a(dataIn+((i-currentSize)*rowStep+(j-currentSize)*channels), channels, 1);
                    InputDescriptor b(dataIn+((i-currentSize)*rowStep+ j             *channels), channels, 1);
                    InputDescriptor c(dataIn+(i              *rowStep+(j-currentSize)*channels), channels, 1);
                    InputDescriptor d(dataIn+(i              *rowStep+ j             *channels), channels, 1);
                    OutputDescriptor y(dataOut+(index*channels), channels, 1);
                    y = (d-b-c+a).cast<float>()/(currentSize*currentSize);
                    index++;
                }
            }
            if (secondOrder) {
                const int numDown = 1+(m.rows-currentSize-1)/currentStep;
                const int numAcross = 1+(m.cols-currentSize-1)/currentStep;
                const float *dataIn = n.ptr<float>(index - numDown*numAcross);
                for (int i=0; i<numDown; i++) {
                    for (int j=0; j<numAcross; j++) {
                        SecondOrderInputDescriptor a(dataIn + (i*numAcross+j)*channels, channels, 1);
                        if (j < numAcross-1) {
                            OutputDescriptor y(dataOut+(index*channels), channels, 1);
                            y = a - SecondOrderInputDescriptor(dataIn + (i*numAcross+j+1)*channels, channels, 1);
                            index++;
                        }
                        if (i < numDown-1) {
                            OutputDescriptor y(dataOut+(index*channels), channels, 1);
                            y = a - SecondOrderInputDescriptor(dataIn + ((i+1)*numAcross+j)*channels, channels, 1);
                            index++;
                        }
                    }
                }
            }
            idealSize /= scaleFactor;
            if (idealSize < minSize) break;
        }

        if (descriptors != index)
            qFatal("Allocated %d descriptors but computed %d.", descriptors, index);

        dst.m() = n;
    }
};

BR_REGISTER(Transform, IntegralSamplerTransform)

/*!
 * \ingroup transforms
 * \brief Construct template in a recursive decent manner.
 * \author Josh Klontz \cite jklontz
 */
class RecursiveIntegralSamplerTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(int scales READ get_scales WRITE set_scales RESET reset_scales STORED false)
    Q_PROPERTY(float scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(int minSize READ get_minSize WRITE set_minSize RESET reset_minSize STORED false)
    Q_PROPERTY(br::Transform *transform READ get_transform WRITE set_transform RESET reset_transform)
    BR_PROPERTY(int, scales, 6)
    BR_PROPERTY(float, scaleFactor, 2)
    BR_PROPERTY(int, minSize, 8)
    BR_PROPERTY(br::Transform*, transform, NULL)

    Transform *subTransform;

    typedef Eigen::Map< const Eigen::Matrix<qint32,Eigen::Dynamic,1> > InputDescriptor;
    typedef Eigen::Map< Eigen::Matrix<float,Eigen::Dynamic,1> > OutputDescriptor;
    typedef Eigen::Map< const Eigen::Matrix<float,Eigen::Dynamic,1> > SecondOrderInputDescriptor;

    void init()
    {
        if (scales >= 2) {
            File subFile = file;
            subFile.set("scales", scales-1);
            subTransform = make(subFile.flat());
        } else {
            subTransform = NULL;
        }
    }

    static void integralHistogram(const Mat &src, const int x, const int y, const int width, const int height, Mat &dst, int index)
    {
        const int channels = src.channels();
        OutputDescriptor(dst.ptr<float>(index), channels, 1) =
            (  InputDescriptor(src.ptr<qint32>(y+height, x+width), channels, 1)
             - InputDescriptor(src.ptr<qint32>(y,        x+width), channels, 1)
             - InputDescriptor(src.ptr<qint32>(y+height, x),       channels, 1)
             + InputDescriptor(src.ptr<qint32>(y,        x),       channels, 1)).cast<float>()/(height*width);
    }

    void computeDescriptor(const Mat &src, Mat &dst) const
    {
        const int channels = src.channels();
        const int rows = src.rows-1; // Integral images have an extra row and column
        const int columns = src.cols-1;

        Mat tmp(5, channels, CV_32FC1);
        integralHistogram(src,         0,      0, columns/2, rows/2, tmp, 0);
        integralHistogram(src, columns/2,      0, columns/2, rows/2, tmp, 1);
        integralHistogram(src,         0, rows/2, columns/2, rows/2, tmp, 2);
        integralHistogram(src, columns/2, rows/2, columns/2, rows/2, tmp, 3);
        integralHistogram(src, columns/4, rows/4, columns/2, rows/2, tmp, 4);
        const SecondOrderInputDescriptor a(tmp.ptr<float>(0), channels, 1);
        const SecondOrderInputDescriptor b(tmp.ptr<float>(1), channels, 1);
        const SecondOrderInputDescriptor c(tmp.ptr<float>(2), channels, 1);
        const SecondOrderInputDescriptor d(tmp.ptr<float>(3), channels, 1);
        const SecondOrderInputDescriptor e(tmp.ptr<float>(4), channels, 1);

        dst = Mat(5, channels, CV_32FC1);
        OutputDescriptor(dst.ptr<float>(0), channels, 1) = (a+b+c+d)/4.f;
        OutputDescriptor(dst.ptr<float>(1), channels, 1) = ((a+b+c+d)/4.f-e);
        OutputDescriptor(dst.ptr<float>(2), channels, 1) = ((a+b)-(c+d))/2.f;
        OutputDescriptor(dst.ptr<float>(3), channels, 1) = ((a+c)-(b+d))/2.f;
        OutputDescriptor(dst.ptr<float>(4), channels, 1) = ((a+d)-(b+c))/2.f;
        dst = dst.reshape(1, 1);
    }

    Template subdivide(const Template &src) const
    {
        // Integral images have an extra row and column
        int subWidth = (src.m().cols-1) / scaleFactor + 1;
        int subHeight = (src.m().rows-1) / scaleFactor + 1;
        return Template(src.file, QList<Mat>() << Mat(src, Rect(0,                     0, subWidth, subHeight))
                                               << Mat(src, Rect(src.m().cols-subWidth, 0, subWidth, subHeight))
                                               << Mat(src, Rect(0,                     src.m().rows-subHeight, subWidth, subHeight))
                                               << Mat(src, Rect(src.m().cols-subWidth, src.m().rows-subHeight, subWidth, subHeight)));
    }

    bool canSubdivide(const Template &t) const
    {
        // Integral images have an extra row and column
        const int subWidth = (t.m().cols-1) / scaleFactor;
        const int subHeight = (t.m().rows-1) / scaleFactor;
        return ((subWidth >= minSize) && (subHeight >= minSize));
    }

    void train(const TemplateList &src)
    {
        if (src.first().m().depth() != CV_32S)
            qFatal("Expected CV_32S depth!");

        if (subTransform != NULL) {
            TemplateList subSrc; subSrc.reserve(src.size());
            foreach (const Template &t, src)
                if (canSubdivide(t))
                    subSrc.append(subdivide(t));

            if (subSrc.isEmpty()) {
                delete subTransform;
                subTransform = NULL;
            } else {
                subTransform->train(subSrc);
            }
        }

        TemplateList dst; dst.reserve(src.size());
        foreach (const Template &t, src) {
            Template u(t.file);
            computeDescriptor(t, u);
            dst.append(u);
        }
        transform->train(dst);
    }

    void project(const Template &src, Template &dst) const
    {
        computeDescriptor(src, dst);
        transform->project(dst, dst);

        if ((subTransform != NULL) && canSubdivide(src)) {
            Template subDst;
            subTransform->project(subdivide(src), subDst);
            dst.append(subDst);
        }
    }

    void store(QDataStream &stream) const
    {
        transform->store(stream);
        stream << (subTransform != NULL);
        if (subTransform != NULL)
            subTransform->store(stream);
    }

    void load(QDataStream &stream)
    {
        transform->load(stream);
        bool hasSubTransform;
        stream >> hasSubTransform;
        if (hasSubTransform) subTransform->load(stream);
        else                 { delete subTransform; subTransform = NULL; }
    }
};

BR_REGISTER(Transform, RecursiveIntegralSamplerTransform)

/*!
 * \ingroup transforms
 * \brief Computes magnitude and/or angle of image.
 * \author Josh Klontz \cite jklontz
 */
class GradientTransform : public UntrainableTransform
{
    Q_OBJECT
    Q_ENUMS(Channel)
    Q_PROPERTY(Channel channel READ get_channel WRITE set_channel RESET reset_channel STORED false)

public:
    enum Channel { Magnitude, Angle, MagnitudeAndAngle };

private:
    BR_PROPERTY(Channel, channel, Angle)

    void project(const Template &src, Template &dst) const
    {
        Mat dx, dy, magnitude, angle;
        Sobel(src, dx, CV_32F, 1, 0, CV_SCHARR);
        Sobel(src, dy, CV_32F, 0, 1, CV_SCHARR);
        cartToPolar(dx, dy, magnitude, angle, true);
        std::vector<Mat> mv;
        if ((channel == Magnitude) || (channel == MagnitudeAndAngle)) {
            const float theoreticalMaxMagnitude = sqrt(2*pow(float(2*(3+10+3)*255), 2.f));
            mv.push_back(magnitude / theoreticalMaxMagnitude);
        }
        if ((channel == Angle) || (channel == MagnitudeAndAngle))
            mv.push_back(angle);
        Mat result;
        merge(mv, result);
        dst.append(result);
    }
};

BR_REGISTER(Transform, GradientTransform)

/*!
 * \ingroup transforms
 * \brief Projects each row based on a computed word.
 * \author Josh Klontz \cite jklontz
 */
class WordWiseTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(br::Transform* getWords READ get_getWords WRITE set_getWords RESET reset_getWords)
    Q_PROPERTY(br::Transform* byWord READ get_byWord WRITE set_byWord RESET reset_byWord)
    Q_PROPERTY(int numWords READ get_numWords WRITE set_numWords RESET reset_numWords)
    BR_PROPERTY(br::Transform*, getWords, NULL)
    BR_PROPERTY(br::Transform*, byWord, NULL)
    BR_PROPERTY(int, numWords, 0)

    void train(const TemplateList &data)
    {
        getWords->train(data);
        TemplateList bins;
        getWords->project(data, bins);

        numWords = 0;
        foreach (const Template &t, bins) {
            double minVal, maxVal;
            minMaxLoc(t, &minVal, &maxVal);
            numWords = max(numWords, int(maxVal)+1);
        }

        TemplateList reworded; reworded.reserve(data.size());
        foreach (const Template &t, data)
            reworded.append(reword(t));
        byWord->train(reworded);
    }

    void project(const Template &src, Template &dst) const
    {
        byWord->project(reword(src), dst);
    }

    Template reword(const Template &src) const
    {
        Template words;
        getWords->project(src, words);
        QVector<int> wordCounts(numWords, 0);
        for (int i=0; i<words.m().rows; i++)
            wordCounts[words.m().at<uchar>(i,0)]++;
        Template reworded(src.file); reworded.reserve(numWords);
        for (int i=0; i<numWords; i++)
            reworded.append(Mat(wordCounts[i], src.m().cols, src.m().type()));
        QVector<int> indicies(numWords, 0);
        for (int i=0; i<src.m().rows; i++) {
            const int word = words.m().at<uchar>(i,0);
            src.m().row(i).copyTo(reworded[word].row(indicies[word]++));
        }
        return reworded;
    }
};

BR_REGISTER(Transform, WordWiseTransform)

} // namespace br

#include "integral.moc"


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <boost/test/unit_test.hpp>

#include <cstdint>
#include <iostream>
#include <limits>
#include <sstream>
#include <string>
#include <boost/format.hpp>
#include <bitcoin/bitcoin.hpp>
#include "number.hpp"
#ifdef ENABLE_DATAGEN
#include "big_number.hpp"
#endif

BOOST_AUTO_TEST_SUITE(number_tests)

using namespace bc;
using namespace bc::machine;

// Helpers
// ----------------------------------------------------------------------------

#define BC_SCRIPT_NUMBER_CHECK_EQ(buffer_num, script_num, value, offset, test) \
    BOOST_CHECK_MESSAGE( \
    	encode_base16((buffer_num).bytes) == encode_base16((script_num).data()), \
    	"\n\tvalue index : " << value << \
		"\n\tvalue       : " << number_values[value] << \
		"\n\toffset index: " << offset << \
		"\n\toffset      : " << number_offsets[offset] << \
		"\n\ttest        : " << test << \
		"\n\tFAILURE     : [" << encode_base16((buffer_num).bytes) << " != " << \
		encode_base16((script_num).data()) << "]"); \
    BOOST_CHECK_MESSAGE((buffer_num).number == (script_num).int32(), \
       	"\n\tvalue index : " << value << \
   		"\n\tvalue       : " << number_values[value] << \
   		"\n\toffset index: " << offset << \
    	"\n\toffset      : " << number_offsets[offset] << \
		"\n\ttest        : " << test << \
		"\n\tFAILURE     : [" << (buffer_num).number << " != " << \
		(script_num).int32() << "]")

static bool is(uint8_t byte)
{
    return byte != 0;
}

// check left - right
static bool subtract_overflow64(const int64_t left, const int64_t right)
{
    return
        ((right > 0 && left < std::numeric_limits<int64_t>::min() + right) ||
        (right < 0 && left > std::numeric_limits<int64_t>::max() + right));
}

static bool add_overflow64(const int64_t left, const int64_t right)
{
    return
        ((right > 0 && left > (std::numeric_limits<int64_t>::max() - right)) ||
        (right < 0 && left < (std::numeric_limits<int64_t>::min() - right)));
}

static bool negate_overflow64(const int64_t number)
{
    return number == std::numeric_limits<int64_t>::min();
}

// Operators
// ----------------------------------------------------------------------------

static void CheckAdd(const int64_t num1, const int64_t num2, size_t value,
	size_t offset, size_t test)
{
	const number_buffer& add = number_adds[value][offset][test];
    const number scriptnum1(num1);
    const number scriptnum2(num2);

    if (!add_overflow64(num1, num2))
    {
        BC_SCRIPT_NUMBER_CHECK_EQ(add, scriptnum1 + scriptnum2, value, offset,
        	test);
        BC_SCRIPT_NUMBER_CHECK_EQ(add, scriptnum1 + num2, value, offset, test);
        BC_SCRIPT_NUMBER_CHECK_EQ(add, scriptnum2 + num1, value, offset, test);
    }
}

static void CheckNegate(const int64_t num, size_t value,
	size_t offset, size_t test)
{
	const number_buffer& negated = number_negates[value][offset][test];
    const number scriptnum(num);

    if (!negate_overflow64(num))
    {
        BC_SCRIPT_NUMBER_CHECK_EQ(negated, -scriptnum, value, offset, test);
    }
}

static void CheckSubtract(const int64_t num1, const int64_t num2, size_t value,
	size_t offset, size_t test)
{
	const number_subtract& subtract = number_subtracts[value][offset][test];
    const number scriptnum1(num1);
    const number scriptnum2(num2);

    if (!subtract_overflow64(num1, num2))
    {
        BC_SCRIPT_NUMBER_CHECK_EQ(subtract.forward, scriptnum1 - scriptnum2,
        	value, offset, test);
        BC_SCRIPT_NUMBER_CHECK_EQ(subtract.forward, scriptnum1 - num2, value,
        	offset, test);
    }

    if (!subtract_overflow64(num2, num1))
    {
        BC_SCRIPT_NUMBER_CHECK_EQ(subtract.reverse, scriptnum2 - scriptnum1,
        	value, offset, test);
        BC_SCRIPT_NUMBER_CHECK_EQ(subtract.reverse, scriptnum2 - num1, value,
        	offset, test);
    }
}

static void CheckCompare(const int64_t num1, const int64_t num2,
	size_t value, size_t offset, size_t test)
{
    const number_compare& compare = number_compares[value][offset][test];
    const number scriptnum1(num1);
    const number scriptnum2(num2);

    BOOST_CHECK(scriptnum1 == scriptnum1);
    BOOST_CHECK(scriptnum1 >= scriptnum1);
    BOOST_CHECK(scriptnum1 <= scriptnum1);
    BOOST_CHECK(!(scriptnum1 != scriptnum1));
    BOOST_CHECK(!(scriptnum1 < scriptnum1));
    BOOST_CHECK(!(scriptnum1 > scriptnum1));

    BOOST_CHECK(scriptnum1 == num1);
    BOOST_CHECK(scriptnum1 >= num1);
    BOOST_CHECK(scriptnum1 <= num1);
    BOOST_CHECK(!(scriptnum1 != num1));
    BOOST_CHECK(!(scriptnum1 < num1));
    BOOST_CHECK(!(scriptnum1 > num1));

    BOOST_CHECK_EQUAL(is(compare.eq), (scriptnum1 == scriptnum2));
    BOOST_CHECK_EQUAL(is(compare.ge), (scriptnum1 >= scriptnum2));
    BOOST_CHECK_EQUAL(is(compare.le), (scriptnum1 <= scriptnum2));
    BOOST_CHECK_EQUAL(is(compare.ne), (scriptnum1 != scriptnum2));
    BOOST_CHECK_EQUAL(is(compare.lt), (scriptnum1 < scriptnum2));
    BOOST_CHECK_EQUAL(is(compare.gt), (scriptnum1 > scriptnum2));

    BOOST_CHECK_EQUAL(is(compare.eq), (scriptnum1 == num2));
    BOOST_CHECK_EQUAL(is(compare.ge), (scriptnum1 >= num2));
    BOOST_CHECK_EQUAL(is(compare.le), (scriptnum1 <= num2));
    BOOST_CHECK_EQUAL(is(compare.ne), (scriptnum1 != num2));
    BOOST_CHECK_EQUAL(is(compare.lt), (scriptnum1 < num2));
    BOOST_CHECK_EQUAL(is(compare.gt), (scriptnum1 > num2));
}

#ifndef ENABLE_DATAGEN

// Test
// ----------------------------------------------------------------------------

static void RunOperators(const int64_t num1, int64_t num2, size_t value,
    size_t offset, size_t test)
{
    //// Diagnostics
    //std::stringstream message;
    //std::cout << boost::format(
    //    ">>> RunOperators: %1% : %2% : %3% : %4% : %5%\n")
    //    % num1 % num2 % value % offset % test;
    //BOOST_MESSAGE(message.str());

	CheckAdd(num1, num2, value, offset, test);
	CheckNegate(num1, value, offset, test);
	CheckSubtract(num1, num2, value, offset, test);
	CheckCompare(num1, num2, value, offset, test);
}

BOOST_AUTO_TEST_CASE(check_operators)
{
    for (size_t i = 0; i < number_values_count; ++i)
    {
        for (size_t j = 0; j < number_offsets_count; ++j)
        {
            auto a = number_values[i];
            auto b = number_offsets[j];

            RunOperators(a, +a,         i, j, 0);
            RunOperators(a, -a,         i, j, 1);
            RunOperators(a, +b,         i, j, 2);
            RunOperators(a, -b,         i, j, 3);
            RunOperators(a + b, +b,     i, j, 4);
            RunOperators(a + b, -b,     i, j, 5);
            RunOperators(a - b, +b,     i, j, 6);
            RunOperators(a - b, -b,     i, j, 7);
            RunOperators(a + b, +a + b, i, j, 8);
            RunOperators(a + b, +a - b, i, j, 9);
            RunOperators(a - b, +a + b, i, j, 10);
            RunOperators(a - b, +a - b, i, j, 11);
        }
    }
}

#else

// big_number value generators
// ----------------------------------------------------------------------------

static number_buffer MakeAdd(const int64_t num1, const int64_t num2)
{
    if (add_overflow64(num1, num2))
        return number_buffer();

    big_number bignum1;
    bignum1.set_int64(num1);
    big_number bignum2;
    bignum2.set_int64(num2);

    auto sum = bignum1 + bignum2;
    const number_buffer add
    {
        sum.int32(),
        sum.data()
    };

    return add;
}

static number_buffer MakeNegate(const int64_t num)
{
    if (negate_overflow64(num))
        return number_buffer();

    big_number bignum;
    bignum.set_int64(num);

    auto negative = -bignum;
    const number_buffer negated
    {
        negative.int32(),
        negative.data()
    };

    return negated;
}

static number_subtract MakeSubtract(const int64_t num1, const int64_t num2)
{
    big_number bignum1;
    bignum1.set_int64(num1);
    big_number bignum2;
    bignum2.set_int64(num2);

    big_number forward;
    if (!subtract_overflow64(num1, num2))
        forward = bignum1 - bignum2;

    big_number reverse;
    if (!subtract_overflow64(num2, num1))
        reverse = bignum2 - bignum1;

    const number_subtract subtract
    {
        { forward.int32(), forward.data() },
        { reverse.int32(), reverse.data() }
    };

    return subtract;
}

static number_compare MakeCompare(const int64_t num1, const int64_t num2)
{
    big_number bignum1;
    bignum1.set_int64(num1);
    big_number bignum2;
    bignum2.set_int64(num2);

    number_compare compare
    {
        bignum1 == bignum2,
        bignum1 != bignum2,
        bignum1 < bignum2,
        bignum1 > bignum2,
        bignum1 <= bignum2,
        bignum1 >= bignum2
    };

    return compare;
}

// Formatter Helpers
// ----------------------------------------------------------------------------

static void write_bytes(bc::data_chunk chunk, std::ostream& out)
{
    for (const auto& byte : chunk)
        out << (boost::format(" 0x%02x, ") % static_cast<uint16_t>(byte));
}

static void write_buffer(number_buffer buffer, std::ostream& out)
{
    out << boost::format("{ %1%, {") % buffer.number;
    write_bytes(buffer.bytes, out);
    out << "} }, ";
}

static void write_compare(number_compare compare, std::ostream& out)
{
    out << boost::format("{ %1%, %2%, %3%, %4%, %5%, %6% }, ") % compare.eq %
        compare.ne % compare.lt % compare.gt % compare.le % compare.ge;
}

static void write_subtract(number_subtract subtract, std::ostream& out)
{
    out << "{ ";
    write_buffer(subtract.forward, out);
    write_buffer(subtract.reverse, out);
    out << "}, ";
}

static void write_names(const std::string& name, size_t count,
    std::ostream& out)
{
    out << boost::format("const %1%[%2%][%3%][%4%]=\n{\n") % name %
        number_values_count % number_offsets_count % count;
}

static void write(const std::string& text, std::ostream& add_out,
    std::ostream& neg_out, std::ostream& sub_out, std::ostream& cmp_out)
{
    add_out << text;
    neg_out << text;
    sub_out << text;
    cmp_out << text;
}

static void replace(std::string& buffer, const std::string& find,
    const std::string& replacement)
{
    size_t pos = 0;
    while ((pos = buffer.find(find, pos)) != std::string::npos)
    {
        buffer.replace(pos, find.length(), replacement);
        pos += replacement.length();
    }
}

// Maker
// ----------------------------------------------------------------------------

static void MakeOperators(const int64_t num1, const int64_t num2,
    std::ostream& add_out, std::ostream& neg_out, std::ostream& sub_out,
    std::ostream& cmp_out)
{
    write("\n              ", add_out, neg_out, sub_out, cmp_out);

    auto add = MakeAdd(num1, num2);
    CheckAdd(num1, num2, add);
    write_buffer(add, add_out);

    auto negate = MakeNegate(num1);
    CheckNegate(num1, negate);
    write_buffer(negate, neg_out);

    auto subtract = MakeSubtract(num1, num2);
    CheckSubtract(num1, num2, subtract);
    write_subtract(subtract, sub_out);

    auto compare = MakeCompare(num1, num2);
    CheckCompare(num1, num2, compare);
    write_compare(compare, cmp_out);
}

BOOST_AUTO_TEST_CASE(make_operator_expectations)
{
    std::stringstream add_out;
    std::stringstream neg_out;
    std::stringstream sub_out;
    std::stringstream cmp_out;

    write_names("number_buffer number_adds", 12, add_out);
    write_names("number_buffer number_negates", 12, neg_out);
    write_names("number_subtract number_subtracts", 12, sub_out);
    write_names("number_compare number_compares", 12, cmp_out);

    for (size_t i = 0; i < number_values_count; ++i)
    {
        write("    {\n", add_out, neg_out, sub_out, cmp_out);

        for (size_t j = 0; j < number_offsets_count; ++j)
        {
            write("        {", add_out, neg_out, sub_out, cmp_out);

            auto a = number_values[i];
            auto b = number_offsets[j];

            MakeOperators(a, +a, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a, -a, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a, +b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a, -b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a + b, +b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a + b, -b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a - b, +b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a - b, -b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a + b, +a + b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a + b, +a - b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a - b, +a + b, add_out, neg_out, sub_out, cmp_out);
            MakeOperators(a - b, +a - b, add_out, neg_out, sub_out, cmp_out);

            write("\n        },\n", add_out, neg_out, sub_out, cmp_out);
        }

        write("    },\n", add_out, neg_out, sub_out, cmp_out);
    }

    write("};\n\n", add_out, neg_out, sub_out, cmp_out);

    std::stringstream dump;
    dump << add_out.str();
    dump << neg_out.str();
    dump << sub_out.str();
    dump << cmp_out.str();

    auto source = dump.str();
    replace(source, "-2147483648", "(-2147483647 - 1)");
    replace(source, "-9223372036854775808", "(-9223372036854775807 - 1)");
}
#endif

BOOST_AUTO_TEST_SUITE_END()


//
//   Project Name:        Kratos
//   Last modified by:    $Author: janosch $
//   Date:                $Date: 2009-01-14 09:40:45 $
//   Revision:            $Revision: 1.2 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "python/add_equation_systems_to_python.h"
#include "spaces/ublas_space.h"
#include "spaces/parallel_ublas_space.h"

#include "linear_solvers/direct_solver.h"
#include "linear_solvers/iterative_solver.h"
#include "external_includes/mkl_pardiso_solver.h"
#include "external_includes/mkl_gmres_solver.h"

namespace Kratos
{

namespace Python
{
    void  AddLinearSolversToPython()
    {
        typedef UblasSpace<double, CompressedMatrix, Vector> SpaceType;
        typedef ParallelUblasSpace<double, CompressedMatrix, Vector> ParallelSpaceType;
        typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;
        typedef UblasSpace<double, Matrix, Vector> ParallelLocalSpaceType;
        typedef LinearSolver<SpaceType,  LocalSpaceType> LinearSolverType;
        typedef LinearSolver<ParallelSpaceType,  ParallelLocalSpaceType> ParallelLinearSolverType;
        typedef DirectSolver<SpaceType,  LocalSpaceType> DirectSolverType;
        typedef Reorderer<ParallelSpaceType,  ParallelLocalSpaceType > ParallelReordererType;
        typedef DirectSolver<ParallelSpaceType,  ParallelLocalSpaceType, ParallelReordererType > ParallelDirectSolverType;
        typedef MKLPardisoSolver<SpaceType, LocalSpaceType> MKLPardisoSolverType;
        typedef MKLPardisoSolver<ParallelSpaceType, ParallelLocalSpaceType> ParallelMKLPardisoSolverType;
        typedef MKLGMRESSolver<SpaceType, LocalSpaceType> MKLGMRESSolverType;
        typedef MKLGMRESSolver<ParallelSpaceType, ParallelLocalSpaceType> ParallelMKLGMRESSolverType;
        typedef IterativeSolver<SpaceType, LocalSpaceType> IterativeSolverType;
        typedef IterativeSolver<ParallelSpaceType, ParallelLocalSpaceType> ParallelIterativeSolverType;
        typedef Preconditioner<SpaceType,  LocalSpaceType> PreconditionerType;

        using namespace boost::python;

        //***************************************************************************
        //linear solvers
        //***************************************************************************

        class_<MKLPardisoSolverType, MKLPardisoSolverType::Pointer,
        bases<DirectSolverType> >( "MKLPardisoSolver" )
                .def(init<unsigned int>() )
                .def(self_ns::str(self))
                ;

        class_<ParallelMKLPardisoSolverType, ParallelMKLPardisoSolverType::Pointer,
        bases<ParallelDirectSolverType> >( "ParallelMKLPardisoSolver" )
                .def(init<unsigned int>() )
                .def(self_ns::str(self))
                ;

        class_<MKLGMRESSolverType, MKLGMRESSolverType::Pointer,
        bases<DirectSolverType> >( "MKLGMRESSolver" )
                .def(init<>() )
                .def(self_ns::str(self))
                ;

        class_<ParallelMKLGMRESSolverType, ParallelMKLGMRESSolverType::Pointer,
        bases<ParallelDirectSolverType> >( "ParallelMKLGMRESSolver" )
                .def(init<>() )
                .def(self_ns::str(self))
                ;
  }

}  // namespace Python.

} // Namespace Kratos


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2020 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files

#include <SAMRAI_config.h>

// Headers for application-specific algorithm/data structure objects
#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/LData.h>

// Set up application namespace declarations
#include <boost/multi_array.hpp>

#include <ibamr/app_namespaces.h>

// Do some basic verification tests for LData to guarantee that we no longer
// leak memory.

void
write_ldata_info(LData& l_data)
{
    if (IBTK_MPI::getRank() == 0)
    {
        std::ofstream output("output", std::ios_base::app);
        output << "object name: " << l_data.getName() << '\n';
    }

    for (int rank = 0; rank < IBTK_MPI::getNodes(); ++rank)
    {
        if (IBTK_MPI::getRank() == rank)
        {
            std::ofstream output("output", std::ios_base::app);
            output << "\nrank: " << rank << '\n'
                   << "global nodes: " << l_data.getGlobalNodeCount() << " local nodes: " << l_data.getLocalNodeCount()
                   << " ghost nodes: " << l_data.getGhostNodeCount() << '\n';

            output << "test locally indexed:\n";

            if (l_data.getDepth() == 1)
            {
                boost::multi_array_ref<double, 1> local_entries = *l_data.getLocalFormArray();
                output << "local entries: ";
                auto it = local_entries.begin();
                for (; it != local_entries.end() - 1; ++it) output << *it << ", ";
                output << *it << '\n';

                boost::multi_array_ref<double, 1> ghost_entries = *l_data.getGhostedLocalFormArray();
                output << "local ghost entries: ";
                it = ghost_entries.begin();
                for (; it != ghost_entries.end() - 1; ++it) output << *it << ", ";
                output << *it << '\n';

                output << '\n';
            }

            {
                output << "with 2D vector:\n";
                boost::multi_array_ref<double, 2> local_entries = *l_data.getLocalFormVecArray();
                output << "local entries: ";
                auto it = local_entries.begin();
                for (; it != local_entries.end(); ++it)
                {
                    output << '(';
                    for (unsigned int i = 0; i < local_entries.shape()[1] - 1; ++i) output << (*it)[i] << ", ";
                    output << (*it)[local_entries.shape()[1] - 1] << ')';
                    if (it != local_entries.end() - 1)
                        output << ", ";
                    else
                        output << '\n';
                }

                boost::multi_array_ref<double, 2> ghost_entries = *l_data.getGhostedLocalFormVecArray();
                output << "local ghost entries: ";
                it = ghost_entries.begin();
                for (; it != ghost_entries.end(); ++it)
                {
                    output << '(';
                    for (unsigned int i = 0; i < ghost_entries.shape()[1] - 1; ++i) output << (*it)[i] << ", ";
                    output << (*it)[ghost_entries.shape()[1] - 1] << ')';
                    if (it != ghost_entries.end() - 1)
                        output << ", ";
                    else
                        output << '\n';
                }

                output << '\n';
            }

            output << "test globally indexed:\n";

            if (l_data.getDepth() == 1)
            {
                boost::multi_array_ref<double, 1> local_entries = *l_data.getArray();
                const int begin = IBTK_MPI::getRank() * 10;
                const int end = begin + (IBTK_MPI::getRank() == 3 ? 6 : 10);
                output << "local range: " << '[' << begin << ", " << end << ")\n";
                output << "local entries: ";
                for (int i = begin; i < end - 1; ++i) output << local_entries[i] << ", ";
                output << local_entries[end - 1] << '\n';
            }

            if (l_data.getDepth() == 2)
            {
                output << "with 2D vector:\n" << std::flush;
                boost::multi_array_ref<double, 2> local_entries = *l_data.getVecArray();
                const int begin = IBTK_MPI::getRank() * 10;
                const int end = begin + (IBTK_MPI::getRank() == 3 ? 3 : 5);
                output << "local range: " << '[' << begin << ", " << end << ")\n";
                output << "index bases: " << local_entries.index_bases()[0] << ", " << local_entries.index_bases()[1]
                       << '\n';
                output << "local entries: ";
                for (int i = begin; i < end - 1; ++i)
                {
                    output << '(' << local_entries[i][0] << ", " << local_entries[i][1] << ')' << ", ";
                }
                output << '(' << local_entries[end - 1][0] << ", " << local_entries[end - 1][1] << ")\n";
            }
            output << "finished rank " << rank << '\n';
        }
        IBTK_MPI::barrier();
    }
}

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);

    const int rank = IBTK_MPI::getRank();

    // clear the output file, since we will append to it from multiple processes below:
    if (rank == 0)
    {
        std::ofstream output("output");
        output << "LData\n";
    }

    // test 1: set up an owning PETSc vector with depth 1.
    {
        std::vector<int> nonlocal_nodes;
        if (IBTK_MPI::getNodes() == 4)
        {
            switch (rank)
            {
            case 0:
                nonlocal_nodes = { 10, 20, 30 };
                break;
            case 1:
                nonlocal_nodes = { 1, 21, 31 };
                break;
            case 2:
                nonlocal_nodes = { 2, 12, 32 };
                break;
            case 3:
                nonlocal_nodes = { 3, 13 };
                break;
            default:
                TBOX_ERROR("only implemented for less than four processes");
            }
        }
        // use fewer entries on the last process
        LData l_data_1("l_data_1", rank == 3 ? 6 : 10, 1, nonlocal_nodes);

        for (int i = 0; i < IBTK_MPI::getNodes(); ++i)
        {
            boost::multi_array_ref<double, 1> entries = *l_data_1.getLocalFormArray();
            double offset = 0.0;
            for (double& entry : entries)
            {
                entry = offset + 100 * rank;
                ++offset;
            }
        }
        l_data_1.beginGhostUpdate();
        l_data_1.endGhostUpdate();
        write_ldata_info(l_data_1);
    }

    // test 2: set up an owning PETSc vector with depth 2.
    {
        std::vector<int> nonlocal_nodes;
        if (IBTK_MPI::getNodes() == 4)
        {
            switch (rank)
            {
            case 0:
                nonlocal_nodes = { 10, 11, 20, 21, 30, 31 };
                break;
            case 1:
                nonlocal_nodes = { 1, 2, 21, 22, 31, 32 };
                break;
            case 2:
                nonlocal_nodes = { 2, 3, 12, 13, 32, 33 };
                break;
            case 3:
                nonlocal_nodes = { 3, 4, 13, 14 };
                break;
            default:
                TBOX_ERROR("only implemented for less than four processes");
            }
        }
        // use fewer entries on the last process
        LData l_data_2("l_data_2", rank == 3 ? 6 : 10, 2, nonlocal_nodes);

        for (int i = 0; i < IBTK_MPI::getNodes(); ++i)
        {
            boost::multi_array_ref<double, 2> entries = *l_data_2.getLocalFormVecArray();
            for (unsigned int row_n = 0; row_n < entries.shape()[0]; ++row_n)
            {
                entries[row_n][0] = row_n + 100 * rank;
                entries[row_n][1] = row_n + 1000 * rank;
            }
        }
        l_data_2.beginGhostUpdate();
        l_data_2.endGhostUpdate();
        write_ldata_info(l_data_2);
    }
} // main


#include <apriltags_ros/apriltag_detector.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <boost/foreach.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <duckietown_msgs/AprilTagDetection.h>
#include <duckietown_msgs/AprilTagDetectionArray.h>
#include <AprilTags/Tag16h5.h>
#include <AprilTags/Tag25h7.h>
#include <AprilTags/Tag25h9.h>
#include <AprilTags/Tag36h9.h>
#include <AprilTags/Tag36h11.h>
#include <XmlRpcException.h>

namespace apriltags_ros{

  //AprilTagDetector::AprilTagDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh): it_(nh){
    AprilTagDetector::AprilTagDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh){  //SUNNY
	 fx = -1;
	 fy = -1;
 	px = -1;
 	py = -1;

    XmlRpc::XmlRpcValue april_tag_descriptions;
    if(!pnh.getParam("tag_descriptions", april_tag_descriptions)){
      ROS_WARN("No april tags specified");
    }
    else{
      try{
	descriptions_ = parse_tag_descriptions(april_tag_descriptions);
      } catch(XmlRpc::XmlRpcException e){
	ROS_ERROR_STREAM("Error loading tag descriptions: "<<e.getMessage());
      }
    }

    if(!pnh.getParam("sensor_frame_id", sensor_frame_id_)){
      sensor_frame_id_ = "";
    }
      if (!pnh.getParam("tf_prefix", tf_prefix_)){
   tf_prefix_ = "";
  }

    AprilTags::TagCodes tag_codes = AprilTags::tagCodes36h11;
    tag_detector_= boost::shared_ptr<AprilTags::TagDetector>(new AprilTags::TagDetector(tag_codes));
    //image_sub_ = it_.subscribeCamera("image_rect", 1, &AprilTagDetector::imageCb, this);               //SUNNY
    switch_sub_ = nh.subscribe("switch",1,&AprilTagDetector::switchCB, this);
    //image_pub_ = it_.advertise("tag_detections_image", 1);                                            //SUNNY
    image_sub_ = nh.subscribe("image_rect", 1, &AprilTagDetector::imagePicture, this);                   //SUNNY
    cam_info_ = nh.subscribe("camera_node/camera_info", 1, &AprilTagDetector::cam_info, this);                       //SUNNY
    image_pub_ = nh.advertise<sensor_msgs::Image>("tag_detections_image", 1);
    detections_pub_ = nh.advertise<duckietown_msgs::AprilTagDetectionArray>("tag_detections", 1);
    pose_pub_ = nh.advertise<geometry_msgs::PoseArray>("tag_detections_pose", 1);
    on_switch=true;
  }
  AprilTagDetector::~AprilTagDetector(){
    image_sub_.shutdown();
  }

  void AprilTagDetector::switchCB(const duckietown_msgs::BoolStamped::ConstPtr& switch_msg){
    on_switch=switch_msg->data;
  }

/*  void AprilTagDetector::imageCb(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& cam_info){
    cv_bridge::CvImagePtr cv_ptr;
    try{
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e){
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv::Mat gray;
    cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
    std::vector<AprilTags::TagDetection>	detections = tag_detector_->extractTags(gray);
    ROS_DEBUG("%d tag detected", (int)detections.size());

    double fx = cam_info->K[0];
    double fy = cam_info->K[4];
    double px = cam_info->K[2];
    double py = cam_info->K[5];

    if(!sensor_frame_id_.empty())
      cv_ptr->header.frame_id = sensor_frame_id_;

    duckietown_msgs::AprilTagDetectionArray tag_detection_array;
    geometry_msgs::PoseArray tag_pose_array;
    tag_pose_array.header = cv_ptr->header;

    BOOST_FOREACH(AprilTags::TagDetection detection, detections){
      std::map<int, AprilTagDescription>::const_iterator description_itr = descriptions_.find(detection.id);
      if(description_itr == descriptions_.end()){
	ROS_WARN_THROTTLE(10.0, "Found tag: %d, but no description was found for it", detection.id);
	continue;
      }
      AprilTagDescription description = description_itr->second;
      double tag_size = description.size();

      detection.draw(cv_ptr->image);
      Eigen::Matrix4d transform = detection.getRelativeTransform(tag_size, fx, fy, px, py);
      Eigen::Matrix3d rot = transform.block(0,0,3,3);
      Eigen::Quaternion<double> rot_quaternion = Eigen::Quaternion<double>(rot);

      geometry_msgs::PoseStamped tag_pose;
      tag_pose.pose.position.x = transform(0,3);
      tag_pose.pose.position.y = transform(1,3);
      tag_pose.pose.position.z = transform(2,3);
      tag_pose.pose.orientation.x = rot_quaternion.x();
      tag_pose.pose.orientation.y = rot_quaternion.y();
      tag_pose.pose.orientation.z = rot_quaternion.z();
      tag_pose.pose.orientation.w = rot_quaternion.w();
      tag_pose.header = cv_ptr->header;

      duckietown_msgs::AprilTagDetection tag_detection;
      tag_detection.pose = tag_pose;
      tag_detection.id = detection.id;
      tag_detection.size = tag_size;
      tag_detection_array.detections.push_back(tag_detection);
      tag_pose_array.poses.push_back(tag_pose.pose);

      tf::Stamped<tf::Transform> tag_transform;
      tf::poseStampedMsgToTF(tag_pose, tag_transform);
      tf_pub_.sendTransform(tf::StampedTransform(tag_transform, tag_transform.stamp_, tag_transform.frame_id_, description.frame_name()));
    }
    detections_pub_.publish(tag_detection_array);
    pose_pub_.publish(tag_pose_array);
    image_pub_.publish(cv_ptr->toImageMsg());
  }
*/
/* //SUNNY */
  void AprilTagDetector::cam_info(const sensor_msgs::CameraInfoConstPtr& cam_info){

       // use camera intrinsic focal length and principal point
       // for backwards compatability
       	fx = cam_info->K[0];
        fy = cam_info->K[4];
        px = cam_info->K[2];
        py = cam_info->K[5];

  }

  void AprilTagDetector::imagePicture(const sensor_msgs::ImageConstPtr& msg){
   cv_bridge::CvImagePtr cv_ptr;
   try{
     cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
  catch (cv_bridge::Exception& e){
    ROS_ERROR("cv_bridge exception: %s", e.what());
     return;
   }
    cv::Mat gray;
    cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
    std::vector<AprilTags::TagDetection>	detections = tag_detector_->extractTags(gray);
    ROS_DEBUG("%d tag detected", (int)detections.size());

 // Check if the camera info has been updated yet
  if (fx ==-1 && fy ==-1 && px ==-1 && py ==-1){
    ROS_WARN_THROTTLE(2.0, "Skipping Frame: Camera Info Not Yet Set");
    return;
  }

    if(!sensor_frame_id_.empty())
      cv_ptr->header.frame_id = sensor_frame_id_;

    duckietown_msgs::AprilTagDetectionArray tag_detection_array;
    geometry_msgs::PoseArray tag_pose_array;
    tag_pose_array.header = cv_ptr->header;

    BOOST_FOREACH(AprilTags::TagDetection detection, detections){
      std::map<int, AprilTagDescription>::const_iterator description_itr = descriptions_.find(detection.id);
      if(description_itr == descriptions_.end()){
    ROS_WARN_THROTTLE(10.0, "Found tag: %d, but no description was found for it", detection.id);
    continue;
      }
      AprilTagDescription description = description_itr->second;
      double tag_size = description.size();

      detection.draw(cv_ptr->image);
      Eigen::Matrix4d transform = detection.getRelativeTransform(tag_size, fx, fy, px, py);
      Eigen::Matrix3d rot = transform.block(0,0,3,3);
      Eigen::Quaternion<double> rot_quaternion = Eigen::Quaternion<double>(rot);

      geometry_msgs::PoseStamped tag_pose;
      tag_pose.pose.position.x = transform(0,3);
      tag_pose.pose.position.y = transform(1,3);
      tag_pose.pose.position.z = transform(2,3);
      tag_pose.pose.orientation.x = rot_quaternion.x();
      tag_pose.pose.orientation.y = rot_quaternion.y();
      tag_pose.pose.orientation.z = rot_quaternion.z();
      tag_pose.pose.orientation.w = rot_quaternion.w();
      tag_pose.header = cv_ptr->header;

      duckietown_msgs::AprilTagDetection tag_detection;
      tag_detection.pose = tag_pose;
      tag_detection.id = detection.id;
      tag_detection.size = tag_size;
      tag_detection_array.detections.push_back(tag_detection);
      tag_pose_array.poses.push_back(tag_pose.pose);

      tf::Stamped<tf::Transform> tag_transform;
      tf::poseStampedMsgToTF(tag_pose, tag_transform);
      tf_pub_.sendTransform(tf::StampedTransform(tag_transform, tag_transform.stamp_, tag_transform.frame_id_, tf_prefix_ +description.frame_name()));
     //SUNNY
    detections_pub_.publish(tag_detection_array);
    pose_pub_.publish(tag_pose_array);
    image_pub_.publish(cv_ptr->toImageMsg());
    }

 }

  std::map<int, AprilTagDescription> AprilTagDetector::parse_tag_descriptions(XmlRpc::XmlRpcValue& tag_descriptions){
    std::map<int, AprilTagDescription> descriptions;
    ROS_ASSERT(tag_descriptions.getType() == XmlRpc::XmlRpcValue::TypeArray);
    for (int32_t i = 0; i < tag_descriptions.size(); ++i) {
      XmlRpc::XmlRpcValue& tag_description = tag_descriptions[i];
      ROS_ASSERT(tag_description.getType() == XmlRpc::XmlRpcValue::TypeStruct);
      ROS_ASSERT(tag_description["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
      ROS_ASSERT(tag_description["size"].getType() == XmlRpc::XmlRpcValue::TypeDouble);

      int id = (int)tag_description["id"];
      double size = (double)tag_description["size"];

      std::string frame_name;
      if(tag_description.hasMember("frame_id")){
	ROS_ASSERT(tag_description["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);
	frame_name = (std::string)tag_description["frame_id"];
      }
      else{
	std::stringstream frame_name_stream;
	frame_name_stream << "tag_" << id;
	frame_name = frame_name_stream.str();
      }
      AprilTagDescription description(id, size, frame_name);
      ROS_INFO_STREAM("Loaded tag config: "<<id<<", size: "<<size<<", frame_name: "<<frame_name);
      descriptions.insert(std::make_pair(id, description));
    }
    return descriptions;
  }

}


// 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 "matrix_to_list.h"

#include <Eigen/Dense>

template <typename DerivedM>
IGL_INLINE void igl::matrix_to_list(
  const Eigen::DenseBase<DerivedM> & M,
  std::vector<std::vector<typename DerivedM::Scalar > > & V)
{
  using namespace std;
  V.resize(M.rows(),vector<typename DerivedM::Scalar >(M.cols()));
  // loop over rows
  for(int i = 0;i<M.rows();i++)
  {
    // loop over cols
    for(int j = 0;j<M.cols();j++)
    {
      V[i][j] = M(i,j);
    }
  }
}

template <typename DerivedM>
IGL_INLINE void igl::matrix_to_list(
  const Eigen::DenseBase<DerivedM> & M,
  std::vector<typename DerivedM::Scalar > & V)
{
  using namespace std;
  V.resize(M.size());
  // loop over cols then rows
  for(int j = 0;j<M.cols();j++)
  {
    for(int i = 0;i<M.rows();i++)
    {
      V[i+j*M.rows()] = M(i,j);
    }
  }
}

template <typename DerivedM>
IGL_INLINE std::vector<typename DerivedM::Scalar > igl::matrix_to_list(
    const Eigen::DenseBase<DerivedM> & M)
{
  std::vector<typename DerivedM::Scalar> V;
  matrix_to_list(M,V);
  return V;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::matrix_to_list<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, 1, true> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, 1, true> > const&, std::vector<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, 1, true>::Scalar, std::allocator<Eigen::Block<Eigen::Matrix<float, -1, -1, 0, -1, -1>, -1, 1, true>::Scalar> >&);
// generated by autoexplicit.sh
template void igl::matrix_to_list<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, 1, true> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, 1, true> > const&, std::vector<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, 1, true>::Scalar, std::allocator<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, 1, true>::Scalar> >&);
// generated by autoexplicit.sh
template void igl::matrix_to_list<Eigen::Matrix<int, -1, 2, 0, -1, 2> >(Eigen::DenseBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, std::vector<Eigen::Matrix<int, -1, 2, 0, -1, 2>::Scalar, std::allocator<Eigen::Matrix<int, -1, 2, 0, -1, 2>::Scalar> >&);
// generated by autoexplicit.sh
template std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1>::Scalar> > igl::matrix_to_list<Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::DenseBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&);
// generated by autoexplicit.sh
template std::vector<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar> > igl::matrix_to_list<Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&);
//template void igl::matrix_to_list<Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, double>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > >&);
//template void igl::matrix_to_list<Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >, int>(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > >&);
template void igl::matrix_to_list<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::DenseBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar> >, std::allocator<std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar> > > >&);
template void igl::matrix_to_list<Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar> >, std::allocator<std::vector<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar> > > >&);
template void igl::matrix_to_list<Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::DenseBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1>::Scalar> >&);
template void igl::matrix_to_list<Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, std::vector<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar> >&);
template void igl::matrix_to_list<Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, std::vector<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar> >&);
template void igl::matrix_to_list<Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, std::vector<std::vector<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar> >, std::allocator<std::vector<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar, std::allocator<Eigen::Matrix<int, -1, 1, 0, -1, 1>::Scalar> > > >&);
template void igl::matrix_to_list<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::DenseBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar> >&);
template void igl::matrix_to_list<Eigen::Matrix<int, 1, -1, 1, 1, -1> >(Eigen::DenseBase<Eigen::Matrix<int, 1, -1, 1, 1, -1> > const&, std::vector<Eigen::Matrix<int, 1, -1, 1, 1, -1>::Scalar, std::allocator<Eigen::Matrix<int, 1, -1, 1, 1, -1>::Scalar> >&);
template std::vector<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar, std::allocator<Eigen::Matrix<int, -1, -1, 0, -1, -1>::Scalar> > igl::matrix_to_list<Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::DenseBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&);
template void igl::matrix_to_list<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false> > const&, std::vector<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false>::Scalar, std::allocator<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false>::Scalar> >&);
template std::vector<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false>::Scalar, std::allocator<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false>::Scalar> > igl::matrix_to_list<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false> >(Eigen::DenseBase<Eigen::Block<Eigen::Matrix<int, -1, -1, 0, -1, -1>, 1, -1, false> > const&);
#endif


#pragma once

#include <array>
#include <string_view>
#include <utility>

#include <boost/hana.hpp>
#include <nameof.hpp>

namespace idg
{
namespace detail
{
struct index_name_pair
{
public:
    std::size_t      index;
    std::string_view name;

public:
    constexpr index_name_pair(std::size_t i, std::string_view n) noexcept
        : index{i}, name{n}
    {}

    // implicitly defined copy operators
};

template<typename T>
constexpr void swap(T& lhs, T& rhs) noexcept
{
    T tmp = std::move(lhs);
    lhs   = std::move(rhs);
    rhs   = std::move(tmp);
}

template<typename It, typename Compare>
constexpr void insertion_sort(It first, It last, Compare comp)
{
    for (auto it = first + 1; it != last; ++it)
    {
        auto j = it;
        for (auto j = it; j != first; --j)
        {
            if (comp(*j, *(j - 1)))
            {
                ::idg::detail::swap(*j, *(j - 1));
            }
        }
    }
}

template<typename T, std::size_t N>
constexpr auto sort_array(const std::array<T, N>& arr) noexcept
{
    auto sorted = arr;
    ::idg::detail::insertion_sort(
        sorted.begin(), sorted.end(), [](const auto& lhs, const auto& rhs) {
            return lhs.name < rhs.name;
        });

    return sorted;
}

template<typename... Ts, std::size_t... Is>
constexpr auto sort_types_impl(std::index_sequence<Is...>,
                               boost::hana::basic_type<Ts>... types) noexcept
{
    using boost::hana::make_tuple;
    using boost::hana::size_c;

    auto types_tuple = make_tuple(types...);

    constexpr std::array<index_name_pair, sizeof...(Ts)> indexed_typenames{
        index_name_pair{Is, nameof::nameof_type<Ts>()}...};

    constexpr auto sorted_names = sort_array(indexed_typenames);

    return make_tuple(types_tuple[size_c<sorted_names[Is].index>]...);
}

template<typename... Ts>
constexpr auto sort_types(boost::hana::basic_type<Ts>... types) noexcept
{
    return ::idg::detail::sort_types_impl(
        std::make_index_sequence<sizeof...(Ts)>(), types...);
}
} // namespace detail
} // namespace idg


/**
 * \author Norihiro Watanabe
 * \date   2013-09-06
 *
 * \copyright
 * Copyright (c) 2012-2018, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "NumLib/Fem/CoordinatesMapping/ShapeMatrices.h"

#include "Tests/TestTools.h"

using namespace NumLib;

TEST(NumLib, FemShapeMatricesWithEigen)
{
    const static unsigned dim = 2;
    const static unsigned e_nnodes = 4;

    // Eigen matrix types
    using NodalVector = Eigen::Matrix<double, e_nnodes, 1>;
    using DimNodalMatrix =
        Eigen::Matrix<double, dim, e_nnodes, Eigen::RowMajor>;
    using DimMatrix = Eigen::Matrix<double, dim, dim, Eigen::RowMajor>;

    // Shape data type
    using ShapeMatricesType =
        ShapeMatrices<NodalVector, DimNodalMatrix, DimMatrix, DimNodalMatrix>;

    auto setShapeDataToOnes = [](ShapeMatricesType &shape)
            {
                shape.N.setOnes();
                shape.dNdr.setOnes();
                shape.dNdx.setOnes();
                shape.J.setOnes();
                shape.invJ.setOnes();
                shape.detJ = 1.0;
            };

    ShapeMatricesType shape(dim, dim, e_nnodes);

    // check dimension of shape matrices
    EXPECT_EQ(e_nnodes, shape.N.size());
    EXPECT_EQ(e_nnodes, shape.dNdr.cols());
    EXPECT_EQ(dim, shape.dNdr.rows());
    EXPECT_EQ(e_nnodes, shape.dNdx.cols());
    EXPECT_EQ(dim, shape.dNdx.rows());
    EXPECT_EQ(dim, shape.J.rows());
    EXPECT_EQ(dim, shape.J.cols());
    EXPECT_EQ(dim, shape.invJ.rows());
    EXPECT_EQ(dim, shape.invJ.cols());

    // check initialized with zero
    EXPECT_TRUE(shape.N.isZero());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isZero());
    EXPECT_TRUE(shape.J.isZero());
    EXPECT_TRUE(shape.invJ.isZero());
    EXPECT_EQ(0.0, shape.detJ);

    // check setZero() for all
    setShapeDataToOnes(shape);
    shape.setZero();
    EXPECT_TRUE(shape.N.isZero());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isZero());
    EXPECT_TRUE(shape.J.isZero());
    EXPECT_TRUE(shape.invJ.isZero());
    EXPECT_EQ(0.0, shape.detJ);

    // check setZero() only for N
    setShapeDataToOnes(shape);
    shape.setZero<ShapeMatrixType::N>();
    EXPECT_TRUE(shape.N.isZero());
    EXPECT_TRUE(shape.dNdr.isOnes());
    EXPECT_TRUE(shape.dNdx.isOnes());
    EXPECT_TRUE(shape.J.isOnes());
    EXPECT_TRUE(shape.invJ.isOnes());
    EXPECT_EQ(1.0, shape.detJ);

    // check setZero() for dNdr
    setShapeDataToOnes(shape);
    shape.setZero<ShapeMatrixType::DNDR>();
    EXPECT_TRUE(shape.N.isOnes());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isOnes());
    EXPECT_TRUE(shape.J.isOnes());
    EXPECT_TRUE(shape.invJ.isOnes());
    EXPECT_EQ(1.0, shape.detJ);

    // check setZero() for N, dNdr, and J
    setShapeDataToOnes(shape);
    shape.setZero<ShapeMatrixType::N_J>();
    EXPECT_TRUE(shape.N.isZero());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isOnes());
    EXPECT_TRUE(shape.J.isZero());
    EXPECT_TRUE(shape.invJ.isOnes());
    EXPECT_EQ(0.0, shape.detJ);

    // check setZero() for dNdr, and J
    setShapeDataToOnes(shape);
    shape.setZero<ShapeMatrixType::DNDR_J>();
    EXPECT_TRUE(shape.N.isOnes());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isOnes());
    EXPECT_TRUE(shape.J.isZero());
    EXPECT_TRUE(shape.invJ.isOnes());
    EXPECT_EQ(0.0, shape.detJ);

    // check setZero() only for DNDX stuff
    setShapeDataToOnes(shape);
    shape.setZero<ShapeMatrixType::DNDX>();
    EXPECT_TRUE(shape.N.isOnes());
    EXPECT_TRUE(shape.dNdr.isZero());
    EXPECT_TRUE(shape.dNdx.isZero());
    EXPECT_TRUE(shape.J.isZero());
    EXPECT_TRUE(shape.invJ.isZero());
    EXPECT_EQ(0.0, shape.detJ);
}


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::is_scalar_distribution.hpp  //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  Use, modification and distribution are subject to the                           //
//  Boost Software License, Version 1.0. (See accompanying file                     //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                //
//////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_IS_SCALAR_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_IS_SCALAR_DISTRIBUTION_HPP_ER_2009
#include <boost/mpl/bool.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_scalar_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

namespace meta{

    template<typename Z>
    struct is_scalar_distribution< location_scale_distribution<Z> >
        : is_scalar_distribution<Z>{};

}// meta
}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2018 The Pandora developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 30000000 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 100000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 2000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 1 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 2000; nHeight < 40000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 2 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 40000; nHeight < 60000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;

    }
    for (int nHeight = 60000; nHeight < 80000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 7 * COIN);
        nSum += nSubsidy;

    }
	    for (int nHeight = 80000; nHeight < 1000000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 5 * COIN);
        nSum += nSubsidy;

    }
	BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);
}

BOOST_AUTO_TEST_SUITE_END()


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program is distributed in the hope that it will be useful,            *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of             *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

/** \file ErrorTermWheelTest.cpp
    \brief This file tests the ErrorTermWheel class.
  */

#include <cmath>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/ScalarExpression.hpp>

#include "aslam/calibration/error-terms/ErrorTermWheel.h"

using namespace aslam::backend;
using namespace aslam::calibration;

TEST(AslamCalibrationTestSuite, testErrorTermWheel) {

  /*ErrorTermWheel(const aslam::backend::EuclideanExpression& w_v_mw, const
          aslam::backend::ScalarExpression& r, double measurement, const
          Covariance& sigma2Wheel);*/
  // linear velocity
  const Eigen::Matrix<double, 3, 1> v(1.5, 1.7, 2.8);
  auto v_dv = boost::make_shared<EuclideanPoint>(v);
  EuclideanExpression v_exp(v_dv);

  // radius
  auto r_dv = boost::make_shared<Scalar>(1.6);
  ScalarExpression r_exp(r_dv);

  // error term wheel
  ErrorTermWheel ew(v_exp, r_exp, 1.5, 1);

  // test the error terms
  try {
    ErrorTermTestHarness<2> harness(&ew);
    harness.testAll(1e-5);
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }

}


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * hier-encoding.cpp
 *
 *  Created on: Dec 2, 2014
 *      Author: David A. Boyuka II
 */

#include <set>
#include <boost/iterator/counting_iterator.hpp>

#include "pique/encoding/hier/hier-encoding.hpp"

// exclusive upper bound
using bin_id_t = IndexEncoding::bin_id_t;
static void build_hierarchical_range(bin_id_t bin_ub, std::set< bin_id_t > &bins) {
	bin_ub = (bin_ub - 1 + 1); // bin_ub must become inclusive (-1), and must also become 1-based (+1)

	bin_id_t bitmask = 1;
	while (bin_ub) {
		if (bin_ub & bitmask) {
			bins.insert(bin_ub - 1 /* convert back to 0-based */);
			bin_ub -= bitmask;
		}
		bitmask <<= 1;
	}
}

template<typename ElemT>
static void inplace_symmetric_difference(std::set< ElemT > &s1, std::set< ElemT > &s2) {
	std::set< ElemT > s12;
	std::set_intersection(s1.begin(), s1.end(), s2.begin(), s2.end(), std::inserter(s12, s12.end()));
	for (const ElemT &e : s12) {
		s1.erase(e);
		s2.erase(e);
	}
}

auto HierarchicalIndexEncoding::get_region_math_impl(bin_count_t nbins, bin_id_t lb, bin_id_t ub, bool prefer_complement) const -> RMath {
	using regid_t = region_id_t;

	// If we reach here, at least one and at most num_bins-1 bins are selected
	const bool has_subtractive_range = lb > 0;
	const bool has_additive_range = ub < nbins;

	// Set up to read both the additive and subtractive bins
	std::set< region_id_t > additive_regions, subtractive_regions;
	if (has_additive_range)
		build_hierarchical_range(ub, additive_regions);
	if (has_subtractive_range)
		build_hierarchical_range(lb, subtractive_regions);

	// If we have both additive and subtractive bins, don't bother reading or
	// processing those bins that appear as both
	if (has_additive_range && has_subtractive_range)
		inplace_symmetric_difference(additive_regions, subtractive_regions);

	RMath rmath;
	if (has_additive_range) {
		for (bin_id_t ar : additive_regions) rmath.push_region((regid_t)ar);
		rmath.push_op(NArySetOperation::UNION);

		if (has_subtractive_range) {
			for (bin_id_t sr : subtractive_regions) rmath.push_region((regid_t)sr);
			rmath.push_op(NArySetOperation::DIFFERENCE, 1 + subtractive_regions.size());
		}
	} else {
		assert(has_subtractive_range); // Otherwise first == 0 and second == nbins, which is not allowed
		for (bin_id_t sr : subtractive_regions) rmath.push_region((regid_t)sr);
		rmath.push_op(NArySetOperation::UNION)
		     .push_op(UnarySetOperation::COMPLEMENT);
	}
	return rmath;
}

auto HierarchicalIndexEncoding::get_encoded_region_definitions_impl(bin_count_t nbins) const -> std::vector< bin_id_vector_t > {
	const region_count_t nregions = nbins - 1;

	std::vector< bin_id_vector_t > enc_region_defs;

	for (region_id_t i = 0; i < nregions; i ++) {
		const region_id_t bins_to_merge_end = i + 1;

		// Find the least-sig. 1-bit in bins_to_merge_end
		region_count_t bins_to_merge_len;
		for (bins_to_merge_len = 1; (bins_to_merge_end & bins_to_merge_len) == 0; bins_to_merge_len <<= 1);

		const region_id_t bins_to_merge_begin = bins_to_merge_end - bins_to_merge_len;

		bin_id_vector_t enc_region_def;
		enc_region_def.insert(
			enc_region_def.end(),
			boost::counting_iterator< bin_id_t >(bins_to_merge_begin),
			boost::counting_iterator< bin_id_t >(bins_to_merge_end));

		enc_region_defs.push_back(std::move(enc_region_def));

	}

	return enc_region_defs;
}

auto HierarchicalIndexEncoding::get_encoded_regions_impl(region_vector_t bins, const AbstractSetOperations& setops) const -> region_vector_t {
	const region_count_t nregions = bins.size() - 1;

	region_vector_t enc_regions;

	for (region_id_t i = 0; i < nregions; i ++) {
		region_vector_t enc_regions_to_merge(1, bins[i]); // Always include the ith bin to merge into this encoded region

		// For every trailing 1 in the binary representation of the current region ID, we need
		// to merge with another region (the ID of which is computed by masking out one of these 1s)
		// Example: region ID 10111 needs to be merged with 10110, 10101 and 10011 (all numbers in binary)
		region_id_t trailing_one_mask = 1;
		while (i & trailing_one_mask) {
			region_id_t merge_region_id = i & ~trailing_one_mask;
			trailing_one_mask <<= 1;

			enc_regions_to_merge.push_back(enc_regions[merge_region_id]); // Get the region from the NEW index (we need merged bins)
		}

		boost::shared_ptr< RegionEncoding > enc_region =
			setops.dynamic_nary_set_op(
				enc_regions_to_merge.begin(),
				enc_regions_to_merge.end(),
				NArySetOperation::UNION
			);

		enc_regions.push_back(enc_region);
	}

	return enc_regions;
}


//
// Created by Kamil Saigol on 3/03/16.
//

#include "RRTsharp.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
#include "ompl/tools/config/MagicConstants.h"
#include "ompl/util/GeometricEquations.h"

#include <boost/math/constants/constants.hpp>

#include <algorithm>
#include <cmath>
// cpp11
#include <functional>
#include <limits>

ompl::RRTsharp::RRTsharp(const ompl::base::SpaceInformationPtr &si)
: ompl::base::Planner(si, "RRTsharp"),
  goalBias_(0.05),
  maxDistance_(0.0),
  lastGoalMotion_(NULL),
  multiplier_(1.1),
  q_(motion_compare(opt_)),
  knn_(false)
{
    specs_.approximateSolutions = true;
    specs_.directed = true;
    specs_.optimizingPaths = true;

    freeSpaceVolume_ = si_->getStateSpace()->getMeasure();

    ompl::base::Planner::declareParam<double>("range", this, &RRTsharp::setRange, &RRTsharp::getRange, "0.:1.:10000.");
    ompl::base::Planner::declareParam<double>("goal_bias", this, &RRTsharp::setGoalBias, &RRTsharp::getGoalBias, "0.:.05:1.");
    ompl::base::Planner::declareParam<double>("multiplier", this, &RRTsharp::setMultiplier, &RRTsharp::getMultiplier, "0.1:0.05:50.");
    ompl::base::Planner::declareParam<bool>("use_kNN_search", this, &RRTsharp::setKnn, &RRTsharp::getKnn, "0,1");
    ompl::base::Planner::declareParam<int>("variant", this, &RRTsharp::setVariant, &RRTsharp::getVariant, "0:3");

    addPlannerProgressProperty("iterations INTEGER", boost::bind(&RRTsharp::numIterationsProperty, this));
    addPlannerProgressProperty("best cost REAL", boost::bind(&RRTsharp::bestCostProperty, this));
}

ompl::RRTsharp::~RRTsharp()
{
    freeMemory();
}

void ompl::RRTsharp::getPlannerData(ompl::base::PlannerData &data) const
{
    ompl::base::Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(ompl::base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(ompl::base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(ompl::base::PlannerDataVertex(motions[i]->parent->state),
                         ompl::base::PlannerDataVertex(motions[i]->state));
    }
}

void ompl::RRTsharp::setup()
{
    ompl::base::Planner::setup();
    tools::SelfConfig sc(si_, getName());

    if (!pdef_ || !pdef_->hasOptimizationObjective())
    {
        OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
        opt_.reset(new ompl::base::PathLengthOptimizationObjective(si_));
    }
    else
        opt_ = pdef_->getOptimizationObjective();

    if (maxDistance_ < std::numeric_limits<double>::epsilon())
    {
        sc.configurePlannerRange(maxDistance_);
        maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
    }

    if (!nn_)
    {
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
    }

    // cpp11
    nn_->setDistanceFunction(std::bind(&RRTsharp::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));

    bestCost_ = opt_->infiniteCost();
    bestMotion_ = NULL;
    iterations_ = 0;
}

ompl::base::PlannerStatus
ompl::RRTsharp::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
    // Error checking
    checkValidity();

    // Goal information
    ompl::base::Goal                 *goal   = pdef_->getGoal().get();
    ompl::base::GoalSampleableRegion *goalRegion = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal);

    // Loop through valid input states and add to tree
    while (const ompl::base::State *state = pis_.nextStart())
    {
        // Allocate memory for a new start state motion based on the "space-information"-size
        Motion *motion = new Motion(si_);

        // Copy destination <= source
        si_->copyState(motion->state, state);

        // Set cost for this start state
        motion->g = opt_->initialCost(motion->state);

        if (nn_->size() == 0)  // do not overwrite best from previous call to solve
        {
            bestCost_ = motion->g;
            bestMotion_ = motion;
        }

        // Add start motion to the tree
        nn_->add(motion);
    }

    // Check that input states exist
    if (nn_->size() == 0)
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return ompl::base::PlannerStatus::INVALID_START;
    }

    // Create state sampler if this is the first run
    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    // Output start information
    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    // solution
    Motion *solution  = NULL;

    // approximate solution returned if no exact solution found
    Motion *approxSolution = NULL;

    // distance from goal to closest solution yet found
    double approxDifference = std::numeric_limits<double>::infinity();

    // distance between states - the intial state and the interpolated state (may be the same)
    double rDistance;

    // Create random motion and motion representing nn
    Motion *mrand   = new Motion(si_);
    Motion *mnearest;

    // STATES

    // Random state
    ompl::base::State *xrand = mrand->state;

    // New interpolated state
    ompl::base::State *interpolatedState = si_->allocState();

    // State resulting from steering
    ompl::base::State *xnew;

    // Key comparator
    key_compare kc(opt_);

    // ----- Begin sampling -----
    while (ptc() == false)
    {
        iterations_++;
        // I. xrand = Sample(k)

        // Sample random state (with goal biasing probability)
        if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
        {
            // Bias sample towards goal
            goalRegion->sampleGoal(xrand);
        }
        else
        {
            // Uniformly Sample
            sampler_->sampleUniform(xrand);
        }

        /** EXTEND **/

        // II. xnearest = Nearest(xrand)

        // Find closest state in the tree
        mnearest = nn_->nearest(mrand);

        // III. xnew = Steer(xnearest, xrand)

        // Distance from near state q_n to a random state
        rDistance = si_->distance(mnearest->state, xrand);

        // Check if the rand_state is too far away
        if (rDistance > maxDistance_)
        {
            // Interpolate state
            si_->getStateSpace()->interpolate(mnearest->state, xrand,
                                              maxDistance_ / rDistance, interpolatedState);

            // Update the distance between near and new with the interpolated_state
            rDistance = si_->distance(mnearest->state, interpolatedState);

            // Use the interpolated state as the new state
            xnew = interpolatedState;
        }
        else  // Random state is close enough
        {
            xnew = xrand;
        }

        // IV. If ObstacleFree(xnearest, xnew)
        if (!si_->checkMotion(mnearest->state, xnew))
            continue; // try a new sample

        // V. Initialize(xnew, xnearest)
        // first create a motion
        Motion *mnew = new Motion(si_);
        si_->copyState(mnew->state, xnew);

        initialize(mnew, nullptr); // cpp11

        // VI. Xnear = Near(xnew, |V|)

        // storage for a set of nearest neighbors
        std::vector<Motion*> Xnear;

        if(!knn_)
        {
            // radius within which to fetch neighbors
            double radius = calculateRadius(si_->getStateDimension(), nn_->size());

            // nearest neighbors in radius
            nn_->nearestR(mnew, radius, Xnear);
        }
        else
        {
            unsigned int k = calculateKnn(nn_->size());
            nn_->nearestK(mnew, k, Xnear);
        }

        // VII. foreach(xnear in Xnear)
        for(std::size_t xnear = 0; xnear < Xnear.size(); ++xnear)
        {
            Motion *mnear = Xnear.at(xnear);

            // if ObstacleFree(xnear, xnew)
            if (!si_->checkMotion(mnear->state, mnew->state))
                continue;

            // calculate c and g values
            mnew->c = opt_->motionCost(mnear->state, mnew->state);
            mnew->g = opt_->combineCosts(mnear->g, mnew->c);

            // if(lmc(xnew) > g(xnear) + c(xnew, xnear)
            if(opt_->isCostBetterThan(mnew->g, mnew->lmc))
            {
                mnew->lmc = mnew->g;
                mnew->parent = mnear;
            }

            // Cycle
            mnear->children.push_back(mnew);
            mnew->children.push_back(mnear);
        }

        // Dynamic Programming Guided Exploration
        bool to_include = includeVertex(mnew);

        // VIII. Add motion
        if(to_include) nn_->add(mnew);
        else continue; // try a new sample

        // IX. UpdateQueue(xnew)
        updateQueue(mnew);

        // X. Update the current best cost and motion if necessary
        if (opt_->isCostBetterThan(mnew->lmc, bestCost_))
        {
            bestCost_ = mnew->lmc;
            bestMotion_ = mnew;
        }

        /** REPLAN **/

        // XI. while(q.findmin() < Key(v*)
        while(1)
        {
            if(q_.empty())
                break;

            Motion* min = q_.top();

            if(!kc(min->key, bestMotion_->key))
                break;

            min->g = min->lmc;

            q_.erase(min->handle);

            for(std::size_t succ = 0; succ < min->children.size(); ++succ)
            {
                Motion *s = min->children.at(succ);
                s->c = opt_->motionCost(min->state, s->state);
                ompl::base::Cost lmc = opt_->combineCosts(min->g, s->c);
                if(opt_->isCostBetterThan(lmc, s->lmc))
                {
                    s->lmc = lmc;
                    s->parent = min;
                    updateQueue(s);
                }
            }
        }

        // Check if this motion is the goal
        double distToGoal = 0.0;
        bool isSatisfied = goal->isSatisfied(mnew->state, &distToGoal);
        if (isSatisfied)
        {
            approxDifference = distToGoal; // state-to-goal error tolerance
            solution = mnew;
            break;
        }

        // Check if this is the best solution so far
        if (distToGoal < approxDifference)
        {
            approxDifference = distToGoal;
            approxSolution = mnew;
        }
    }

    // ----- Finish solution processing -----

    bool solved = false;
    bool approximate = false;

    // Substitute an empty solution with the best approximation
    if (solution == NULL)
    {
        solution = approxSolution;
        approximate = true;
    }

    // Generate solution path for real/approx solution
    if (solution != NULL)
    {
        lastGoalMotion_ = solution;

        // construct the solution path
        std::vector<Motion*> mpath;
        while (solution != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent;
        }

        // set the solution path
        geometric::PathGeometric *path = new geometric::PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            path->append(mpath[i]->state);

        pdef_->addSolutionPath(ompl::base::PathPtr(path), approximate, approxDifference, getName());
        solved = true;
    }

    // ----- Clean up -----

    si_->freeState(interpolatedState);
    if (mrand->state)
        si_->freeState(mrand->state);
    delete mrand;

    OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());

    return ompl::base::PlannerStatus(solved, approximate);
}

bool ompl::RRTsharp::includeVertex(const Motion *x) const
{
    static key_compare kc(opt_);
    switch(getVariant())
    {
        case 1:
        {
            return (opt_->isCostBetterThan(x->key.first,  opt_->infiniteCost()) &&
                    opt_->isCostBetterThan(x->key.second, opt_->infiniteCost()));
            break;
        }
        case 2:
        {
            if(x->parent)
                return kc(x->parent->key, bestMotion_->key);
            else return true; // when no parent assigned
            break;
        }
        case 3:
        {
            return kc(x->key, bestMotion_->key);
            break;
        }
        default: // assume 0
            return true;
            break;
    }
}

void ompl::RRTsharp::clear()
{
    ompl::base::Planner::clear();
    sampler_.reset();
    freeMemory();
    if (nn_)
    {
        nn_->clear();
    }
    lastGoalMotion_ = NULL;

    if (opt_)
    {
        bestCost_ = opt_->infiniteCost();
        bestMotion_ = NULL;
    }
    iterations_ = 0;
}

void ompl::RRTsharp::initialize(Motion *x, Motion *x_pr)
{
    x->g = opt_->infiniteCost();
    x->lmc = opt_->infiniteCost();
    x->parent = x_pr;
    if(x_pr)
    {
        x->c = opt_->motionCost(x_pr->state, x->state);
        x->lmc = opt_->combineCosts(x_pr->g, x->c);
        x_pr->children.push_back(x);
    }
}

void ompl::RRTsharp::updateQueue(Motion *x)
{
    bool equal_cost = opt_->isCostEquivalentTo(x->g, x->lmc);

    if(equal_cost)
    {
        x->key = key(x);
        if(x->handle.node_)
        {
            q_.update(x->handle);
        }
        else
        {
            x->handle = q_.push(x);
        }
    }
    else
    {
        if(x->handle.node_)
        {
            q_.erase(x->handle);
        }
    }
}

ompl::RRTsharp::key_type ompl::RRTsharp::key(Motion *x) const
{
    ompl::base::Cost k1 = opt_->combineCosts(x->lmc, heuristicValue(x));
    return std::make_pair(k1, x->lmc);
}

unsigned int ompl::RRTsharp::calculateKnn(const unsigned int nsize) const
{
    return std::ceil(multiplier_
             * (boost::math::constants::e<double>() + (boost::math::constants::e<double>() / static_cast<double>(si_->getStateDimension())))
             * std::log(static_cast<double>(nsize + 1u)));
}

double ompl::RRTsharp::calculateRadius(const unsigned int dimension, const unsigned int n) const
{
    double a = 1.0 / (double)dimension;
    double unitBallVolume = ompl::unitNBallMeasure(dimension);

    return multiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) * std::pow(std::log((double)n) / (double)n, a);
}

double ompl::RRTsharp::distanceFunction(const Motion *a, const Motion *b) const
{
    return si_->distance(a->state, b->state);
}

ompl::base::Cost ompl::RRTsharp::heuristicValue(Motion *v) const
{
    return opt_->costToGo(v->state, pdef_->getGoal().get());
}

void ompl::RRTsharp::freeMemory()
{
    if (nn_)
    {
        std::vector<Motion*> motions;
        nn_->list(motions);
        for (unsigned int i = 0; i < motions.size(); ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }
}