// Copyright (C) 2008  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/geometry.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/string.h>
#include <dlib/matrix.h>
#include <dlib/rand.h>
#include <dlib/array2d.h>
#include <dlib/image_transforms.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.geometry");

    void geometry_test (
    )
    /*!
        ensures
            - runs tests on the geometry stuff compliance with the specs
    !*/
    {
        print_spinner();

        point p1;
        point p2(2,3);

        DLIB_TEST(p1.x() == 0);
        DLIB_TEST(p1.y() == 0);
        DLIB_TEST(p2.x() == 2);
        DLIB_TEST(p2.y() == 3);

        DLIB_TEST((-p2).x() == -2);
        DLIB_TEST((-p2).y() == -3);

        p2 += p2;
        DLIB_TEST(p2.x() == 4);
        DLIB_TEST(p2.y() == 6);

        dlib::vector<double> v1 = point(1,0);
        dlib::vector<double> v2(0,0,1);

        p1 = v2.cross(v1);
        DLIB_TEST(p1 == point(0,1));
        DLIB_TEST(p1 != point(1,1));
        DLIB_TEST(p1 != point(1,0));

        p1 = point(2,3);
        rectangle rect1 = p1;
        DLIB_TEST(rect1.width() == 1);
        DLIB_TEST(rect1.height() == 1);
        p2 = point(1,1);

        rect1 += p2;
        DLIB_TEST(rect1.left() == 1);
        DLIB_TEST(rect1.top() == 1);
        DLIB_TEST(rect1.right() == 2);
        DLIB_TEST(rect1.bottom() == 3);

        DLIB_TEST(rect1.width() == 2);
        DLIB_TEST(rect1.height() == 3);

        // test the iostream << and >> operators (via string_cast and cast_to_string)
        DLIB_TEST(string_cast<point>(" (1, 2 )") == point(1,2));
        DLIB_TEST(string_cast<point>(" ( -1, 2 )") == point(-1,2));
        DLIB_TEST(string_cast<rectangle>(" [(1, 2 )(3,4)]") == rectangle(1,2,3,4));
        DLIB_TEST(string_cast<dlib::vector<double> >(" (1, 2 , 3.5)") == dlib::vector<double>(1,2,3.5));

        DLIB_TEST(string_cast<rectangle>(cast_to_string(rect1)) == rect1);
        DLIB_TEST(string_cast<point>(cast_to_string(p1)) == p1);
        DLIB_TEST(string_cast<dlib::vector<double> >(cast_to_string(v1)) == v1);

        rectangle rect2;

        // test the serialization code
        ostringstream sout;
        serialize(rect1,sout);
        serialize(p1,sout);
        serialize(v1,sout);
        serialize(rect1,sout);
        serialize(p1,sout);
        serialize(v1,sout);

        istringstream sin(sout.str());

        deserialize(rect2,sin);
        deserialize(p2,sin);
        deserialize(v2,sin);
        DLIB_TEST(rect2 == rect1);
        DLIB_TEST(p2 == p1);
        DLIB_TEST(v2 == v1);
        deserialize(rect2,sin);
        deserialize(p2,sin);
        deserialize(v2,sin);
        DLIB_TEST(rect2 == rect1);
        DLIB_TEST(p2 == p1);
        DLIB_TEST(v2 == v1);
        DLIB_TEST(sin.good());
        DLIB_TEST(sin.get() == EOF);

        v1.x() = 1;
        v1.y() = 2;
        v1.z() = 3;

        matrix<double> mv = v1;
        DLIB_TEST(mv.nr() == 3);
        DLIB_TEST(mv.nc() == 1);
        DLIB_TEST(mv(0) == 1);
        DLIB_TEST(mv(1) == 2);
        DLIB_TEST(mv(2) == 3);

        set_all_elements(mv,0);
        DLIB_TEST(mv(0) == 0);
        DLIB_TEST(mv(1) == 0);
        DLIB_TEST(mv(2) == 0);

        mv(0) = 5;
        mv(1) = 6;
        mv(2) = 7;

        v1 = mv;
        DLIB_TEST(v1.x() == 5);
        DLIB_TEST(v1.y() == 6);
        DLIB_TEST(v1.z() == 7);

        {
            dlib::vector<double,2> vd2;
            dlib::vector<double,3> vd3;
            dlib::vector<long,2> vl2;
            dlib::vector<long,3> vl3;

            vd2.x() = 2.3;
            vd2.y() = 4.7;

            vd3.z() = 9;

            vd3 = vd2;

            vl2 = vd3;
            vl3 = vd3;

            DLIB_TEST(vd2.z() == 0);
            DLIB_TEST(vd3.z() == 0);
            DLIB_TEST(vl2.z() == 0);
            DLIB_TEST(vl3.z() == 0);

            DLIB_TEST(vl2.x() == 2);
            DLIB_TEST(vl3.x() == 2);
            DLIB_TEST(vl2.y() == 5);
            DLIB_TEST(vl3.y() == 5);

            DLIB_TEST(abs(vd2.cross(vd3).dot(vd2)) < 1e-7);
            DLIB_TEST(abs(vd3.cross(vd2).dot(vd2)) < 1e-7);
            DLIB_TEST(abs(vd2.cross(vd3).dot(vd3)) < 1e-7);
            DLIB_TEST(abs(vd3.cross(vd2).dot(vd3)) < 1e-7);

            DLIB_TEST(abs(vl2.cross(vl3).dot(vl2)) == 0);
            DLIB_TEST(abs(vl3.cross(vl2).dot(vl2)) == 0);
            DLIB_TEST(abs(vl2.cross(vl3).dot(vl3)) == 0);
            DLIB_TEST(abs(vl3.cross(vl2).dot(vl3)) == 0);

            DLIB_TEST((vd2-vd3).length() < 1e-7);

            DLIB_TEST(vl2 == vl3);

            vl2.x() = 0;
            vl2.y() = 0;
            vl3 = vl2;

            vl2.x() = 4;
            vl3.y() = 3;

            DLIB_TEST(vl2.cross(vl3).length() == 12);
            DLIB_TEST(vl3.cross(vl2).length() == 12);

            matrix<double> m(3,3);
            m = 1,2,3,
                4,5,6,
                7,8,9;

            vd3.x() = 2;
            vd3.y() = 3;
            vd3.z() = 4;

            vd3 = m*vd3;

            DLIB_TEST_MSG(vd3.x() == 1*2 + 2*3 + 3*4,vd3.x() << " == " << (1*2 + 2*3 + 3*4));
            DLIB_TEST(vd3.y() == 4*2 + 5*3 + 6*4);
            DLIB_TEST(vd3.z() == 7*2 + 8*3 + 9*4);

            (vd3*2).dot(vd3);
            (vd2*2).dot(vd3);
            (vd3*2).dot(vd2);
            (vd2*2).dot(vd2);
            (2*vd3*2).dot(vd3);
            (2*vd2*2).dot(vd3);
            (2*vd3*2).dot(vd2);
            (2*vd2*2).dot(vd2);

            (vd2 + vd3).dot(vd2);
            (vd2 - vd3).dot(vd2);
            (vd2/2).dot(vd2);
            (vd3/2).dot(vd2);
        }

        {
            dlib::vector<double,2> vd2;
            dlib::vector<long,3> vl3;

            vl3.x() = 1;
            vl3.y() = 2;
            vl3.z() = 3;

            vd2.x() = 6.5;
            vd2.y() = 7.5;

            DLIB_TEST((vl3 + vd2).x() == 1+6.5);
            DLIB_TEST((vl3 + vd2).y() == 2+7.5);
            DLIB_TEST((vl3 + vd2).z() == 3+0);

            DLIB_TEST((vl3 - vd2).x() == 1-6.5);
            DLIB_TEST((vl3 - vd2).y() == 2-7.5);
            DLIB_TEST((vl3 - vd2).z() == 3-0);

        }

        {
            dlib::vector<double> v(3,4,5);
            DLIB_TEST((-v).x() == -3.0);
            DLIB_TEST((-v).y() == -4.0);
            DLIB_TEST((-v).z() == -5.0);
        }

        {
            rectangle rect;

            point tl(2,3);
            point tr(8,3);
            point bl(2,9);
            point br(8,9);

            rect += tl;
            rect += tr;
            rect += bl;
            rect += br;

            DLIB_TEST(rect.tl_corner() == tl);
            DLIB_TEST(rect.tr_corner() == tr);
            DLIB_TEST(rect.bl_corner() == bl);
            DLIB_TEST(rect.br_corner() == br);

        }

        {
            point p1, center;

            center = point(3,4);
            p1 = point(10,4);

            DLIB_TEST(rotate_point(center, p1, pi/2) == point(3,7+4));

            center = point(3,3);
            p1 = point(10,3);

            DLIB_TEST(rotate_point(center, p1, pi/4) == point(8,8));
            DLIB_TEST(rotate_point(center, p1, -pi/4) == point(8,-2));

            DLIB_TEST(rotate_point(center, p1, pi/4 + 10*pi) == point(8,8));
            DLIB_TEST(rotate_point(center, p1, -pi/4 + 10*pi) == point(8,-2));
            DLIB_TEST(rotate_point(center, p1, pi/4 - 10*pi) == point(8,8));
            DLIB_TEST(rotate_point(center, p1, -pi/4 - 10*pi) == point(8,-2));

            point_rotator rot(pi/2);
            DLIB_TEST(rot(point(1,0)) == point(0,1));
            DLIB_TEST(rot(point(0,1)) == point(-1,0));
            DLIB_TEST(point(rot.get_m()*(dlib::vector<double,2>(1,0))) == point(0,1));
            DLIB_TEST(point(rot.get_m()*(dlib::vector<double,2>(0,1))) == point(-1,0));
        }

        {
            rectangle rect;

            rect = grow_rect(rect,1);
            DLIB_TEST(rect.width() == 2);
            DLIB_TEST(rect.height() == 2);
            DLIB_TEST(rect.left() == -1);
            DLIB_TEST(rect.top() == -1);
            DLIB_TEST(rect.right() == 0);
            DLIB_TEST(rect.bottom() == 0);
        }
        {
            rectangle rect;

            rect = grow_rect(rect,2);
            DLIB_TEST(rect.width() == 4);
            DLIB_TEST(rect.height() == 4);
            DLIB_TEST(rect.left() == -2);
            DLIB_TEST(rect.top() == -2);
            DLIB_TEST(rect.right() == 1);
            DLIB_TEST(rect.bottom() == 1);

            rect = shrink_rect(rect,1);
            DLIB_TEST(rect.width() == 2);
            DLIB_TEST(rect.height() == 2);
            DLIB_TEST(rect.left() == -1);
            DLIB_TEST(rect.top() == -1);
            DLIB_TEST(rect.right() == 0);
            DLIB_TEST(rect.bottom() == 0);
        }
        {
            std::vector< dlib::vector<double> > a;

            dlib::vector<double> v;
            dlib::rand rnd;

            for (int i = 0; i < 10; ++i)
            {
                v.x() = rnd.get_random_double();
                v.y() = rnd.get_random_double();
                v.z() = rnd.get_random_double();
                a.push_back(v);

            }

            // This test is just to make sure the covariance function can compile when used
            // on a dlib::vector.  The actual test doesn't matter.
            DLIB_TEST(sum(covariance(mat(a))) < 10);

        }

        DLIB_TEST(rectangle() + point(5,4) + point(10,10) == rectangle(5,4,10,10));

        // make sure the center of a centered rectangle is always right
        for (long x = -10; x <= 10; ++x)
        {
            for (long y = -10; y <= 10; ++y)
            {
                for (long w = 0; w < 10; ++w)
                {
                    for (long h = 0; h < 10; ++h)
                    {
                        DLIB_TEST(center(centered_rect(x,y,w,h)) == point(x,y));
                    }
                }
            }
        }

        {
            const rectangle input(1,1,6,4);
            const rectangle output = set_rect_area(input,4*input.area());
            DLIB_TEST(output.area() == 4*input.area());
            DLIB_TEST(output.width() == 2*input.width());
            DLIB_TEST(output.height() == 2*input.height());

            const auto input_center = center(input);
            const auto output_center = center(output);
            DLIB_TEST(std::abs(input_center.x() - output_center.x()) <= 1);
            DLIB_TEST(std::abs(input_center.y() - output_center.y()) <= 1);
        }

    }

// ----------------------------------------------------------------------------------------

    void test_border_enumerator()
    {

        border_enumerator be;
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 0);
        DLIB_TEST(be.current_element_valid() == false);
        DLIB_TEST(be.move_next() == false);
        DLIB_TEST(be.at_start() == false);
        DLIB_TEST(be.current_element_valid() == false);
        DLIB_TEST(be.move_next() == false);
        DLIB_TEST(be.at_start() == false);
        DLIB_TEST(be.current_element_valid() == false);
        DLIB_TEST(be.size() == 0);

        be = border_enumerator(rectangle(4,4,4,4),1);
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 1);
        be = border_enumerator(rectangle(4,4,4,4),3);
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 1);
        be = border_enumerator(rectangle(4,4,4,4),0);
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 0);
        be = border_enumerator(rectangle(4,4,5,5),0);
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 0);
        be = border_enumerator(rectangle(4,4,5,5),1);
        DLIB_TEST(be.at_start() == true);
        DLIB_TEST(be.size() == 4);
        be = border_enumerator(rectangle(4,4,5,5),2);
        DLIB_TEST(be.size() == 4);
        be = border_enumerator(rectangle(4,4,6,6),1);
        DLIB_TEST(be.size() == 8);
        be = border_enumerator(rectangle(4,4,6,6),2);
        DLIB_TEST(be.size() == 9);
        be = border_enumerator(rectangle(4,4,6,6),3);
        DLIB_TEST(be.size() == 9);
        DLIB_TEST(be.at_start() == true);

        array2d<unsigned char> img, img2;
        for (int size = 1; size < 10; ++size)
        {
            for (int bs = 0; bs < 4; ++bs)
            {
                img.set_size(size,size);
                img2.set_size(size,size);

                assign_all_pixels(img, 1);
                assign_all_pixels(img2, 1);

                zero_border_pixels(img2, bs,bs);

                be = border_enumerator(get_rect(img),bs);
                DLIB_TEST(be.at_start() == true);
                DLIB_TEST(be.current_element_valid() == false);
                while (be.move_next())
                {
                    DLIB_TEST(be.at_start() == false);
                    DLIB_TEST(be.current_element_valid() == true);
                    DLIB_TEST_MSG(get_rect(img).contains(be.element()) == true,
                                 get_rect(img) << "   " << be.element()
                    );
                    const point p = be.element();
                    img[p.y()][p.x()] = 0;
                }
                DLIB_TEST(be.at_start() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.at_start() == false);

                DLIB_TEST(mat(img) == mat(img2));

            }
        }

        for (int size = 1; size < 10; ++size)
        {
            for (int bs = 0; bs < 4; ++bs)
            {
                img.set_size(size,size+5);
                img2.set_size(size,size+5);

                assign_all_pixels(img, 1);
                assign_all_pixels(img2, 1);

                zero_border_pixels(img2, bs,bs);

                const point shift(4,5);

                be = border_enumerator(translate_rect(get_rect(img),shift),bs);
                DLIB_TEST(be.at_start() == true);
                DLIB_TEST(be.current_element_valid() == false);
                while (be.move_next())
                {
                    DLIB_TEST(be.current_element_valid() == true);
                    DLIB_TEST(be.at_start() == false);
                    DLIB_TEST_MSG(get_rect(img).contains(be.element()-shift) == true,
                                 get_rect(img) << "   " << be.element()
                    );
                    const point p = be.element()-shift;
                    img[p.y()][p.x()] = 0;
                }
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.at_start() == false);

                DLIB_TEST(mat(img) == mat(img2));

            }
        }

        for (int size = 1; size < 10; ++size)
        {
            for (int bs = 0; bs < 4; ++bs)
            {
                img.set_size(size+2,size);
                img2.set_size(size+2,size);

                assign_all_pixels(img, 1);
                assign_all_pixels(img2, 1);

                zero_border_pixels(img2, bs,bs);

                const point shift(-4,5);

                be = border_enumerator(translate_rect(get_rect(img),shift),bs);
                DLIB_TEST(be.current_element_valid() == false);
                while (be.move_next())
                {
                    DLIB_TEST(be.current_element_valid() == true);
                    DLIB_TEST_MSG(get_rect(img).contains(be.element()-shift) == true,
                                 get_rect(img) << "   " << be.element()
                    );
                    const point p = be.element()-shift;
                    img[p.y()][p.x()] = 0;
                }
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);
                DLIB_TEST(be.move_next() == false);
                DLIB_TEST(be.current_element_valid() == false);

                DLIB_TEST(mat(img) == mat(img2));

            }
        }

        {
            matrix<bool,4,5> hits, truth;
            const rectangle rect = rectangle(1,1,4,3);

            border_enumerator be(rect, rectangle(2,2, 3, 3));
            DLIB_TEST(be.size() == 8);
            hits = false;
            while (be.move_next())
            {
                DLIB_TEST(rect.contains(be.element()));
                hits(be.element().y(), be.element().x()) = true;
            }
            DLIB_TEST(be.current_element_valid() == false);
            DLIB_TEST(be.size() == 8);
            truth = false;
            truth(1,1) = truth(1,2) = truth(1,3) = truth(1,4) = truth(2,1) =
                truth(3,1) = truth(2,4) = truth(3,4) = true;
            DLIB_TEST_MSG(truth == hits, truth << endl << hits);

            be = border_enumerator(rect, rectangle(0,0, 9, 9));
            DLIB_TEST(be.size() == 0);
            hits = false;
            while (be.move_next())
            {
                DLIB_TEST(rect.contains(be.element()));
                hits(be.element().y(), be.element().x()) = true;
            }
            DLIB_TEST(be.current_element_valid() == false);
            DLIB_TEST(be.size() == 0);
            truth = false;
            DLIB_TEST(truth == hits);

            be = border_enumerator(rect, rectangle(0,0, 3, 9));
            DLIB_TEST(be.size() == 3);
            hits = false;
            while (be.move_next())
            {
                DLIB_TEST(rect.contains(be.element()));
                hits(be.element().y(), be.element().x()) = true;
            }
            DLIB_TEST(be.current_element_valid() == false);
            DLIB_TEST(be.size() == 3);
            truth = false;
            truth(1,4) = truth(2,4) = truth(3,4) = true;
            DLIB_TEST(truth == hits);

            be = border_enumerator(rect, rectangle(2,1, 4, 3));
            DLIB_TEST(be.size() == 3);
            hits = false;
            while (be.move_next())
            {
                DLIB_TEST(rect.contains(be.element()));
                hits(be.element().y(), be.element().x()) = true;
            }
            DLIB_TEST(be.current_element_valid() == false);
            DLIB_TEST(be.size() == 3);
            truth = false;
            truth(1,1) = truth(2,1) = truth(3,1) = true;
            DLIB_TEST(truth == hits);

            be = border_enumerator(rect, rectangle(1,1, 5, 2));
            DLIB_TEST(be.size() == 4);
            hits = false;
            while (be.move_next())
            {
                DLIB_TEST(rect.contains(be.element()));
                hits(be.element().y(), be.element().x()) = true;
            }
            DLIB_TEST(be.current_element_valid() == false);
            DLIB_TEST(be.size() == 4);
            truth = false;
            truth(3,1) = truth(3,2) = truth(3,3) = truth(3,4) = true;
            DLIB_TEST(truth == hits);

        }

    }

// ----------------------------------------------------------------------------------------

    void test_find_affine_transform()
    {
        //typedef dlib::vector<double,2> vect;
        typedef point vect;
        std::vector<vect> from, to;

        from.push_back(vect(0,0));
        to.push_back(vect(0,1));

        from.push_back(vect(0,1));
        to.push_back(vect(1,1));

        from.push_back(vect(1,1));
        to.push_back(vect(1,0));

        from.push_back(vect(1,0));
        to.push_back(vect(0,0));

        point_transform_affine t = find_affine_transform(from,to);
        point_transform_affine tinv = inv(t);

        for (unsigned long i = 0; i < from.size(); ++i)
        {
            dlog << LINFO << "affine transformation error: "<< length(t(from[i])-to[i]);
            DLIB_TEST(length(t(from[i])-to[i]) < 1e-14);
            DLIB_TEST(length(tinv(t(from[i]))-from[i]) < 1e-14);
            DLIB_TEST(length(t(tinv(from[i]))-from[i]) < 1e-14);

            point_transform_affine temp = t*inv(t);
            DLIB_TEST(length(temp.get_b()) < 1e-14);
            DLIB_TEST(max(abs(temp.get_m() - identity_matrix<double>(2))) < 1e-14);
        }

        ostringstream sout;
        serialize(t, sout);
        istringstream sin(sout.str());
        point_transform_affine t2;
        DLIB_TEST(length(t2(point(2,3)) - point(2,3)) < 1e-14);
        deserialize(t2, sin);
        DLIB_TEST(max(abs(t2.get_m()-t.get_m())) < 1e-14);
        DLIB_TEST(max(abs(t2.get_b()-t.get_b())) < 1e-14);
    }

// ----------------------------------------------------------------------------------------

    double projective_transform_pass_rate(const double error_rate)
    {
        print_spinner();
        dlog << LINFO << "projective_transform_pass_rate, error_rate: "<< error_rate;
        dlib::rand rnd;
        running_stats<double> pass_rate;
        for (int rounds = 0; rounds < 1000; ++rounds)
        {
            running_stats<double> rs, rs_true;
            matrix<double> H = 2*(randm(3,3,rnd)-0.5);

            H(0,2) = rnd.get_random_gaussian()*10;
            H(1,2) = rnd.get_random_gaussian()*10;

            H(2,0) = rnd.get_random_double()*2.1;
            H(2,1) = rnd.get_random_double()*2.1;
            H(2,2) = 1 + rnd.get_random_gaussian()*3.1;

            point_transform_projective tran(H);
            point_transform_projective traninv = inv(tran);

            const int num = rnd.get_random_32bit_number()%8 + 4;

            std::vector<dlib::vector<double,2> > from_points, to_points;
            for (int i = 0; i < num; ++i)
            {
                dlib::vector<double,2> p = randm(2,1,rnd)*1000;
                from_points.push_back(p);
                to_points.push_back(tran(p) + (randm(2,1,rnd)-0.5)*error_rate);
                DLIB_TEST(length(traninv(tran(p))-p) <= 1e-5);
                DLIB_TEST(length(tran(traninv(p))-p) <= 1e-5);

                point_transform_projective temp = tran*traninv;
                DLIB_TEST_MSG(max(abs(temp.get_m() - identity_matrix<double>(3))) < 1e-10, temp.get_m());
                temp = traninv*tran;
                DLIB_TEST_MSG(max(abs(temp.get_m() - identity_matrix<double>(3))) < 1e-10, temp.get_m());
            }

            point_transform_projective tran2 = find_projective_transform(from_points, to_points);

            for (unsigned long i = 0; i < from_points.size(); ++i)
            {
                const double err = length_squared(tran2(from_points[i]) - to_points[i]);
                rs.add(err);
                const double err_true = length_squared(tran(from_points[i]) - to_points[i]);
                rs_true.add(err_true);
            }

            if ( rs.mean() < 0.01)
            {
                pass_rate.add(1);
            }
            else
            {
                dlog << LINFO << " errors: mean/max: " << rs.mean() << "  " << rs.max();
                pass_rate.add(0);
            }

            ostringstream sout;
            serialize(tran, sout);
            istringstream sin(sout.str());
            point_transform_projective tran3;
            DLIB_TEST(length(tran3(point(2,3)) - point(2,3)) < 1e-14);
            deserialize(tran3, sin);
            DLIB_TEST(max(abs(tran3.get_m()-tran.get_m())) < 1e-14);
        }

        dlog << LINFO << " pass_rate.mean(): "<< pass_rate.mean();
        return pass_rate.mean();
    }

// ----------------------------------------------------------------------------------------

    template <typename T>
    void test_find_similarity_transform()
    {
        print_spinner();
        std::vector<dlib::vector<T,2> > from_points, to_points;

        from_points.push_back(dlib::vector<T,2>(0,0));
        from_points.push_back(dlib::vector<T,2>(0,1));
        from_points.push_back(dlib::vector<T,2>(1,0));

        to_points.push_back(dlib::vector<T,2>(8,0));
        to_points.push_back(dlib::vector<T,2>(6,0));
        to_points.push_back(dlib::vector<T,2>(8,2));

        point_transform_affine tform = find_similarity_transform(from_points, to_points);

        for (unsigned long i = 0; i < from_points.size(); ++i)
        {
            DLIB_TEST(length(tform(from_points[i]) - to_points[i]) < 1e-14);
        }
    }

    template <typename T>
    void test_find_similarity_transform2()
    {
        print_spinner();
        std::vector<dlib::vector<T,2> > from_points, to_points;

        from_points.push_back(dlib::vector<T,2>(0,0));
        from_points.push_back(dlib::vector<T,2>(0,1));

        to_points.push_back(dlib::vector<T,2>(8,0));
        to_points.push_back(dlib::vector<T,2>(6,0));

        point_transform_affine tform = find_similarity_transform(from_points, to_points);

        for (unsigned long i = 0; i < from_points.size(); ++i)
        {
            DLIB_TEST(length(tform(from_points[i]) - to_points[i]) < 1e-14);
        }
    }

// ----------------------------------------------------------------------------------------

    void test_rect_to_drect()
    {
        print_spinner();
        dlib::rand rnd;
        for (int i = 0; i < 5000; ++i)
        {
            rectangle rect = centered_rect(rnd.get_random_32bit_number()%100,
                rnd.get_random_32bit_number()%100,
                rnd.get_random_32bit_number()%100,
                rnd.get_random_32bit_number()%100);

            drectangle drect = rect;
            rectangle rect2 = drect;
            DLIB_TEST(rect2 == rect);
            DLIB_TEST(rect.width() == drect.width());
            DLIB_TEST(rect.height() == drect.height());
            DLIB_TEST(dcenter(rect) == dcenter(drect));
            DLIB_TEST(rect.is_empty() == drect.is_empty());
        }
    }

// ----------------------------------------------------------------------------------------

    void test_affine3d()
    {
        const dlib::vector<double> x(1,0,0);
        const dlib::vector<double> y(0,1,0);
        const dlib::vector<double> z(0,0,1);
        const dlib::vector<double> e(1,1,1);
        const dlib::vector<double> ex(-1,1,1);
        const dlib::vector<double> ey(1,-1,1);
        const dlib::vector<double> ez(1,1,-1);

        dlib::vector<double> w;

        w = rotate_around_z(pi/2)(x);
        DLIB_TEST(length(w-y) < 1e-12);
        w = rotate_around_z(pi/2)(e);
        DLIB_TEST(length(w-ex) < 1e-12);

        w = rotate_around_y(-pi/2)(x);
        DLIB_TEST(length(w-z) < 1e-12);
        w = rotate_around_y(pi/2)(e);
        DLIB_TEST(length(w-ez) < 1e-12);

        w = rotate_around_x(pi/2)(y);
        DLIB_TEST(length(w-z) < 1e-12);
        w = rotate_around_x(pi/2)(e);
        DLIB_TEST(length(w-ey) < 1e-12);

        w = translate_point(x)(y);
        DLIB_TEST(length(w-x-y) < 1e-12);

        point_transform_affine3d tform;
        tform = rotate_around_x(pi/2)*rotate_around_z(pi/2)*translate_point(x);
        DLIB_TEST(length(tform(dlib::vector<double>())-z) < 1e-12);
        DLIB_TEST(length(inv(tform)(z)) < 1e-12);

        point_transform_affine tform2;
        tform = tform*tform2;// the default tform is the identity mapping so this shouldn't do anything different
        DLIB_TEST(length(tform(dlib::vector<double>())-z) < 1e-12);
        DLIB_TEST(length(inv(tform)(z)) < 1e-12);
    }

// ----------------------------------------------------------------------------------------

    void test_line()
    {
        print_spinner();
        line l1(point(0,1),point(1,0));
        line l2(point(0,0),point(1,1));

        DLIB_TEST(length(intersect(l1,l2) - dpoint(0.5,0.5)) < 1e-12);

        DLIB_TEST(std::abs(distance_to_line(l1, l1.p1())) < 1e-12);

        DLIB_TEST(std::abs(distance_to_line(l1, dpoint(0,0)) - sqrt(0.5)) < 1e-12);
        DLIB_TEST(std::abs(distance_to_line(l1, dpoint(1,1)) - sqrt(0.5)) < 1e-12);

        DLIB_TEST(std::abs(signed_distance_to_line(l1, dpoint(0,0)) + sqrt(0.5)) < 1e-12);
        DLIB_TEST(std::abs(signed_distance_to_line(l1, dpoint(1,1)) - sqrt(0.5)) < 1e-12);
        DLIB_TEST(std::abs(signed_distance_to_line(reverse(l1), dpoint(1,1)) + sqrt(0.5)) < 1e-12);

        dpoint reference_point(0.7,0.5);
        std::vector<dpoint> pts = {dpoint(0.01,0), dpoint(1,0.5), dpoint(0.9, 0.3), dpoint(0.4, 2), dpoint(1.3, 0.9)};

        DLIB_TEST(count_points_between_lines(l1,l2,reference_point,pts) == 3);
        DLIB_TEST(count_points_on_side_of_line(l1,reference_point,pts) == 4);

        reference_point = dpoint(0,0.5);
        DLIB_TEST(count_points_between_lines(l1,l2,reference_point,pts) == 0);
        DLIB_TEST(count_points_on_side_of_line(l1,reference_point,pts) == 1);

    }

// ----------------------------------------------------------------------------------------

    class geometry_tester : public tester
    {
    public:
        geometry_tester (
        ) :
            tester ("test_geometry",
                    "Runs tests on the geometry stuff.")
        {}

        void perform_test (
        )
        {
            test_affine3d();
            test_rect_to_drect();
            geometry_test();
            test_border_enumerator();
            test_find_affine_transform();
            DLIB_TEST(projective_transform_pass_rate(0.1) > 0.99);
            DLIB_TEST(projective_transform_pass_rate(0.0) == 1);

            test_find_similarity_transform<double>();
            test_find_similarity_transform2<double>();
            test_find_similarity_transform<float>();
            test_find_similarity_transform2<float>();
            test_line();
        }
    } a;

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Tests only compilability with pedantic warnings, run-time values aren't tested

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

using namespace mtl;

template <typename Size>
void test()
{
    io::tout << "\n\nSize type is " << typeid(Size).name() << '\n';
    typedef mat::parameters<row_major, mtl::index::c_index, non_fixed::dimensions, false, Size> para;
    compressed2D<double, para>   A;
    laplacian_setup(A, 2, 3);

    io::tout << "A is\n" << A << "\nsize of index is " << sizeof(A.ref_minor()[0]) << '\n';
}

int main()
{
    test<boost::uint_least16_t>();
    test<boost::uint_least32_t>();
    test<std::size_t>();

    test<boost::int_least16_t>();
    test<boost::int_least32_t>();
    test<std::ptrdiff_t>();

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_PIO_4LO_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_PIO_4LO_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    This constant is such that, for pairs of types (Tup, T)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_4lo<T>())+Tup(Pio_4<T>())-Pio_4\<Tup\>())  is  less than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    Pio_4 + x with x small,  by replacing them by Pio_4 + (Pio_4lo+x)

    @par Semantic:

    For type T:

    @code
    T r = Pio_4lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 3.061616997868383e-17
    else if T is float
      r = -2.1855694e-08
    @endcode

    @return a value of type T

**/
  template<typename T> T Pio_4lo();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      This constant is such that, for pairs of types (Tup, T)
      (namely (float,  double) and (double, long double)) the sum:

      Generate the  constant pio_4lo.

      @return The Pio_4lo constant for the proper type
    **/
    const boost::dispatch::functor<tag::pio_4lo_> pio_4lo = {};
  }
} }
#endif

#include <boost/simd/constant/definition/pio_4lo.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <vector>
#include <iostream>

#include <stdlib.h>
#include <boost/pending/bucket_sorter.hpp>

template <class Integral>
struct trivial_id {
  std::size_t operator[](Integral i) {
    return i;
  }
  std::size_t operator[](Integral i) const {
    return i;
  }
};

int main() {
  using namespace std;
  using boost::bucket_sorter;

  const std::size_t N = 10;

  vector<std::size_t> bucket(N);
  for (std::size_t i=0; i<N; i++) {
    bucket[i] = rand() % N;
    cout.width(6);
    cout << "Number " << i << " has its bucket "  << bucket[i] << endl;
  }

  typedef trivial_id<int> ID;
  typedef bucket_sorter<std::size_t, int,
    vector<std::size_t>::iterator, ID> BS;
  BS my_bucket_sorter(N, N, bucket.begin());

  for (std::size_t ii=0; ii<N; ii++)
    my_bucket_sorter.push(ii);

  std::size_t j;
  for (j=0; j<N; j++) {
    cout << "The bucket " << j;
    if ( ! my_bucket_sorter[j].empty() ) {
      cout << " has number ";
      do {
        int v = my_bucket_sorter[j].top();
        my_bucket_sorter[j].pop();
        cout << v << " ";
      } while ( ! my_bucket_sorter[j].empty() );
      cout << endl;
    } else {
      cout << " has no number associated with." << endl;
    }
  }

  for (std::size_t k=0; k<N; k++)
    my_bucket_sorter.push(k);

  my_bucket_sorter.remove(5);
  my_bucket_sorter.remove(7);

  cout << "Afer remove number 5 and 7, check correctness again." << endl;

  for (j=0; j<N; j++) {
    cout << "The bucket " << j;
    if ( ! my_bucket_sorter[j].empty() ) {
      cout << " has number ";
      do {
        int v = my_bucket_sorter[j].top();
        my_bucket_sorter[j].pop();
        cout << v << " ";
      } while ( ! my_bucket_sorter[j].empty() );
      cout << endl;
    } else {
      cout << " has no number associated with." << endl;
    }
  }

  std::size_t iii;
  for (iii=0; iii<N; iii++) {
    std::size_t current = rand() % N;
    if ( ! my_bucket_sorter[current].empty() ) {
      int v = my_bucket_sorter[current].top();
      my_bucket_sorter[current].pop();
      bucket[v] = rand() % N;
      my_bucket_sorter.push(v);
    }
  }

  for (iii=0; iii<N; iii++) {
    std::size_t current = rand() % N;
    if ( ! my_bucket_sorter[current].empty() ) {
      int v = my_bucket_sorter[current].top();
      bucket[v] = rand() % N;
      my_bucket_sorter.update(v);
    }
  }

    return 0;
}


// -*- coding: utf-8 -*-
#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <doctest/doctest.h>
//#include <fmt/format.h>
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair
#include <vector>


#include "occupancy_grid.h"
#include "occupancy_grid.inl"
#include "robot_configuration.h" // Data structures and configuration data shared with Arduino controller
#include "error_handling.h"

#include <cstdlib>
#include <cmath>
#include <algorithm>

#include <boost/range/size.hpp>
#include <boost/range/iterator_range.hpp>
#include <opencv2/imgproc.hpp>

COccupancyGrid::COccupancyGrid()
:   m_matnMapObstacle(c_nMapExtent, c_nMapExtent, CV_8UC1, cv::Scalar(128))
{}

COccupancyGrid::COccupancyGrid(COccupancyGrid const& occgrid)
:   COccupancyGridBaseT(occgrid), m_matnMapObstacle(occgrid.m_matnMapObstacle.clone())
{}

COccupancyGrid& COccupancyGrid::operator=(COccupancyGrid const& occgrid) {
    COccupancyGridBaseT::operator=(occgrid);
    m_matnMapObstacle=occgrid.m_matnMapObstacle.clone();
    return *this;
}

void COccupancyGrid::updateGrid(rbt::point<int> const& pt, double fOdds) {
    // Calculating the greyscale map is pretty expensive
    // If we ever need a non-binary version, a lookup table
    // would be useful instead of this:
    // auto const nColor = rbt::numeric_cast<std::uint8_t>(1.0 / ( 1.0 + std::exp( fOdds )) * 255);
    assert(is_inside(pt));
    m_matnMapObstacle.at<std::uint8_t>(pt.y, pt.x) = 0 < fOdds ? 0 : 255;
}

void COccupancyGrid::updateGridPoly(std::vector<rbt::point<int>> const& rngpt, double fOdds) {
    std::vector<cv::Point> vecpt(boost::begin(rngpt), boost::end(rngpt));
    cv::fillConvexPoly(m_matnMapObstacle, vecpt.data(), vecpt.size(), 0 < fOdds ? 0 : 255);
}

cv::Mat ObstacleMapWithPoses(cv::Mat const& m, std::vector<rbt::pose<double>> const& vecpose) {
    rbt::point<int> ptnPrev = ToGridCoordinate(vecpose.front().m_pt);
    boost::for_each(vecpose, [&](rbt::pose<double> const& pose) {
        auto const ptnGrid = ToGridCoordinate(pose.m_pt);
        cv::line(m, ptnPrev, ptnGrid, cv::Scalar(0));
        ptnPrev = ptnGrid;
    });
    return m;
}

std::vector<rbt::point<int>> RenderRobotPose(cv::Mat& mat, rbt::pose<double> const& pose, cv::Scalar color) {
    rbt::size<double> const szfHalfSize(c_nRobotWidth/2.0, c_nRobotHeight/2.0);
    std::vector<rbt::point<int>> vecpt = {
        ToGridCoordinate(pose.m_pt - szfHalfSize.rotated(pose.m_fYaw)),
        ToGridCoordinate(pose.m_pt + rbt::size<double>(szfHalfSize.x, -szfHalfSize.y).rotated(pose.m_fYaw)),
        ToGridCoordinate(pose.m_pt + szfHalfSize.rotated(pose.m_fYaw)),
        ToGridCoordinate(pose.m_pt + rbt::size<double>(-szfHalfSize.x, szfHalfSize.y).rotated(pose.m_fYaw))
    };
    cv::fillConvexPoly(mat, reinterpret_cast<cv::Point*>(vecpt.data()), vecpt.size(), color);
    return vecpt;
}

cv::Mat COccupancyGrid::ObstacleMapWithPoses(std::vector<rbt::pose<double>> const& vecpose) const {
    return ::ObstacleMapWithPoses(ObstacleMap(), vecpose);
}


#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

using Value = float;

int main() {

  const int N = 1 << 24;
  const int num_sections = 8;
  const int section_width = N / num_sections;

  std::fstream ofs("data.dat", std::ios::out);

  {
    std::uniform_real_distribution<> reals(0.0, 1.0);
    std::random_device gen;

    std::vector<Value> values;

    for (int i = 0; i < N; i++) {
      values.push_back(reals(gen));
    }

    boost::archive::text_oarchive oa(ofs);

    for (int i = 0; i < N; i++) {
      oa << values[i];
    }

    // Too lazy to write a deserialize function that can read ranges, so this will have to do
    for (int section = 0; section < num_sections; section++) {
      std::string filename = "data_" + std::to_string(section) + ".dat";
      std::fstream local_ofs(filename, std::ios::out);
      boost::archive::text_oarchive local_oa(local_ofs);

      for (int i = 0; i < section_width; i++) {
        int idx = (section * section_width) + i;
        local_oa << values[idx];
      }
    }
  }

  std::vector<Value> restore_values;
  {
    std::ifstream ifs("data.dat");
    boost::archive::text_iarchive ia(ifs);

    for (int i = 0; i < N; i++) {
      Value value;
      ia >> value;
      restore_values.push_back(value);
    }
  }

  for (int i = 0; i < 10; i++) {
    std::cout << restore_values[i] << " ";
  }
  std::cout << std::endl;
}


// 生成随机地图，并模拟无人机检测，模拟得到局部地图与全局地图

#include <ros/ros.h>
#include <ros/console.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>
#include <random>
#include <tf/transform_broadcaster.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include "prometheus_msgs/DroneState.h"
using namespace std;

//无人机状态 [位置、速度、姿态]
vector<double> _state;

// 障碍物点云地图
pcl::PointCloud<pcl::PointXYZ> cloudMap;

// kdtree地图，为了索引方便？
pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeLocalMap;

sensor_msgs::PointCloud2 localMap_pcd;
sensor_msgs::PointCloud2 globalMap_pcd;

vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double> rand_x;
uniform_real_distribution<double> rand_y;
uniform_real_distribution<double> rand_w;
uniform_real_distribution<double> rand_h;

ros::Publisher local_map_pub;
ros::Publisher global_map_pub;
ros::Subscriber drone_state_sub;

int i = 0;

int _obs_num;
double _x_size, _y_size, _z_size;
double map_x_0, map_x_1, map_y_0, map_y_1, _w_l, _w_h, _h_l, _h_h;
double _z_limit, sense_range, _resolution, _sense_rate, _init_x, _init_y;

bool _map_ok = false;
bool _has_odom = false;

void drone_state_cb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
  _has_odom = true;

  _state = { msg->position[0],
                     msg->position[1],
                    msg->position[2],
                    msg->velocity[0],
                     msg->velocity[1],
                    msg->velocity[2],
                    msg->attitude_q.w,
                     msg->attitude_q.x,
                    msg->attitude_q.y,
                    msg->attitude_q.z,
             0.0,
             0.0 };
}

void pubSensedPoints();
void RandomMapGenerate();

//  主函数
int main(int argc, char** argv)
{
  ros::init(argc, argv, "random_map_sensing");
  ros::NodeHandle n("~");

  // 订阅无人机位置信息
  drone_state_sub = n.subscribe("/prometheus/drone_state", 50, drone_state_cb);

  // 发布 模拟的局部点云
  local_map_pub = n.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/local_pcl_sim", 1);

  // 发布 模拟的全局点云
  global_map_pub = n.advertise<sensor_msgs::PointCloud2>("/prometheus/planning/global_pcl_sim", 1);

  // 参数读取
  // 初始位置
  n.param("init_pos_x", _init_x, 0.0);
  n.param("init_pos_y", _init_y, 0.0);

  // 地图尺寸、分辨率
  n.param("map/x_size", _x_size, 10.0);
  n.param("map/y_size", _y_size, 10.0);
  n.param("map/z_size", _z_size, 5.0);
  n.param("map/resolution", _resolution, 0.2);

  // 障碍物
  n.param("obs/obs_num", _obs_num, 30);
  n.param("obs/min_radius", _w_l, 0.3);
  n.param("obs/max_radius", _w_h, 0.8);
  n.param("obs/min_height", _h_l, 3.0);
  n.param("obs/max_height", _h_h, 7.0);

  // 传感器感知半径与感知频率
  n.param("sensing/sense_range", sense_range, 10.0);
  n.param("sensing/sense_rate", _sense_rate, 10.0);

  // 运行频率 = 传感器更新频率
  ros::Rate loop_rate(_sense_rate);

  // 地图坐标 [x_0,  x_1] - [y_0, y_1]
  map_x_0 = -_x_size / 2.0;
  map_x_1 = +_x_size / 2.0;

  map_y_0 = -_y_size / 2.0;
  map_y_1 = +_y_size / 2.0;

  _z_limit = _z_size;

  _obs_num = min(_obs_num, (int)_x_size * 10);

  ros::Duration(0.1).sleep();

  //生成随机地图
  RandomMapGenerate();

  while (ros::ok())
  {
    // 更新无人机位置
    ros::spinOnce();

    // 发布感知数据
    pubSensedPoints();

    loop_rate.sleep();
  }
}

void RandomMapGenerate()
{
  pcl::PointXYZ pt_random;

  // uniform_real_distribution函数：
  rand_x = uniform_real_distribution<double>(map_x_0, map_x_1);
  rand_y = uniform_real_distribution<double>(map_y_0, map_y_1);
  rand_w = uniform_real_distribution<double>(_w_l, _w_h);
  rand_h = uniform_real_distribution<double>(_h_l, _h_h);

  //  循环生成随机障碍物
  for (int i = 0; i < _obs_num; i++)
  {
    // 得到障碍物的随机xy位置和大小，高度
    double x, y, w, h;
    x = rand_x(eng);
    y = rand_y(eng);
    w = rand_w(eng);
    h = rand_h(eng);

    // 如果障碍物距离无人机初始位置 过近，则跳过
    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 1.5)
      continue;

    // 障碍物中心点坐标？
    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;

    // 障碍物占据的格子数
    int widNum = ceil(w / _resolution);

    for (int r = -widNum / 2.0; r < widNum / 2.0; r++)
      for (int s = -widNum / 2.0; s < widNum / 2.0; s++)
      {
        int heiNum = ceil(h / _resolution);
        for (int t = -0; t < heiNum; t++)
        {
          pt_random.x = x + (r + 0.5) * _resolution + 1e-2;
          pt_random.y = y + (s + 0.5) * _resolution + 1e-2;
          pt_random.z = (t + 0.5) * _resolution + 1e-2;
          // 将障碍物点推入cloudMap
          cloudMap.points.push_back(pt_random);
        }
      }
  }

  cloudMap.width = cloudMap.points.size();
  cloudMap.height = 1;
  cloudMap.is_dense = true;

  ROS_WARN("Finished generate random map ");

  kdtreeLocalMap.setInputCloud(cloudMap.makeShared());

  _map_ok = true;
}

void pubSensedPoints()
{
  // 定时更新全局点云
  static int num = 0;

  if (num  == 10)
  {
    pcl::toROSMsg(cloudMap, globalMap_pcd);
    globalMap_pcd.header.frame_id = "world";
    global_map_pub.publish(globalMap_pcd);
    num=0;
  }else
  {
    num++;
  }

  if (!_map_ok || !_has_odom)
  {
    printf("[generate random map]: don't have the odom ");
    return;
  }

  pcl::PointCloud<pcl::PointXYZ> localMap;

  pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]);
  pointIdxRadiusSearch.clear();
  pointRadiusSquaredDistance.clear();

  pcl::PointXYZ pt;
  pcl::PointXYZ pt_new;

  if (isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z)){
    printf("[generate random map]: the odom state is bad, can't generate the local map!\n");
    return;
  }

Eigen::Vector3d p3d;
Eigen::Matrix<double, 3,3> rotaion_mat_global_to_local = Eigen::Quaterniond(_state[6],
                                                                                _state[7],
                                                                                _state[8],
                                                                                _state[9]).toRotationMatrix().transpose();

  if (kdtreeLocalMap.radiusSearch(searchPoint, sense_range, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
    {
      pt = cloudMap.points[pointIdxRadiusSearch[i]];
      {
        p3d(0)=pt.x - searchPoint.x;
        p3d(1)=pt.y - searchPoint.y;
        p3d(2)=pt.z - searchPoint.z;
        p3d = rotaion_mat_global_to_local *(p3d);
        pt_new.x = p3d(0);
        pt_new.y = p3d(1);
        pt_new.z = p3d(2);
      }
      localMap.points.push_back(pt_new);
      // localMap.points.push_back(pt);
    }
  }
  else
  {
    ROS_ERROR("[Map server] No obstacles .");
    return;
  }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  // 发布局部点云,此处需要发布 base_link与 world的关系
  pcl::toROSMsg(localMap, localMap_pcd);
  localMap_pcd.header.frame_id = "base_link";
  local_map_pub.publish(localMap_pcd);

  //发布tf
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(_state[0], _state[1], _state[2]) );
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));

}


/**
 * This code takes images, extracts keypoints and descriptors and clusters them into a vocabulary tree
 */

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/contrib/contrib.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include <cstdio>

// Based on opencv/samples/cpp/bagofwords_classification.cpp
//
/*
using cv::Mat;
using cv::imshow;
using cv::waitKey;
using cv::Ptr;
*/
using namespace cv;
using std::string;

static bool WriteVocabulary(const string& filename, const Mat& vocabulary) {
  printf("Saving vocabulary...\n");
  FileStorage fs(filename, FileStorage::WRITE);
  if (fs.isOpened()) {
    fs << "Vocabulary" << vocabulary;
    return true;
  }
  return false;
}

static cv::Mat TrainVocabulary(const string &vocab_dir, int total_frames,
    float descriptor_proportion) {
//If you have fewer descriptors than kvocab_size, it will crash in kmeans. But, set this appropriately large
  const int kvocab_size = 300; //Should be large enough to create a rich vocab relative to your data
  cv::RNG rng(1234);
  Ptr<FeatureDetector> fdetector(new DynamicAdaptedFeatureDetector(
            AdjusterAdapter::create("SURF"), 100, 130, 5));
  Ptr<DescriptorExtractor> dextractor =
    // DescriptorExtractor::create("SIFT");
    (new SurfDescriptorExtractor(1000, 4, 2, false, true));

  TermCriteria terminate_criterion;
  terminate_criterion.epsilon = FLT_EPSILON;
  BOWKMeansTrainer bow_trainer(kvocab_size, terminate_criterion, 3, KMEANS_PP_CENTERS);

  for (int i = 0; i < total_frames; i++) {
    Mat vocab_frame;
    // TODO: Hardcoded prefix
    string frame_filename = vocab_dir + "vocab_%06d.png";
    vocab_frame = cv::imread(cv::format(frame_filename.c_str(), i));

    std::vector<cv::KeyPoint> image_keypoints;
    fdetector->detect(vocab_frame, image_keypoints);
    Mat image_descriptors;
    dextractor->compute(vocab_frame, image_keypoints, image_descriptors);
#if 1
    // TODO: pull out of the loop. Using it here is inefficient
    CV_Assert( dextractor->descriptorType() == CV_32FC1 );
    const int elem_size = CV_ELEM_SIZE(dextractor->descriptorType());
    const int desc_byte_size = dextractor->descriptorSize() * elem_size;
    const long int bytes_in_MB = 1048576;
    const long int memory_use = 100000;

    // Total number of descs to use for training.
    const long int max_desc_count = (memory_use * bytes_in_MB) / desc_byte_size;
    if (bow_trainer.descripotorsCount() > max_desc_count) {
      printf("Breaking due to full memory ( descriptors count =");
      printf("%d; descriptor size in bytes = %d; all used memory = %d\n",
          bow_trainer.descripotorsCount(), desc_byte_size,
          bow_trainer.descripotorsCount()*desc_byte_size);
      break;
    }

    if (!image_descriptors.empty()) {
      int desc_count = image_descriptors.rows;

      // Extracting desc_proportion descriptors from the image
      int descs_to_extract = static_cast<int>(descriptor_proportion
                               *static_cast<float>(desc_count));
      vector<char> used_mask( desc_count, false );
      fill( used_mask.begin(), used_mask.begin() + descs_to_extract, true );
      for( int i = 0; i < desc_count; i++ ) {
        int i1 = rng(desc_count), i2 = rng(desc_count);
        char tmp = used_mask[i1]; used_mask[i1] = used_mask[i2]; used_mask[i2] = tmp;
      }

      for(int i = 0; i < desc_count; i++) {
        if(used_mask[i] && bow_trainer.descripotorsCount() < max_desc_count)
          bow_trainer.add(image_descriptors.row(i));
      }
    }
#endif
    drawKeypoints(vocab_frame, image_keypoints, vocab_frame);
    cv::imshow("image read in", vocab_frame);
    int keyp = cv::waitKey(0);
    if (keyp == 27) {
      printf("Done building vocabulary tree\n");
      break;
    }
  }

  Mat vocabulary = bow_trainer.cluster();
  printf("Done building vocabulary tree\n");
  return vocabulary;
}

// directory creation utility function
static void make_img_directory(string *img_save_dir) {
  // FIXME: Assumes linux style directory structure
  // need to generalize to windows
  if ((*img_save_dir)[img_save_dir->length() - 1] != '/') {
    *img_save_dir += "/";
  }
  boost::filesystem::path dir(*img_save_dir);
  if (boost::filesystem::exists(dir)) {
    printf("Sequence directory already exists. Change dir name. Aborting. \n");
    exit(-1);
  }
  if (boost::filesystem::create_directory(dir)) {
    printf("Directory '%s' created\n", img_save_dir->c_str());
  }
}

void help()
{
	printf("\n"
" Keyboard controls: <ESC>       - quits image capturing mode and starts\n"
"                                  vocabulary tree building \n"
"                    <SPACE BAR> - saves the image during data collection\n"
"                                - moves to next image while showing\n"
"                                  keypoints and building a vocabulary tree \n"
" Sample usage:\n"
"\n"
" ./build_vocab_tree [<img_save_dir>] [<camera-deviceid>]\n"
"\n"
"arguments:\n"

"   <img_save_dir>    -- directory where vocabulary images are saved\n"
"   <camera-deviceid> -- should be non-negative integer. default camera is 0\n\n"
);
}

//
// Keyboard controls: <ESC>       - quits image capturing mode and starts
//                                  vocabulary tree building
//                    <SPACE BAR> - saves the image during data collection
//                                - moves to next image while showing
//                                   keypoints and building a vocabulary tree
// Sample usage:
//
// ./build_vocab_tree [<img_save_dir>] [<camera-deviceid>]
//
// arguments:
//   <img_save_dir>    -- directory where vocabulary images are saved
//   <camera-deviceid> -- should be non-negative integer. default camera is 0
//
//   TODO: <vocab_file>   -- yml file that stores vocabulary

int main(int argc, char *argv[]) {
  string img_save_dir;
  int cam_deviceid = 0;
  if (argc == 1) {
    help();
    return(0);
  } else if (argc == 2) {
    if (!strcmp(argv[1],"-h") || !strcmp(argv[1],"--help")) {
      help();
      return(0);
    }
    img_save_dir = string(argv[1]);
    make_img_directory(&img_save_dir);
  } else if (argc == 3) {
    if (!strcmp(argv[1],"-h") || !strcmp(argv[1],"--help")) {
      help();
      return(0);
    }
    img_save_dir = string(argv[1]);
    make_img_directory(&img_save_dir);
    cam_deviceid = boost::lexical_cast<int>(argv[2]);
  } else {
    //incorrect arguments
    help();
    return -1;
  }

  // read images from camera
  cv::VideoCapture cap(cam_deviceid);
  if (!cap.isOpened()) {
    printf("Opening the default camera did not succeed\n");
    return -1;
  }

  int framenum = 0;
  for (;;) {
    Mat frame;
    cap >> frame;
    imshow("input RGB image", frame);
    int keyp = waitKey(30);
    if(keyp == 'h')
      help();
    else if (keyp == 27) {
      printf("Done capturing.\n");
      break;
    } else if (keyp == 32) {
      printf("Saving frame\n");
      string frame_filename;
      frame_filename = img_save_dir + "vocab_%06d.png";
      cv::imwrite(cv::format(frame_filename.c_str(), framenum), frame);
      framenum++;
    }
  }

  destroyWindow("input RGB image");
  const float kdescriptor_proportion = 0.7;
  cv::Mat vocabulary = TrainVocabulary(img_save_dir, framenum - 1, kdescriptor_proportion);
  string filename = "vocab_big.yml";
  if (!WriteVocabulary(filename, vocabulary)) {
    printf("Error: file %s cannot be opened to write\n", filename.c_str());
    exit(-1);
  }

  return 0;
}


#ifndef MTGP_CALC_JUMP_HPP
#define MTGP_CALC_JUMP_HPP
/**
 * @file mtgp-calc-jump.hpp
 *
 * @brief functions for calculating jump polynomial.
 *
 * calculate jump polynomial from jump step and the
 * characteristic polynomial using PowerXMod.

 * jump polynomial p is calculated

 *     p = Xs mod q,

 * where s is jump step and q is characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <iomanip>
#include <sstream>
#include <NTL/GF2X.h>

/**
 * convert polynomial to string
 * @param[out] x output string
 * @param[in] polynomial polynomial
 */
static inline void polytostring(std::string& x, NTL::GF2X& polynomial)
{
    using namespace NTL;
    using namespace std;

    long degree = deg(polynomial);
    int buff = 0;
    int index = 0;
    stringstream ss;
    for (int i = 0; i <= degree; i++) {
	if (index < i / 4) {
	    ss << hex << buff;
	    buff = 0;
	    index = i / 4;
	}
	if (IsOne(coeff(polynomial, i))) {
	    buff |= 1 << (i % 4);
	}
    }
    if (buff != 0) {
	ss << hex << buff;
    }
    ss << flush;
    x = ss.str();
}

/**
 * Convert polynomial to 32-bit unsigned array.
 * Resulted array is used in kernel functions of cuda and OpenCL.
 * @param[out] array array format polynomial
 * @param[in] size max size of array
 * @param polynomial polynomial
 */
static inline void polytoarray(uint32_t array[],
			       int size,
			       NTL::GF2X& polynomial)
{
    using namespace NTL;
    using namespace std;

    long degree = deg(polynomial);
    if (size == 0) {
	return;
    } else if (size < degree / 32 + 1) {
	array[0] = 0;
	return;
    }
    for (int i = 0; i < size; i++) {
	array[i] = 0;
    }
    for (int i = 0; i <= degree; i++) {
	int index = i / 32;
	int pos = i % 32;
	if (IsOne(coeff(polynomial, i))) {
	    array[index] |= 1 << pos;
	}
    }
}

/**
 * converts string to polynomial
 * @param[in] str string
 * @param[out] poly output polynomial
 */
static inline void stringtopoly(NTL::GF2X& poly, std::string& str)
{
    using namespace NTL;
    using namespace std;

    stringstream ss(str);
    char c;
    long p = 0;
    clear(poly);
    while(ss) {
	ss >> c;
	if (!ss) {
	    break;
	}
	if (c >= 'a') {
	    c = c - 'a' + 10;
	} else {
	    c = c - '0';
	}
	for (int j = 0; j < 4; j++) {
	    if (c & (1 << j)) {
		SetCoeff(poly, p, 1);
	    } else {
		SetCoeff(poly, p, 0);
	    }
	    p++;
	}
    }
}

/**
 * calculate the jump polynomial.
 * @param[out] array output string which represents jump polynomial.
 * @param[in] size max size of array
 * @param[in] step jump step of internal state
 * @param[in] characteristic characteristic polynomial
 */
static inline void calc_jump(uint32_t array[],
			     int size,
			     NTL::ZZ& step,
			     NTL::GF2X& characteristic)
{
    using namespace NTL;
    using namespace std;
    GF2X jump;
    PowerXMod(jump, step, characteristic);
    polytoarray(array, size, jump);
}

/**
 * calculate the jump polynomial.
 * @param[out] jump_str output string which represents jump polynomial.
 * @param[in] step jump step of internal state
 * @param[in] characteristic characteristic polynomial
 */
static inline void calc_jump(std::string& jump_str,
			     NTL::ZZ& step,
			     NTL::GF2X& characteristic)
{
    using namespace NTL;
    using namespace std;
    GF2X jump;
    PowerXMod(jump, step, characteristic);
    polytostring(jump_str, jump);
}

#endif


#include "SPlisHSPlasH/Common.h"
#include <Eigen/Dense>
#include "extern/cxxopts/cxxopts.hpp"
#include "Utilities/Timing.h"
#include "Utilities/Counting.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/TriangleMesh.h"
#include "Utilities/PartioReaderWriter.h"
#include "Utilities/SystemInfo.h"
#include "Utilities/Version.h"
#include "Discregrid/All"
#include "Utilities/FileSystem.h"
#include "SPHVolumeSampling.h"
#include "SPHVolumeSampling_Jiang2015.h"

using namespace SPH;
using namespace Eigen;
using namespace std;
using namespace Utilities;

INIT_TIMING
INIT_COUNTING
INIT_LOGGING

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
#define new DEBUG_NEW
#endif
#endif

void sampleObject(TriangleMesh &mesh);
void generateSDF(SPH::TriangleMesh &mesh);
void computeBoundingBox(TriangleMesh &mesh);

double distance(const Vector3r &x, const Real tolerance);

string exePath;
string inputFile = "";
string outputFile = "";
int output_format = 0; // 0: partio, 1: vtk
Real radius = static_cast<Real>(0.025);
Real viscosity = 0.25;
Real cohesion = 0.0;
Real adhesion = 0.0;
Real cflFactor = 0.25;
Real stiffness = 10000.0;
Real dt = 0.0005;
Vector3r scale(1.0, 1.0, 1.0);
Real diameter = radius* static_cast<Real>(2.0);
int mode = 4;
unsigned int steps = 100;
Eigen::Matrix<unsigned int, 3, 1> resolutionSDF(30, 30, 30);
SamplingBase::Region region;
bool invert = false;
bool useRegion = false;
std::vector<Vector3r> particles;
Vector3r bbmin, bbmax;
std::string outputPath = "";
bool useCache = true;
std::shared_ptr<Discregrid::CubicLagrangeDiscreteGrid> distanceField;

std::istream& operator >> (std::istream& istream, SamplingBase::Region& r)
{
	return istream >> std::skipws >> r.m_min[0] >> r.m_min[1] >> r.m_min[2] >> r.m_max[0] >> r.m_max[1] >> r.m_max[2];
}

std::ostream& operator << (std::ostream& out, const SamplingBase::Region& r)
{
	out << r.m_min[0] << ", " << r.m_min[1] << ", " << r.m_min[2] << ", " << r.m_max[0] << ", " << r.m_max[1] << ", " << r.m_max[2];
	return out;
}

std::istream& operator >> (std::istream& istream, Eigen::Matrix<unsigned int, 3, 1>& r)
{
	return istream >> std::skipws >> r[0] >> r[1] >> r[2];
}

std::ostream& operator << (std::ostream& out, const Eigen::Matrix<unsigned int, 3, 1>& r)
{
	out << r[0] << ", " << r[1] << ", " << r[2];
	return out;
}

std::istream& operator >> (std::istream& istream, Vector3r& r)
{
	return istream >> std::skipws >> r[0] >> r[1] >> r[2];
}

std::ostream& operator << (std::ostream& out, const Vector3r& r)
{
	out << r[0] << ", " << r[1] << ", " << r[2];
	return out;
}

// main
int main(int argc, char **argv)
{
	REPORT_MEMORY_LEAKS;

	exePath = FileSystem::getProgramPath();

	Timing::m_dontPrintTimes = true;

	try
	{
		cxxopts::Options options(argv[0], "VolumeSampling - Sample a volumetric geometry given by an OBJ file.");

		options.add_options()
			("h,help", "Print help")
			("i,input", "Input file (obj)", cxxopts::value<std::string>())
			("o,output", "Output file (bgeo or vtk)", cxxopts::value<std::string>())
			("r,radius", "Particle radius", cxxopts::value<Real>()->default_value("0.025"))
			("s,scale", "Scaling of input geometry (e.g. --scale \"2 1 2\")", cxxopts::value<Vector3r>()->default_value("1 1 1"))
			("m,mode", "Mode (regular=0, almost dense=1, dense=2, Jiang et al. 2015=3, Kugelstadt et al. 2021=4)", cxxopts::value<int>()->default_value("4"))
			("region", "Region to fill with particles (e.g. --region \"0 0 0 1 1 1\")", cxxopts::value<SamplingBase::Region>())
			("steps", "SPH time steps", cxxopts::value<unsigned int>()->default_value("100"))
			("cflFactor", "CFL factor", cxxopts::value<Real>()->default_value("0.25"))
			("viscosity", "Viscosity coefficient (XSPH)", cxxopts::value<Real>()->default_value("0.25"))
			("cohesion", "Cohesion coefficient", cxxopts::value<Real>())
			("adhesion", "Adhesion coefficient", cxxopts::value<Real>())
			("stiffness", "Stiffness coefficient (only mode 3)", cxxopts::value<Real>()->default_value("10000.0"))
			("dt", "Time step size (only mode 3)", cxxopts::value<Real>()->default_value("0.0005"))
			("res", "Resolution of the Signed Distance Field (e.g. --res \"30 30 30\")", cxxopts::value<Eigen::Matrix<unsigned int, 3, 1>>())
			("invert", "Invert the SDF to sample the outside of the object in the bounding box/region")
			("no-cache", "Disable caching of SDF.")
			;

		auto result = options.parse(argc, argv);

		if (result.count("help"))
		{
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(0);
		}

		if (result.count("input") && result.count("output"))
		{
			inputFile = result["input"].as<std::string>();
			outputFile = result["output"].as<std::string>();
			output_format = 0;
			if (Utilities::StringTools::to_upper(FileSystem::getFileExt(outputFile)) == "VTK")
				output_format = 1;
		}
		else
		{
			std::cout << "Input or output missing!" << std::endl;
			std::cout << options.help({ "", "Group" }) << std::endl;
			exit(1);
		}

		if (result.count("mode"))
			mode = result["mode"].as<int>();

		outputPath = FileSystem::normalizePath(exePath + "/output/" + FileSystem::getFileName(inputFile));

		Utilities::logger.addSink(unique_ptr<Utilities::ConsoleSink>(new Utilities::ConsoleSink(Utilities::LogLevel::INFO)));
		std::string logPath = FileSystem::normalizePath(outputPath + "/log");
		FileSystem::makeDirs(logPath);
		Utilities::logger.addSink(unique_ptr<Utilities::FileSink>(new Utilities::FileSink(Utilities::LogLevel::DEBUG, logPath + "/SPH_log.txt")));

		LOG_INFO << "SPlisHSPlasH version: " << SPLISHSPLASH_VERSION;
		LOG_DEBUG << "Git refspec:          " << GIT_REFSPEC;
		LOG_DEBUG << "Git SHA1:             " << GIT_SHA1;
		LOG_DEBUG << "Git status:           " << GIT_LOCAL_STATUS;
		LOG_DEBUG << "Host name:            " << Utilities::SystemInfo::getHostName();

		LOG_INFO << "Input = " << inputFile;
		LOG_INFO << "Output = " << outputFile;
		LOG_INFO << "Mode: " << mode;

#ifdef DL_OUTPUT
		std::string modelsFilePath = FileSystem::normalizePath(outputPath + "/models");
		FileSystem::makeDirs(modelsFilePath);
		FileSystem::copyFile(inputFile, modelsFilePath + "/" + FileSystem::getFileNameWithExt(inputFile));
#endif

		if (result.count("radius"))
			radius = result["radius"].as<Real>();
		LOG_INFO << "Radius: " << radius;

		if (result.count("scale"))
			scale = result["scale"].as<Vector3r>();
		LOG_INFO << "Scale: " << scale;

		if (result.count("steps"))
			steps = result["steps"].as<unsigned int>();
		LOG_INFO << "SPH steps: " << steps;

		if (result.count("cflFactor"))
			cflFactor = result["cflFactor"].as<Real>();
		LOG_INFO << "CFL Factor: " << cflFactor;

		if (result.count("viscosity"))
			viscosity = result["viscosity"].as<Real>();
		LOG_INFO << "Viscosity coefficient: " << viscosity;

		if (result.count("cohesion"))
			cohesion = result["cohesion"].as<Real>();

		if (result.count("adhesion"))
			adhesion = result["adhesion"].as<Real>();

		if (result.count("stiffness"))
			stiffness = result["stiffness"].as<Real>();
		LOG_INFO << "Stiffness coefficient: " << stiffness;

		if (result.count("dt"))
			dt = result["dt"].as<Real>();
		LOG_INFO << "Time step size: " << dt;

		if (result.count("res"))
		{
			resolutionSDF = result["res"].as<Eigen::Matrix<unsigned int, 3, 1>>();
			LOG_INFO << "SDF resolution: " << resolutionSDF;
		}

		if (result.count("invert"))
		{
			invert = true;
		}

		if (result.count("no-cache"))
			useCache = false;

		if (result.count("region"))
		{
			region = result["region"].as<SamplingBase::Region>();
			useRegion = true;
			LOG_INFO << "Region: " << region;
		}
	}
	catch (const cxxopts::OptionException& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}

	SPHSamplingBase* sampling = nullptr;
	if (mode == 3)
	{
		sampling = new SPHVolumeSampling_Jiang2015();
		((SPHVolumeSampling_Jiang2015*)sampling)->setStiffness(stiffness);
		((SPHVolumeSampling_Jiang2015*)sampling)->setTimeStepSize(dt);
	}
	else if (mode == 4)
		sampling = new SPHVolumeSampling();
	else if (mode > 4)
		LOG_ERR << "Mode not supported!";

	if (sampling != nullptr)
	{
		sampling->setOutputPath(outputPath);
		sampling->setScale(scale);
		sampling->setUseRegion(useRegion);
		sampling->setRegion(region);
		sampling->setResolutionSdf(resolutionSDF);
		sampling->setViscosity(viscosity);
		sampling->setCFLFactor(cflFactor);
		sampling->setSteps(steps);
		sampling->setRadius(radius);
		sampling->setInvert(invert);
		sampling->setUseCache(useCache);
		if (cohesion != 0.0)
			sampling->setCohesion(cohesion);
		if (adhesion != 0.0)
			sampling->setAdhesion(adhesion);

		LOG_INFO << "Cohesion coefficient: " << sampling->getCohesion();
		LOG_INFO << "Adhesion coefficient: " << sampling->getAdhesion();

		sampling->generateSampling(inputFile, outputFile);
		Timing::printTimeSums();
		Timing::printAverageTimes();

		return 0;
	}

	diameter = static_cast<Real>(2.0) * radius;
	TriangleMesh mesh;
	SamplingBase::loadObj(inputFile, mesh, scale);
	computeBoundingBox(mesh);
	START_TIMING("generateSDF");
	generateSDF(mesh);
	STOP_TIMING_AVG_PRINT;

	sampleObject(mesh);

	if (output_format == 0)
		PartioReaderWriter::writeParticles(outputFile, (unsigned int)particles.size(), particles.data(), NULL, 0.0);
	else
		SamplingBase::writeParticlesVTK(outputFile, particles);

	LOG_INFO << "Generated particles: " << particles.size();

	Timing::printTimeSums();
	Timing::printAverageTimes();

	return 0;
}

 void sampleObject(TriangleMesh &mesh)
 {
	 int surface = 0;

 	// sample object
 	const unsigned int numberOfSamplePoints = (((unsigned int)((1.0f / diameter) * (bbmax[2] - bbmin[2]))) + 1) *
		(((unsigned int)((1.0f / diameter) * (bbmax[1] - bbmin[1]))) + 1) *
		(((unsigned int)((1.0f / diameter) * (bbmax[0] - bbmin[0]))) + 1);
 	unsigned int currentSample = 0;
 	Real currentPercent = 0.01;
 	int counter_x = 0;
 	int counter_y = 0;
 	Real xshift = diameter;
 	Real yshift = diameter;

 	if ((mode == 1) || (mode == 4))
 		yshift = sqrt(static_cast<Real>(3.0)) * radius;
 	else if (mode == 2)
 	{
 		xshift = sqrt(static_cast<Real>(3.0)) * radius;
 		yshift = sqrt(static_cast<Real>(6.0)) * diameter / static_cast<Real>(3.0);
 	}
 	for (Real z = bbmin[2]; z <= bbmax[2]; z += diameter)
 	{
 		for (Real y = bbmin[1]; y <= bbmax[1]; y += yshift)
 		{
 			for (Real x = bbmin[0]; x <= bbmax[0]; x += xshift)
 			{
 				Vector3r particlePosition;
				if ((mode == 1) || (mode == 4))
 				{
 					if (counter_y % 2 == 0)
 						particlePosition = Vector3r(x, y + radius, z + radius);
 					else
 						particlePosition = Vector3r(x + radius, y + radius, z);
 				}
 				else if (mode == 2)
 				{
 					particlePosition = Vector3r(x, y + radius, z + radius);

 					Vector3r shift_vec(0, 0, 0);
 					if (counter_x % 2)
 					{
 						shift_vec[2] += diameter / (static_cast<Real>(2.0) * (counter_y % 2 ? -1 : 1));
 					}
 					if (counter_y % 2)
 					{
 						shift_vec[0] += xshift / static_cast<Real>(2.0);
 						shift_vec[2] += diameter / static_cast<Real>(2.0);
 					}
 					particlePosition += shift_vec;
 				}
 				else
 				{
 					// Use center of voxel
 					particlePosition = Vector3r(x + radius, y + radius, z + radius);
 				}

				const double dist = distance(particlePosition, 0.0);
				if ((dist != std::numeric_limits<double>::max()) && (dist > 0.0))
				{
					particles.push_back(particlePosition);
				}
 				currentSample++;

 				if ((Real)currentSample / (Real)numberOfSamplePoints > currentPercent)
 				{
					std::cout << "\r" << "Generating samples " << std::setw(14)	<< currentPercent * 100.0 << "%";
 					currentPercent += 0.01;
 				}
 				counter_x++;
 			}
 			counter_x = 0;
 			counter_y++;
 		}
 		counter_y = 0;
 	}
	std::cout << "\r" << "Generating samples " << std::setw(14) << 100.0 << "%\n";
	LOG_INFO << "Surface particles: " << surface << "\n";
}

void computeBoundingBox(TriangleMesh &mesh)
{
	Vector3r *v = mesh.getVertices().data();

	// compute bounding box
	bbmin = v[0];
	bbmax = bbmin;
	for (unsigned int i = 1; i < mesh.numVertices(); ++i)
	{
		const Vector3r& p = v[i];
		for (unsigned int j = 0; j < 3; ++j) {
			bbmin[j] = std::min(bbmin[j], p[j]);
			bbmax[j] = std::max(bbmax[j], p[j]);
		}
	}

	if (useRegion)
	{
		for (unsigned int i = 0; i < 3; i++)
		{
			bbmin[i] = std::max(scale[i] * region.m_min[i], bbmin[i]);
			bbmax[i] = std::min(scale[i] * region.m_max[i], bbmax[i]);
		}
	}
}

// Determine distance of a point x to the surface of the mesh
double distance(const Vector3r &x, const Real tolerance)
{
	const double dist = distanceField->interpolate(0, x.template cast<double>());
	if (dist == std::numeric_limits<double>::max())
		return dist;
	return dist - tolerance;
}

void generateSDF(SPH::TriangleMesh &mesh)
{
	//////////////////////////////////////////////////////////////////////////
	// Generate distance field of object using Discregrid
	//////////////////////////////////////////////////////////////////////////
#ifdef USE_DOUBLE
	Discregrid::TriangleMesh sdfMesh(&mesh.getVertices()[0][0], mesh.getFaces().data(), mesh.numVertices(), mesh.numFaces());
#else
	// if type is float, copy vector to double vector
	std::vector<double> doubleVec;
	doubleVec.resize(3 * mesh.numVertices());
	for (unsigned int i = 0; i < mesh.numVertices(); i++)
		for (unsigned int j = 0; j < 3; j++)
			doubleVec[3 * i + j] = mesh.getVertices()[i][j];
	Discregrid::TriangleMesh sdfMesh(&doubleVec[0], mesh.getFaces().data(), mesh.numVertices(), mesh.numFaces());
#endif

	Discregrid::MeshDistance md(sdfMesh);
	Eigen::AlignedBox3d domain;
	domain.extend(bbmin.cast<double>());
	domain.extend(bbmax.cast<double>());
	//domain.max() += 1.0e-3 * domain.diagonal().norm() * Eigen::Vector3d::Ones();
	//domain.min() -= 1.0e-3 * domain.diagonal().norm() * Eigen::Vector3d::Ones();
	domain.max() += Eigen::Vector3d::Ones() * radius * 4.0;
	domain.min() -= Eigen::Vector3d::Ones() * radius * 4.0;

	LOG_INFO << "Set SDF resolution: " << resolutionSDF[0] << ", " << resolutionSDF[1] << ", " << resolutionSDF[2];
	distanceField = std::make_shared<Discregrid::CubicLagrangeDiscreteGrid>(domain, std::array<unsigned int, 3>({ resolutionSDF[0], resolutionSDF[1], resolutionSDF[2] }));
	auto func = Discregrid::DiscreteGrid::ContinuousFunction{};

	// invert the distance field since the particles should stay inside
	func = [&md](Eigen::Vector3d const& xi) {return -md.signedDistanceCached(xi); };

	LOG_INFO << "Generate SDF";
	distanceField->addFunction(func, false);
}


/***************************************************************************
 *   Copyright (C) 2006 by Nicola Bellotto                                 *
 *   nbellotto@lincoln.ac.uk                                                    *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "ukf.h"
#include <float.h>
#include <boost/numeric/ublas/io.hpp>
#include "bayes_tracking/BayesFilter/matSup.hpp"
#include "bayes_tracking/BayesFilter/bayesFlt.hpp"

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;

UKFilter::UKFilter(std::size_t x_size) :
        Kalman_state_filter(x_size),
        Unscented_scheme(x_size),
        z_p(Empty)
{
    UKFilter::x_size = x_size;
    UKFilter::XX_size = 2*x_size+1;
    FM::Vec x0(x_size);
    x0.clear();
    FM::SymMatrix P0(x_size, x_size);
    P0.clear();
    init(x0, P0);
}

UKFilter::UKFilter(const FM::Vec& x0, const FM::SymMatrix& P0) :
        Kalman_state_filter(x0.size()),
        Unscented_scheme(x0.size()),
        z_p(Empty)
{
    UKFilter::x_size = x0.size();
    UKFilter::XX_size = 2*x0.size()+1;
    init(x0, P0);
}

UKFilter::~UKFilter()
{
}

void UKFilter::init(const FM::Vec& x0, const FM::SymMatrix& P0)
{
    init_kalman(x0, P0);
}

void UKFilter::update(Additive_predict_model& predict_model,
                      Correlated_additive_observe_model& observe_model,
                      const FM::Vec& z)
{
    predict(predict_model);
    observe(observe_model, z);
}

void UKFilter::predict_observation(Correlated_additive_observe_model& observe_model, FM::Vec& z_pred, FM::SymMatrix& R_pred)
{
    std::size_t z_size = z_pred.size();
    ColMatrix zXX(z_size, 2*x_size+1);
    SymMatrix Xzz(z_size,z_size);
    Matrix Xxz(x_size,z_size);

    z_p.resize(z_size);
    observe_size (z_size);  // Dynamic sizing

    // Create unscented distribution
    kappa = observe_Kappa(x_size);
    Float x_kappa = Float(x_size) + kappa;
    unscented (XX, x, X, x_kappa);

    // Predict points of XX using supplied observation model
    {
        Vec zXXi(z_size), zXX0(z_size);
        zXX0 = static_cast<Correlated_additive_observe_model&>(observe_model).h( column(XX,0) );
        column(zXX,0) = zXX0;
        for (std::size_t i = 1; i < XX.size2(); ++i) {
            zXXi = static_cast<Correlated_additive_observe_model&>(observe_model).h( column(XX,i) );
            // Normalise relative to zXX0
            observe_model.normalise (zXXi, zXX0);
            column(zXX,i) = zXXi;
        }
    }

    // Mean of predicted distribution: z_p
    noalias(z_p) = column(zXX,0) * kappa;
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(z_p) += column(zXX,i) / Float(2); // ISSUE uBlas may not be able to promote integer 2
    }
    z_pred = z_p /= x_kappa;

    // Covariance of observation predict: Xzz
    // Subtract mean from each point in zXX
    for (std::size_t i = 0; i < XX_size; ++i) {
        column(zXX,i).minus_assign (z_p);
    }
    // Center point, premult here by 2 for efficency
    {
        ColMatrix::Column zXX0 = column(zXX,0);
        noalias(Xzz) = FM::outer_prod(zXX0, zXX0);
        Xzz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        ColMatrix::Column zXXi = column(zXX,i);
        noalias(Xzz) += FM::outer_prod(zXXi, zXXi);
    }
    Xzz /= 2*x_kappa;

    /** Fix for non-positive semidefinite covariance **/
    {
        ColMatrix::Column zXX0 = column(zXX,0);      // mean zp already subtracted above
        noalias(Xzz) += FM::outer_prod(zXX0, zXX0);  // modified covariance about mean
    }

    R_pred = Xzz;

//    // Correlation of state with observation: Xxz
//    // Center point, premult here by 2 for efficency
//    {
//       noalias(Xxz) = FM::outer_prod(column(XX,0) - x, column(zXX,0));
//       Xxz *= 2*kappa;
//    }
//    // Remaining unscented points
//    for (std::size_t i = 1; i < zXX.size2(); ++i) {
//       noalias(Xxz) += FM::outer_prod(column(XX,i) - x, column(zXX,i));
//    }
//    Xxz /= 2* (Float(x_size) + kappa);
}

Bayes_base::Float UKFilter::observeInnovation(Correlated_additive_observe_model& h, const Vec& si, const SymMatrix& Si)
/*
 * Observation fusion
 *  Pre : x,X
 *  Post: x,X is PSD
 */
{
    std::size_t z_size = si.size();
    ColMatrix zXX (z_size, 2*x_size+1);
    Vec zp(z_size);
    SymMatrix Xzz(z_size,z_size);
    Matrix Xxz(x_size,z_size);
    Matrix W(x_size,z_size);

    observe_size (si.size());   // Dynamic sizing

    // Create unscented distribution
    kappa = observe_Kappa(x_size);
    Float x_kappa = Float(x_size) + kappa;
    unscented (XX, x, X, x_kappa);

    // Predict points of XX using supplied observation model
    {
        Vec zXXi(z_size), zXX0(z_size);
        zXX0 = h.h( column(XX,0) );
        column(zXX,0) = zXX0;
        for (std::size_t i = 1; i < XX.size2(); ++i) {
            zXXi = h.h( column(XX,i) );
            // Normalise relative to zXX0
            h.normalise (zXXi, zXX0);
            column(zXX,i) = zXXi;
        }
    }

    // Mean of predicted distribution: zp
    noalias(zp) = column(zXX,0) * kappa;
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(zp) += column(zXX,i) / Float(2); // ISSUE uBlas may not be able to promote integer 2
    }
    zp /= x_kappa;

    // Covariance of observation predict: Xzz
    // Subtract mean from each point in zXX
    for (std::size_t i = 0; i < XX_size; ++i) {
        column(zXX,i).minus_assign (zp);
    }
    // Center point, premult here by 2 for efficency
    {
        ColMatrix::Column zXX0 = column(zXX,0);
        noalias(Xzz) = FM::outer_prod(zXX0, zXX0);
        Xzz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        ColMatrix::Column zXXi = column(zXX,i);
        noalias(Xzz) += FM::outer_prod(zXXi, zXXi);
    }
    Xzz /= 2*x_kappa;

    /** Fix for non-positive semidefinite covariance **/
    {
        ColMatrix::Column zXX0 = column(zXX,0);      // mean zp already subtracted above
        noalias(Xzz) += FM::outer_prod(zXX0, zXX0);  // modified covariance about mean
    }

    // Correlation of state with observation: Xxz
    // Center point, premult here by 2 for efficency
    {
        noalias(Xxz) = FM::outer_prod(column(XX,0) - x, column(zXX,0));
        Xxz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(Xxz) += FM::outer_prod(column(XX,i) - x, column(zXX,i));
    }
    Xxz /= 2* (Float(x_size) + kappa);

    // Innovation covariance
    S = Xzz;
    noalias(S) += h.Z;
    // Inverse innovation covariance
    Float rcond = UdUinversePD (SI, S);
    rclimit.check_PD(rcond, "S not PD in observeInnovation");
    // Kalman gain
    noalias(W) = prod(Xxz,SI);

    // Store innovation
    noalias(s) = si;

    // Filter update
    noalias(x) += prod(W,s);
    RowMatrix WStemp(W.size1(), S.size2());
    // update state cov with modified innovation cov
    noalias(X) -= prod_SPD(W, Si, WStemp);

    return rcond;
}

double UKFilter::logLikelihood() {
    SymMatrix Si(S.size1(), S.size2());
    Float detS;
    Float rcond = UdUinversePD(Si, detS, S);  // Si = inv(S)
    Numerical_rcond rclimit;
    rclimit.check_PD(rcond, "S not PD in UKFilter::logLikelihood");
    // exp(-0.5 * (s' * Si * s)) / sqrt(2pi^ns * |S|)
    return -0.5*(inner_prod(trans(s),prod(Si, s))) - 0.5*((double)s.size()*log(2*M_PI)+log(detS));
}

void UKFilter::unscented(FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale) {
    UTriMatrix Sigma(x_size, x_size);

    // Get a upper Cholesky factoriation
    Float rcond = UCfactor(Sigma, X);
    rclimit.check_PSD(rcond, "X not PSD in UKFilter::unscented(...)");
    Sigma *= std::sqrt(scale);

    // Generate XX with the same sample Mean and Covar as before
    column(XX,0) = x;

    for (std::size_t c = 0; c < x_size; ++c) {
        UTriMatrix::Column SigmaCol = column(Sigma,c);
        noalias(column(XX,c+1)) = x  + SigmaCol;
        noalias(column(XX,x_size+c+1)) = x - SigmaCol;
    }
}


// Boost.Geometry Index
// Additional tests

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is 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)

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>

int main()
{
    namespace bg = boost::geometry;
    namespace bgi = bg::index;
    typedef boost::chrono::thread_clock clock_t;
    typedef boost::chrono::duration<float> dur_t;

    size_t values_count = 1000000;
    size_t queries_count = 100000;
    size_t nearest_queries_count = 10000;
    unsigned neighbours_count = 10;

    std::vector< std::pair<float, float> > coords;

    //randomize values
    {
        boost::mt19937 rng;
        //rng.seed(static_cast<unsigned int>(std::time(0)));
        float max_val = static_cast<float>(values_count / 2);
        boost::uniform_real<float> range(-max_val, max_val);
        boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(rng, range);

        coords.reserve(values_count);

        std::cout << "randomizing data\n";
        for ( size_t i = 0 ; i < values_count ; ++i )
        {
            coords.push_back(std::make_pair(rnd(), rnd()));
        }
        std::cout << "randomized\n";
    }

    typedef bg::model::point<double, 2, bg::cs::cartesian> P;
    typedef bg::model::box<P> B;
    typedef bgi::rtree<B, bgi::linear<16, 4> > RT;
    //typedef bgi::rtree<B, bgi::quadratic<8, 3> > RT;
    //typedef bgi::rtree<B, bgi::rstar<8, 3> > RT;

    std::cout << "sizeof rtree: " << sizeof(RT) << std::endl;

    for (;;)
    {
        RT t;

        // inserting test
        {
            clock_t::time_point start = clock_t::now();
            for (size_t i = 0 ; i < values_count ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                B b(P(x - 0.5f, y - 0.5f), P(x + 0.5f, y + 0.5f));

                t.insert(b);
            }
            dur_t time = clock_t::now() - start;
            std::cout << time << " - insert " << values_count << '\n';
        }

        std::vector<B> result;
        result.reserve(100);
        B result_one;

        {
            clock_t::time_point start = clock_t::now();
            size_t temp = 0;
            for (size_t i = 0 ; i < queries_count ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                result.clear();
                t.query(bgi::intersects(B(P(x - 10, y - 10), P(x + 10, y + 10))), std::back_inserter(result));
                temp += result.size();
            }
            dur_t time = clock_t::now() - start;
            std::cout << time << " - query(B) " << queries_count << " found " << temp << '\n';
        }

        {
            clock_t::time_point start = clock_t::now();
            size_t temp = 0;
            for (size_t i = 0 ; i < queries_count / 2 ; ++i )
            {
                float x1 = coords[i].first;
                float y1 = coords[i].second;
                float x2 = coords[i+1].first;
                float y2 = coords[i+1].second;
                float x3 = coords[i+2].first;
                float y3 = coords[i+2].second;
                result.clear();
                t.query(
                    bgi::intersects(B(P(x1 - 10, y1 - 10), P(x1 + 10, y1 + 10)))
                    &&
                    !bgi::within(B(P(x2 - 10, y2 - 10), P(x2 + 10, y2 + 10)))
                    &&
                    !bgi::overlaps(B(P(x3 - 10, y3 - 10), P(x3 + 10, y3 + 10)))
                    ,
                    std::back_inserter(result)
                    );
                temp += result.size();
            }
            dur_t time = clock_t::now() - start;
            std::cout << time << " - query(i && !w && !o) " << queries_count << " found " << temp << '\n';
        }

        result.clear();

        {
            clock_t::time_point start = clock_t::now();
            size_t temp = 0;
            for (size_t i = 0 ; i < nearest_queries_count ; ++i )
            {
                float x = coords[i].first + 100;
                float y = coords[i].second + 100;
                result.clear();
                temp += t.query(bgi::nearest(P(x, y), neighbours_count), std::back_inserter(result));
            }
            dur_t time = clock_t::now() - start;
            std::cout << time << " - query(nearest(P, " << neighbours_count << ")) " << nearest_queries_count << " found " << temp << '\n';
        }

        {
            clock_t::time_point start = clock_t::now();
            for (size_t i = 0 ; i < values_count / 10 ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                B b(P(x - 0.5f, y - 0.5f), P(x + 0.5f, y + 0.5f));

                t.remove(b);
            }
            dur_t time = clock_t::now() - start;
            std::cout << time << " - remove " << values_count / 10 << '\n';
        }

        std::cout << "------------------------------------------------\n";
    }

    return 0;
}


// Copyright (c) 2005 - 2014 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/shared_ptr.hpp>
#include <GeomLib.hpp>
//#include "../UnitDynamicLib/SinglePopulationNetworkFixtureCode.h"
//#include "AlgorithmInputSimulationFixture.h"

using namespace GeomLib;
using namespace MPILib;

//BOOST_FIXTURE_TEST_SUITE( s, SinglePopulationNetworkFixture<OrnsteinUhlenbeckConnection> )

/*
//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( QIF_Algorithm_Create_And_Copy )
{

	BOOST_REQUIRE( true );
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE(InitializeAlgorithm)
{
	Potential gamma		=  0.5;
	Potential V_reset	=  0.0;
	Potential V_peak	= 10.0;
	Potential V_min		= -5.0;
	Time t_mem			= 10e-3;
	Time t_ref			= 0.0;

	QifParameter
		par_qif
		(
			gamma
		);

	InitialDensityParameter par_dense(0.0,1e10);

	PopulationParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
	Number n_bins = 400;

	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
         	par_dense
		);

	SpikingQifNeuralDynamics dyn(par_ode, par_qif);
	QifOdeSystem sys(dyn);

	GeomAlgorithm<MPILib::populist::OrnsteinUhlenbeckConnection> alg(sys);
}
/*
BOOST_AUTO_TEST_CASE(SingleInputTest)
{

	Potential V_min		= -10.0;
	Potential V_peak	=  10.0;
	Potential V_reset	= -10.0;
	Potential gamma		=  0.5;
	Time t_mem			=  10e-3;
	Time  t_ref			= 0.0;

	QifParameter
		par_qif
		(
			gamma
		);

	Number n_bins = 200; // 100 bins not enough; triggers sigma correction

	PopulationParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);

	InitialDensityParameter par_dense(0.0,1e10);
	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
		    par_dense
		);

	SpikingQifNeuralDynamics dyn(par_ode, par_qif);
	QIFOdeSystem sys(dyn);
	GeomAlgorithm<PopulationConnection> alg_qif(sys);

	DynamicNode<PopulationConnection> node(alg_qif,DynamicLib::EXCITATORY_DIRECT);

	RateAlgorithm<PopulationConnection> alg_e(1.0);
	RateAlgorithm<PopulationConnection> alg_i(1.0);

	DynamicNode<PopulationConnection> node_e(alg_e,DynamicLib::EXCITATORY);
	DynamicNode<PopulationConnection> node_i(alg_i,DynamicLib::INHIBITORY);

	PopulationConnection con_e(1.0,1.0);
	PopulationConnection con_i(1.0,-1.0);

	DynamicNode<PopulationConnection>::connection c_e;
	c_e.first  = &node_e;
	c_e.second = con_e;

	DynamicNode<PopulationConnection>::connection c_i;
	c_i.first = &node_i;
	c_i.second = con_i;

	node.PushBackConnection(c_e);
	node.PushBackConnection(c_i);

	node_e.SetValue(1.0);
	node_i.SetValue(1.0);

	node.CollectExternalInput();

}
*/
//-----------------------------------------------
/*BOOST_AUTO_TEST_CASE(NoInputNetworkTest)
{

	Potential V_min		 = -10.0;
	Potential V_peak	 =  10.0;
	Potential V_reset	 = -10.0;

	Potential V_reversal = 0.0;

	Potential gamma		 = 0.5;
	Time t_mem	       	 = 10e-3;
	Time t_ref	       	 = 0.0;
	QifParameter
		par_qif
		(
			gamma
		);

	Number n_bins = 100;

	PopulationParameter par_neuron(V_peak, V_reset, V_reversal, t_ref, t_mem);

	InitialDensityParameter par_dense(V_min,0.0);
	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
			par_dense
		);

	SpikingQifNeuralDynamics dyn(par_ode, par_qif);
	QifOdeSystem sys(dyn);

	DiffusionParameter par_diff(0.03,0.05);
	CurrentCompensationParameter par_curr(par_qif.Gammasys(), 0.01);
	GeomParameter par_geom(sys,sqrt(gamma), par_diff, par_curr,"NumericalMasterEquation", true);

	GeomAlgorithm<MPILib::populist::OrnsteinUhlenbeckConnection> alg_qif(par_geom);

	Time t_period = dyn.TimeToInf(V_min);
	Time t_step   = t_period/n_bins;

	// the grid_before is empty because no report has been triggered at t = 0;
	// this is normal behaviour
	AlgorithmGrid grid_before = alg_qif.getGrid();

	vector<MPILib::Rate> vec_rate;
	vec_rate.push_back(0.0);
	vector<MPILib::populist::OrnsteinUhlenbeckConnection> vec_con;
	MPILib::populist::OrnsteinUhlenbeckConnection con(1.0, 1.0, 0.0);
	vec_con.push_back(con);

	alg_qif.evolveNodeState(vec_rate, vec_con, t_step);

	AlgorithmGrid grid_after1 = alg_qif.getGrid();
	vector<double> vec_dense1 = grid_after1.toStateVector();

	BOOST_CHECK(vec_dense1[0] == 0);
	BOOST_CHECK(vec_dense1[1] != 0);
	BOOST_CHECK(vec_dense1[2] == 0);

	alg_qif.evolveNodeState(vec_rate, vec_con, 2*t_step);
	AlgorithmGrid grid_after2  = alg_qif.getGrid();
	vector<double> vec_dense2 = grid_after2.toStateVector();

	BOOST_CHECK(vec_dense2[1] == 0);
	BOOST_CHECK(vec_dense2[2] != 0);
	BOOST_CHECK(vec_dense2[3] == 0);
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE(NonStandardResetTest){

	Potential V_min   = -10.0;
	Potential V_peak  =  10.0;
	Potential V_reset =   0.0;
	Potential gamma	  =   0.5;

	Potential V_reversal = 0.0;

	Time t_mem	  =  10e-3;
	Time t_ref	  =   0.0;

	QifParameter
		par_qif
		(
			gamma
		);

	Number n_bins = 100;
	InitialDensityParameter par_dense(V_min,0.0);

	PopulationParameter par_neuron(V_peak, V_reset, V_reversal, t_ref, t_mem);

	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
			par_dense
		);

	DiffusionParameter par_diff(0.03,0.05);
	CurrentCompensationParameter par_curr(par_qif.Gammasys(),0.01);
	SpikingQifNeuralDynamics dyn(par_ode, par_qif);
	QifOdeSystem sys(dyn);
	GeomAlgorithm<MPILib::populist::OrnsteinUhlenbeckConnection> alg_qif(GeomParameter(sys, sqrt(gamma), par_diff, par_curr, "NumericalMasterEquation", true));

	vector<MPILib::Rate> vec_rate;
	vec_rate.push_back(0.0);
	vector<MPILib::populist::OrnsteinUhlenbeckConnection> vec_con;
	MPILib::populist::OrnsteinUhlenbeckConnection con(1.0, 1.0, 0.0);
	vec_con.push_back(con);

	Time t_period = dyn.TimeToInf(V_min);
	Time t_step   = t_period/n_bins;
	alg_qif.evolveNodeState(vec_rate, vec_con, (n_bins-1)*t_step);
	AlgorithmGrid grid_before_reset  = alg_qif.getGrid();
	vector<Density> vec_dense = grid_before_reset.toStateVector();
	BOOST_CHECK(vec_dense[n_bins-1] != 0);

	alg_qif.evolveNodeState(vec_rate, vec_con, n_bins*t_step);
	AlgorithmGrid grid_after_reset = alg_qif.getGrid();
	vec_dense = grid_after_reset.toStateVector();
	BOOST_CHECK(vec_dense[51] != 0);
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE(StandardResetTest){

	Potential V_min   = -10.0;
	Potential V_peak  =  10.0;
	Potential V_reset = -10.0;
	Potential gamma	  =   0.5;

	Potential V_reversal = 0.0;

	Time t_mem	  =  10e-3;
	Time t_ref	  =   0.0;

	QifParameter
		par_qif
		(
			gamma
		);

	Number n_bins = 100;
	InitialDensityParameter par_dense(V_min,0.0);

	PopulationParameter par_neuron(V_peak, V_reset, V_reversal, t_ref, t_mem);
	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
			par_dense
		);

	DiffusionParameter par_diff(0.03,0.05);
	CurrentCompensationParameter par_curr(par_qif.Gammasys(),0.01);
	SpikingQifNeuralDynamics dyn(par_ode, par_qif);
	QifOdeSystem sys(dyn);
	GeomAlgorithm<MPILib::populist::OrnsteinUhlenbeckConnection> alg_qif(GeomParameter(sys,sqrt(gamma),par_diff,par_curr,"NumericalMasterEquation",true));

	vector<MPILib::Rate> vec_rate;
	vec_rate.push_back(0.0);
	vector<MPILib::populist::OrnsteinUhlenbeckConnection> vec_con;
	MPILib::populist::OrnsteinUhlenbeckConnection con(1.0, 1.0, 0.0);
	vec_con.push_back(con);

	Time t_period = dyn.TimeToInf(V_min);
	Time t_step   = t_period/n_bins;
	alg_qif.evolveNodeState(vec_rate, vec_con, (n_bins-1)*t_step);
	AlgorithmGrid grid_before_reset = alg_qif.getGrid();
	vector<Density> vec_dense = grid_before_reset.toStateVector();
	BOOST_CHECK(vec_dense[n_bins-1] != 0);

	alg_qif.evolveNodeState(vec_rate, vec_con, n_bins*t_step);
	AlgorithmGrid grid_after_reset = alg_qif.getGrid();
	vec_dense = grid_after_reset.toStateVector();

	BOOST_CHECK(vec_dense[1] != 0.0);
}

/*
BOOST_AUTO_TEST_CASE(LIFSingleInputTest){

	Potential V_min	     =  0.0;
	Potential V_peak     =  1.0;
	Potential V_reset    =  0.0;
	Potential V_reversal =  0.0;
	Time t_mem	         =  50e-3;
	Time t_ref	         =  0.0;

	Number n_bins = 300;

	PopulationParameter par_neuron(V_peak, V_reset, V_reversal, t_ref, t_mem);

	InitialDensityParameter par_dense(V_min,0.0);
	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
		    par_dense
		);

	LifNeuralDynamics dyn(par_ode, 0.01);
	LeakingOdeSystem sys(dyn);
	GeomAlgorithm<PopulationConnection> alg_geom(sys);

	AlgorithmInputSimulationFixture<OU_Connection> fixt(800.,0.03);
	pair<predecessor_iterator, predecessor_iterator>  par = fixt.GenerateInput();

	Time t_step = sys.TStep();
	alg_geom.EvolveNodeState(par.first, par.second, t_step);

}

*//*
BOOST_AUTO_TEST_CASE(LIFSingleInputTestWithNegativePotential){

	Potential V_min	     = -1.0;
	Potential V_peak     =  1.0;
	Potential V_reset    =  0.0;
	Potential V_reversal =  0.0;
	Time t_mem	         =  50e-3;
	Time t_ref	         =  0.0;

	Number n_bins = 300;

	NeuronParameter par_neuron(V_peak, V_reset, V_reversal, t_ref, t_mem);

	InitialDensityParameter par_dense(V_min,0.0);
	OdeParameter
		par_ode
		(
			n_bins,
			V_min,
			par_neuron,
			par_dense
		);

	LifNeuralDynamics dyn(par_ode, 0.01);
	LeakingOdeSystem sys(dyn);
	GeomAlgorithm<DelayedConnection> alg_geom(sys);

	AlgorithmInputSimulationFixture<OU_Connection> fixt(800.,0.03);
	pair<predecessor_iterator, predecessor_iterator>  par = fixt.GenerateInput();

	Time t_step = sys.TStep();
	alg_geom.EvolveNodeState(par.first, par.second, t_step);

}
//____________________________________________________________________________//

BOOST_AUTO_TEST_SUITE_END()
*/