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

#include <dlib/statistics.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include "tester.h"
#include <dlib/pixel.h>
#include <dlib/svm_threaded.h>
#include <dlib/array.h>
#include <dlib/set_utils.h>
#include <dlib/array2d.h>
#include <dlib/image_keypoint.h>
#include <dlib/image_processing.h>
#include <dlib/image_transforms.h>

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

    logger dlog("test.object_detector");

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

    struct funny_image
    {
        array2d<unsigned char> img;
        long nr() const { return img.nr(); }
        long nc() const { return img.nc(); }
    };

    void swap(funny_image& a, funny_image& b)
    {
        a.img.swap(b.img);
    }

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

    template <
        typename image_array_type,
        typename detector_type
        >
    void validate_some_object_detector_stuff (
        const image_array_type& images,
        detector_type& detector,
        double eps = 1e-10
    )
    {
        for (unsigned long i = 0; i < images.size(); ++i)
        {
            std::vector<rectangle> dets = detector(images[i]);
            std::vector<std::pair<double,rectangle> > dets2;

            detector(images[i], dets2);

            matrix<double,0,1> psi(detector.get_w().size());
            matrix<double,0,1> psi2(detector.get_w().size());
            const double thresh = detector.get_w()(detector.get_w().size()-1);

            DLIB_TEST(dets.size() == dets2.size());
            for (unsigned long j = 0; j < dets.size(); ++j)
            {
                DLIB_TEST(dets[j] == dets2[j].second);

                const full_object_detection fdet = detector.get_scanner().get_full_object_detection(dets[j], detector.get_w());
                psi = 0;
                detector.get_scanner().get_feature_vector(fdet, psi);

                double check_score = dot(psi,detector.get_w()) - thresh;
                DLIB_TEST_MSG(std::abs(check_score - dets2[j].first) < eps, std::abs(check_score - dets2[j].first) << "  check_score: "<< check_score);
            }

        }
    }

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

    class very_simple_feature_extractor : noncopyable
    {
        /*!
        WHAT THIS OBJECT REPRESENTS
            This object is a feature extractor which goes to every pixel in an image and
            produces a 32 dimensional feature vector.  This vector is an indicator vector
            which records the pattern of pixel values in a 4-connected region.  So it should
            be able to distinguish basic things like whether or not a location falls on the
            corner of a white box, on an edge, in the middle, etc.

            Note that this object also implements the interface defined in dlib/image_keypoint/hashed_feature_image_abstract.h.
            This means all the member functions in this object are supposed to behave as
            described in the hashed_feature_image specification.  So when you define your own
            feature extractor objects you should probably refer yourself to that documentation
            in addition to reading this example program.
        !*/

    public:

        inline void load (
            const funny_image& img_
        )
        {
            const array2d<unsigned char>& img = img_.img;

            feat_image.set_size(img.nr(), img.nc());
            assign_all_pixels(feat_image,0);
            for (long r = 1; r+1 < img.nr(); ++r)
            {
                for (long c = 1; c+1 < img.nc(); ++c)
                {
                    unsigned char f = 0;
                    if (img[r][c])   f |= 0x1;
                    if (img[r][c+1]) f |= 0x2;
                    if (img[r][c-1]) f |= 0x4;
                    if (img[r+1][c]) f |= 0x8;
                    if (img[r-1][c]) f |= 0x10;

                    // Store the code value for the pattern of pixel values in the 4-connected
                    // neighborhood around this row and column.
                    feat_image[r][c] = f;
                }
            }
        }

        inline void load (
            const array2d<unsigned char>& img
        )
        {
            feat_image.set_size(img.nr(), img.nc());
            assign_all_pixels(feat_image,0);
            for (long r = 1; r+1 < img.nr(); ++r)
            {
                for (long c = 1; c+1 < img.nc(); ++c)
                {
                    unsigned char f = 0;
                    if (img[r][c])   f |= 0x1;
                    if (img[r][c+1]) f |= 0x2;
                    if (img[r][c-1]) f |= 0x4;
                    if (img[r+1][c]) f |= 0x8;
                    if (img[r-1][c]) f |= 0x10;

                    // Store the code value for the pattern of pixel values in the 4-connected
                    // neighborhood around this row and column.
                    feat_image[r][c] = f;
                }
            }
        }

        inline unsigned long size () const { return feat_image.size(); }
        inline long nr () const { return feat_image.nr(); }
        inline long nc () const { return feat_image.nc(); }

        inline long get_num_dimensions (
        ) const
        {
            // Return the dimensionality of the vectors produced by operator()
            return 32;
        }

        typedef std::vector<std::pair<unsigned int,double> > descriptor_type;

        inline const descriptor_type& operator() (
            long row,
            long col
        ) const
            /*!
                requires
                    - 0 <= row < nr()
            - 0 <= col < nc()
                ensures
                    - returns a sparse vector which describes the image at the given row and column.
                      In particular, this is a vector that is 0 everywhere except for one element.
            !*/
        {
            feat.clear();
            const unsigned long only_nonzero_element_index = feat_image[row][col];
            feat.push_back(make_pair(only_nonzero_element_index,1.0));
            return feat;
        }

        // This block of functions is meant to provide a way to map between the row/col space taken by
        // this object's operator() function and the images supplied to load().  In this example it's trivial.
        // However, in general, you might create feature extractors which don't perform extraction at every
        // possible image location (e.g. the hog_image) and thus result in some more complex mapping.
        inline const rectangle get_block_rect       ( long row, long col) const { return centered_rect(col,row,3,3); }
        inline const point image_to_feat_space      ( const point& p) const { return p; }
        inline const rectangle image_to_feat_space  ( const rectangle& rect) const { return rect; }
        inline const point feat_to_image_space      ( const point& p) const { return p; }
        inline const rectangle feat_to_image_space  ( const rectangle& rect) const { return rect; }

        inline friend void serialize   ( const very_simple_feature_extractor& item, std::ostream& out)  { serialize(item.feat_image, out); }
        inline friend void deserialize ( very_simple_feature_extractor& item, std::istream& in ) { deserialize(item.feat_image, in); }

        void copy_configuration ( const very_simple_feature_extractor& ){}

    private:
        array2d<unsigned char> feat_image;

        // This variable doesn't logically contribute to the state of this object.  It is here
        // only to avoid returning a descriptor_type object by value inside the operator() method.
        mutable descriptor_type feat;
    };

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

    template <
        typename image_array_type
        >
    void make_simple_test_data (
        image_array_type& images,
        std::vector<std::vector<rectangle> >& object_locations
    )
    {
        images.clear();
        object_locations.clear();

        images.resize(3);
        images[0].set_size(400,400);
        images[1].set_size(400,400);
        images[2].set_size(400,400);

        // set all the pixel values to black
        assign_all_pixels(images[0], 0);
        assign_all_pixels(images[1], 0);
        assign_all_pixels(images[2], 0);

        // Now make some squares and draw them onto our black images. All the
        // squares will be 70 pixels wide and tall.

        std::vector<rectangle> temp;
        temp.push_back(centered_rect(point(100,100), 70,70));
        fill_rect(images[0],temp.back(),255); // Paint the square white
        temp.push_back(centered_rect(point(200,300), 70,70));
        fill_rect(images[0],temp.back(),255); // Paint the square white
        object_locations.push_back(temp);

        temp.clear();
        temp.push_back(centered_rect(point(140,200), 70,70));
        fill_rect(images[1],temp.back(),255); // Paint the square white
        temp.push_back(centered_rect(point(303,200), 70,70));
        fill_rect(images[1],temp.back(),255); // Paint the square white
        object_locations.push_back(temp);

        temp.clear();
        temp.push_back(centered_rect(point(123,121), 70,70));
        fill_rect(images[2],temp.back(),255); // Paint the square white
        object_locations.push_back(temp);

        // corrupt each image with random noise just to make this a little more
        // challenging
        dlib::rand rnd;
        for (unsigned long i = 0; i < images.size(); ++i)
        {
            for (long r = 0; r < images[i].nr(); ++r)
            {
                for (long c = 0; c < images[i].nc(); ++c)
                {
                    typedef typename image_array_type::type image_type;
                    typedef typename image_type::type type;
                    images[i][r][c] = (type)put_in_range(0,255,images[i][r][c] + 10*rnd.get_random_gaussian());
                }
            }
        }
    }

    template <
        typename image_array_type
        >
    void make_simple_test_data (
        image_array_type& images,
        std::vector<std::vector<full_object_detection> >& object_locations
    )
    {
        images.clear();
        object_locations.clear();

        images.resize(3);
        images[0].set_size(400,400);
        images[1].set_size(400,400);
        images[2].set_size(400,400);

        // set all the pixel values to black
        assign_all_pixels(images[0], 0);
        assign_all_pixels(images[1], 0);
        assign_all_pixels(images[2], 0);

        // Now make some squares and draw them onto our black images. All the
        // squares will be 70 pixels wide and tall.
        const int shrink = 0;
        std::vector<full_object_detection> temp;

        rectangle rect = centered_rect(point(100,100), 70,71);
        std::vector<point> movable_parts;
        movable_parts.push_back(shrink_rect(rect,shrink).tl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).tr_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).bl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).br_corner());
        temp.push_back(full_object_detection(rect, movable_parts));
        fill_rect(images[0],rect,255); // Paint the square white

        rect = centered_rect(point(200,200), 70,71);
        movable_parts.clear();
        movable_parts.push_back(shrink_rect(rect,shrink).tl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).tr_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).bl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).br_corner());
        temp.push_back(full_object_detection(rect, movable_parts));
        fill_rect(images[0],rect,255); // Paint the square white

        object_locations.push_back(temp);
        // ------------------------------------
        temp.clear();

        rect = centered_rect(point(140,200), 70,71);
        movable_parts.clear();
        movable_parts.push_back(shrink_rect(rect,shrink).tl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).tr_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).bl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).br_corner());
        temp.push_back(full_object_detection(rect, movable_parts));
        fill_rect(images[1],rect,255); // Paint the square white

        rect = centered_rect(point(303,200), 70,71);
        movable_parts.clear();
        movable_parts.push_back(shrink_rect(rect,shrink).tl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).tr_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).bl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).br_corner());
        temp.push_back(full_object_detection(rect, movable_parts));
        fill_rect(images[1],rect,255); // Paint the square white

        object_locations.push_back(temp);
        // ------------------------------------
        temp.clear();

        rect = centered_rect(point(123,121), 70,71);
        movable_parts.clear();
        movable_parts.push_back(shrink_rect(rect,shrink).tl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).tr_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).bl_corner());
        movable_parts.push_back(shrink_rect(rect,shrink).br_corner());
        temp.push_back(full_object_detection(rect, movable_parts));
        fill_rect(images[2],rect,255); // Paint the square white

        object_locations.push_back(temp);

        // corrupt each image with random noise just to make this a little more
        // challenging
        dlib::rand rnd;
        for (unsigned long i = 0; i < images.size(); ++i)
        {
            for (long r = 0; r < images[i].nr(); ++r)
            {
                for (long c = 0; c < images[i].nc(); ++c)
                {
                    typedef typename image_array_type::type image_type;
                    typedef typename image_type::type type;
                    images[i][r][c] = (type)put_in_range(0,255,images[i][r][c] + 40*rnd.get_random_gaussian());
                }
            }
        }
    }

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

    void test_fhog_pyramid (
    )
    {
        print_spinner();
        dlog << LINFO << "test_fhog_pyramid()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef scan_fhog_pyramid<pyramid_down<2> > image_scanner_type;
        image_scanner_type scanner;
        scanner.set_detection_window_size(35,35);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector, 1e-6);
        }

        {
            std::vector<object_detector<image_scanner_type> > detectors;
            detectors.push_back(detector);
            detectors.push_back(detector);
            detectors.push_back(detector);

            std::vector<rectangle> dets1 = evaluate_detectors(detectors, images[0]);
            std::vector<rectangle> dets2 = detector(images[0]);
            DLIB_TEST(dets1.size() > 0);
            DLIB_TEST(dets2.size()*3 == dets1.size());
            dlib::set<rectangle>::kernel_1a_c d1, d2;
            for (unsigned long i = 0; i < dets1.size(); ++i)
            {
                if (!d1.is_member(dets1[i]))
                    d1.add(dets1[i]);
            }
            for (unsigned long i = 0; i < dets2.size(); ++i)
            {
                if (!d2.is_member(dets2[i]))
                    d2.add(dets2[i]);
            }
            DLIB_TEST(d1.size() == d2.size());
            DLIB_TEST(set_intersection_size(d1,d2) == d1.size());
        }
    }

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

    void test_1 (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<hog_image<3,3,1,4,hog_signed_gradient,hog_full_interpolation> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<2>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,35*35);
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2));
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1_boxes (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_boxes()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<hog_image<3,3,1,4,hog_signed_gradient,hog_full_interpolation> > feature_extractor_type;
        typedef scan_image_boxes<feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1m (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1m()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<full_object_detection> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<hog_image<3,3,1,4,hog_signed_gradient,hog_full_interpolation> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<2>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,35*35);
        std::vector<rectangle> mboxes;
        const int mbox_size = 20;
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,1,1), mboxes);
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1_fine_hog (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_fine_hog()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<fine_hog_image<3,3,2,4,hog_signed_gradient> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<2>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,35*35);
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2));
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1_poly (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_poly()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<poly_image<2> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<2>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,35*35);
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2));
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1m_poly (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_poly()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<full_object_detection> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef hashed_feature_image<poly_image<2> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<3>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,35*35);
        std::vector<rectangle> mboxes;
        const int mbox_size = 20;
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        mboxes.push_back(centered_rect(0,0, mbox_size,mbox_size));
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2), mboxes);
        setup_hashed_features(scanner, images, 9);
        use_uniform_feature_weights(scanner);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        trainer.set_overlap_tester(test_box_overlap(0,0));
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1_poly_nn (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_poly_nn()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef nearest_neighbor_feature_image<poly_image<5> > feature_extractor_type;
        typedef scan_image_pyramid<pyramid_down<2>, feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;

        setup_grid_detection_templates(scanner, object_locations, 2, 2);
        feature_extractor_type nnfe;
        pyramid_down<2> pyr_down;
        poly_image<5> polyi;
        nnfe.set_basis(randomly_sample_image_features(images, pyr_down, polyi, 80));
        scanner.copy_configuration(nnfe);

        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_1_poly_nn_boxes (
    )
    {
        print_spinner();
        dlog << LINFO << "test_1_poly_nn_boxes()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef nearest_neighbor_feature_image<poly_image<5> > feature_extractor_type;
        typedef scan_image_boxes<feature_extractor_type> image_scanner_type;
        image_scanner_type scanner;

        feature_extractor_type nnfe;
        pyramid_down<2> pyr_down;
        poly_image<5> polyi;
        nnfe.set_basis(randomly_sample_image_features(images, pyr_down, polyi, 80));
        scanner.copy_configuration(nnfe);

        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);

            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    void test_2 (
    )
    {
        print_spinner();
        dlog << LINFO << "test_2()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        grayscale_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images, object_locations);

        typedef scan_image_pyramid<pyramid_down<5>, very_simple_feature_extractor> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,70*70);
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2));
        scanner.set_max_pyramid_levels(1);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(0);
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        res = cross_validate_object_detection_trainer(trainer, images, object_locations, 3);
        dlog << LINFO << "3-fold cross validation (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);
            validate_some_object_detector_stuff(images, detector);
        }
    }

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

    class pyramid_down_funny : noncopyable
    {
        pyramid_down<2> pyr;
    public:

        template <typename T>
        dlib::vector<double,2> point_down ( const dlib::vector<T,2>& p) const { return pyr.point_down(p); }

        template <typename T>
        dlib::vector<double,2> point_up ( const dlib::vector<T,2>& p) const { return pyr.point_up(p); }

        template <typename T>
        dlib::vector<double,2> point_down ( const dlib::vector<T,2>& p, unsigned int levels) const { return pyr.point_down(p,levels); }

        template <typename T>
        dlib::vector<double,2> point_up ( const dlib::vector<T,2>& p, unsigned int levels) const { return pyr.point_up(p,levels); }

        rectangle rect_up ( const rectangle& rect) const { return pyr.rect_up(rect); }

        rectangle rect_up ( const rectangle& rect, unsigned int levels) const { return pyr.rect_up(rect,levels); }

        rectangle rect_down ( const rectangle& rect) const { return pyr.rect_down(rect); }

        rectangle rect_down ( const rectangle& rect, unsigned int levels) const { return pyr.rect_down(rect,levels); }

        template <
            typename in_image_type,
            typename out_image_type
            >
        void operator() (
            const in_image_type& original,
            out_image_type& down
        ) const
        {
            pyr(original.img, down.img);
        }

    };

    // make sure everything works even when the image isn't a dlib::array2d.
    // So test with funny_image.
    void test_3 (
    )
    {
        print_spinner();
        dlog << LINFO << "test_3()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        typedef dlib::array<funny_image>  funny_image_array_type;
        grayscale_image_array_type images_temp;
        funny_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images_temp, object_locations);
        images.resize(images_temp.size());
        for (unsigned long i = 0; i < images_temp.size(); ++i)
        {
            images[i].img.swap(images_temp[i]);
        }

        typedef scan_image_pyramid<pyramid_down_funny, very_simple_feature_extractor> image_scanner_type;
        image_scanner_type scanner;
        const rectangle object_box = compute_box_dimensions(1,70*70);
        scanner.add_detection_template(object_box, create_grid_detection_template(object_box,2,2));
        scanner.set_max_pyramid_levels(1);
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        res = cross_validate_object_detection_trainer(trainer, images, object_locations, 3);
        dlog << LINFO << "3-fold cross validation (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);
        }
    }

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

    class funny_box_generator
    {
    public:
        template <typename image_type>
        void operator() (
            const image_type& img,
            std::vector<rectangle>& rects
        ) const
        {
            rects.clear();
            find_candidate_object_locations(img.img, rects);
            dlog << LINFO << "funny_box_generator, rects.size(): "<< rects.size();
        }
    };

    inline void serialize(const funny_box_generator&, std::ostream& ) {}
    inline void deserialize(funny_box_generator&, std::istream& ) {}

    // make sure everything works even when the image isn't a dlib::array2d.
    // So test with funny_image.
    void test_3_boxes (
    )
    {
        print_spinner();
        dlog << LINFO << "test_3_boxes()";

        typedef dlib::array<array2d<unsigned char> >  grayscale_image_array_type;
        typedef dlib::array<funny_image>  funny_image_array_type;
        grayscale_image_array_type images_temp;
        funny_image_array_type images;
        std::vector<std::vector<rectangle> > object_locations;
        make_simple_test_data(images_temp, object_locations);
        images.resize(images_temp.size());
        for (unsigned long i = 0; i < images_temp.size(); ++i)
        {
            images[i].img.swap(images_temp[i]);
        }

        typedef scan_image_boxes<very_simple_feature_extractor, funny_box_generator> image_scanner_type;
        image_scanner_type scanner;
        structural_object_detection_trainer<image_scanner_type> trainer(scanner);
        trainer.set_num_threads(4);
        object_detector<image_scanner_type> detector = trainer.train(images, object_locations);

        matrix<double> res = test_object_detection_function(detector, images, object_locations);
        dlog << LINFO << "Test detector (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        res = cross_validate_object_detection_trainer(trainer, images, object_locations, 3);
        dlog << LINFO << "3-fold cross validation (precision,recall): " << res;
        DLIB_TEST(sum(res) == 3);

        {
            ostringstream sout;
            serialize(detector, sout);
            istringstream sin(sout.str());
            object_detector<image_scanner_type> d2;
            deserialize(d2, sin);
            matrix<double> res = test_object_detection_function(d2, images, object_locations);
            dlog << LINFO << "Test detector (precision,recall): " << res;
            DLIB_TEST(sum(res) == 3);
        }
    }

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

    class object_detector_tester : public tester
    {
    public:
        object_detector_tester (
        ) :
            tester ("test_object_detector",
                    "Runs tests on the structural object detection stuff.")
        {}

        void perform_test (
        )
        {
            test_fhog_pyramid();
            test_1_boxes();
            test_1_poly_nn_boxes();
            test_3_boxes();

            test_1();
            test_1m();
            test_1_fine_hog();
            test_1_poly();
            test_1m_poly();
            test_1_poly_nn();
            test_2();
            test_3();
        }
    } a;

}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/nbexponentbits.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check nbexponentbits behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbexponentbits;
  using boost::simd::Nbexponentbits;

  STF_TYPE_IS(decltype(Nbexponentbits<T>()), T);
  STF_EQUAL(Nbexponentbits<T>(), T(0));
  STF_EQUAL(nbexponentbits( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check nbexponentbits behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbexponentbits;
  using boost::simd::Nbexponentbits;

  STF_TYPE_IS(decltype(Nbexponentbits<T>()), std::int64_t);
  STF_IEEE_EQUAL(Nbexponentbits<T>(), 11);
  STF_IEEE_EQUAL(nbexponentbits( as(T{}) ), 11);
}
STF_CASE_TPL( "Check nbexponentbits behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nbexponentbits;
  using boost::simd::Nbexponentbits;

  STF_TYPE_IS(decltype(Nbexponentbits<T>()), std::int32_t);
  STF_IEEE_EQUAL(Nbexponentbits<T>(), 8);
  STF_IEEE_EQUAL(nbexponentbits( as(T{}) ), 8);
}


#include <iostream>
#include <iterator>
#include <string>
#include <boost/tokenizer.hpp>
#include <typeinfo>

#include "IPv4.h"

bool IPv4::strToV4(std::string &in_) {
    std::string num;
    boost::char_separator<char> seq(".");
    boost::tokenizer<boost::char_separator<char>> tokens(in_, seq);

    uint32_t i = 0;
    for (const auto &t : tokens) {
        bool isDigit = (t.find_first_not_of("0123456789") == std::string::npos);
        if (!isDigit || i > 3 || t.length() > 3 || t.length() == 0)
            return false;
        this->addr[i++] = u_int8_t(std::stoi(t));
    }

    if (i != 4)
        return false;

    return true;
}

void IPv4::readFromConsole() {
    std::string in_;
    std::cin >> in_;

    this->saved = strToV4(in_);
    if (!this->saved)
        printf("Incorrect Format on Input\n");
}

void IPv4::writeToConsole() {
    if (this->saved)
        printf("%d.%d.%d.%d\n", this->addr[0], this->addr[1], this->addr[2], this->addr[3]);
}

IPv4& IPv4::operator++() {
    bool carry = true;
    for (uint32_t idx = 3; idx >= 0 && carry ; idx--) {
        if (this->addr[idx] == 255) {
            this->addr[idx] = 0;
            carry = true;
        }
        else {
            this->addr[idx] += 1;
            carry = false;
        }
    }
    return *this;
}

IPv4 IPv4::operator++(int) {
    IPv4 tmp = *this;
    ++*this;
    return tmp;
}

bool IPv4::operator>(const IPv4 &a) {
    for (uint32_t idx = 0 ; idx < 4 ; idx++) {
        if (this->addr[idx] > a.addr[idx])
            return true;
        else if (this->addr[idx] < a.addr[idx])
            return false;
    }
    return false;
}

bool IPv4::operator<(const IPv4 &a) {
    return !(*this > a) && !(*this == a);
}

bool IPv4::operator==(const IPv4 &a) {
    return std::equal(std::begin(this->addr),
                      std::end(this->addr),
                      std::begin(a.addr));
}


/*
 * Chrono: measure time duration between two points with boost.datetime
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: boost.datetime library
 * Usage: g++ timer.cc
 *
 */

#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>

int main() {

    for(int i=0 ; i<10 ; i++) {
        double wait_seconds = 1;
        boost::posix_time::ptime end_time = boost::posix_time::microsec_clock::local_time() + boost::posix_time::microseconds(wait_seconds * 1000000.);

        while(boost::posix_time::microsec_clock::local_time() < end_time) {
            // do nothing
        }

        std::cout << end_time << std::endl;
    }

    return 0;
}


//meshビューア

#include <ros/ros.h>
#include <stdio.h>
#include <iostream>
#include <string>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/vtk_io.h>
#include <pcl/PolygonMesh.h>

#include <boost/thread/thread.hpp>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_broadcaster.h>

#include <local_tool/filters.hpp>
#include <local_tool/mathematics.hpp>
#include <local_tool/registration.hpp>

using namespace std;

int main (int argc, char** argv)
{

	// double l_roll, l_pitch, l_yaw;//角度etc

	ros::init(argc, argv, "mesh");
	ros::NodeHandle n;
	ros::Rate roop(1);

	ros::Publisher ipt_pub = n.advertise<sensor_msgs::PointCloud2>("/input_cloud", 1);
	ros::Publisher out_pub = n.advertise<sensor_msgs::PointCloud2>("/output_cloud", 1);

	ros::Publisher mesh_pub = n.advertise<pcl_msgs::PolygonMesh>("/mesh_cloud", 1);

	// Loading first scan of room.
	pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);

	tf::TransformBroadcaster br;
	tf::Transform transform;

	string file_input;

	n.getParam("input/cloud",file_input);

	// Load input file into a PointCloud<T> with an appropriate type
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 cloud_blob;
	pcl::io::loadPCDFile(file_input, cloud_blob);
	pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
	//* the data should be available in cloud

	// Normal estimation*
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> nor;
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud (cloud);
	nor.setInputCloud (cloud);
	nor.setSearchMethod (tree);
	nor.setKSearch (20);
	nor.compute (*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud (cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	// pcl::PolygonMesh triangles;
	pcl::PolygonMesh::Ptr triangles(new pcl::PolygonMesh());

	// Set the maximum distance between connected points (maximum edge length)
	// gp3.setSearchRadius (0.025);
	gp3.setSearchRadius (0.5);

	// Set typical values for the parameters
	gp3.setMu (10);//defalut 2.5
	gp3.setMaximumNearestNeighbors (100);
	gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
	gp3.setMinimumAngle(M_PI/18); // 10 degrees
	gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
	// gp3.setNormalConsistency(false);
	gp3.setNormalConsistency(true);

	// Get result
	gp3.setInputCloud (cloud_with_normals);
	gp3.setSearchMethod (tree2);
	gp3.reconstruct (*triangles);

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();

	// pcl::io::saveVTKFile("/home/amsl/Desktop/mesh.vtk",triangles);

	cout <<triangles->polygons.size() <<"triangles created" << endl;

	pcl::fromPCLPointCloud2 (triangles->cloud, *output_cloud);
	// pcl_conversions::fromPCL (triangles->cloud, *output_cloud);

	sensor_msgs::PointCloud2 pc, pc2;
	pcl_msgs::PolygonMesh pc3;
	pcl::toROSMsg(*cloud, pc);
	pcl::toROSMsg(*output_cloud, pc2);

	pc3.cloud = pc2;

	// 点群のビューア
	pcl::visualization::PCLVisualizer viewer("Cloud Viewer");

	viewer.setBackgroundColor (0.0, 0.2, 0.6);
	// viewer.setBackgroundColor (0.0, 0.0, 0.0);

	// 原点に座標軸を追加
	// viewer.addCoordinateSystem(100.0, 0.0, 0.0, 0.0);

	// カメラ座標のセット
	// double camPos[] = { 0.0, 60.0, 350.0 };
	// double camLookAt[] = { 0.0, 60.0, 0.0 };
	// double camUp[] = { 0.0, 1.0, 0.0 };
	// viewer.setCameraPose(camPos[0], camPos[1], camPos[2],
	// 		camLookAt[0], camLookAt[1], camLookAt[2], camUp[0], camUp[1], camUp[2]);

	// ビューアに点群を追加
	//  viewer.addPointCloud(filtered_cloud);
	// viewer.addPointCloud(cloud);

	// ビューアにポリゴンメッシュを追加
	 viewer.addPolygonMesh(*triangles);

	while (!viewer.wasStopped ())
	{
		viewer.spinOnce(10);

		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
	}

	// boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	// viewer->setBackgroundColor (0, 0, 0);
	// viewer->addPolygonMesh(*triangles,"meshes",0);

	// while(ros::ok()){
	//
    //
	// 	pc.header.frame_id  = "map";
	// 	pc2.header.frame_id = "map";
	// 	pc3.header.frame_id = "map";
    //
	// 	pc.header.stamp  = ros::Time::now();
	// 	pc2.header.stamp = ros::Time::now();
	// 	pc3.header.stamp = ros::Time::now();
    //
	// 	ipt_pub.publish(pc);
	// 	out_pub.publish(pc2);
	//
	// 	// mesh_pub.publish(pc3);
    //
	// 	roop.sleep();
	// 	ros::spinOnce();
	// }

	return (0);
}


#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdlib.h>
#include <string>
#include "vector3d.h"
#include <math.h>
#include <vector>
#include <gsl/gsl_rng.h>
#include <time.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace boost::program_options;

int main(int argc, char **argv) {

    /*Replacable variables*/
    string permitivityText = "USERINPUT_E";
    string totalTimeText = "USERINPUT_TIME";
    string initFileNameText = "USERINPUT_INIT_FILENAME";
    string mpiText = "NODESIZE";

    string initFile = "infiles/P22CD40L.in";

    // Set up the ligand xyz coordinates in a vector array

    vector<VECTOR3D> ligandCoordinates;
    double x;
    double y;
    double z;

    ifstream crds;                                      //open coordinates file
    crds.open("infiles/ligandCoordinates.coords");
    if (!crds) {                                        //check to make sure file is there
        cerr << "ERR: FILE ligandCoordinates.coords NOT OPENED. Check directory and/or filename.";
        exit(1);
    }
    for (int i = 0; i < 60; i++) {
        crds >> x >> y >> z;                // Add coordinates the the vector array
        ligandCoordinates.push_back(VECTOR3D(x, y, z));
    }

    // Get important parameters from the user

    double epsilon;
    double wallSpacing;
    double concentration;
    int numberComplexes;
    double timesteps;
    bool verbose;

    options_description desc("Usage:\nrandom_mesh <options>");
    desc.add_options()
            ("help,h", "print usage message")
            ("lennard jones well depth,E", value<double>(&epsilon)->default_value(1),
             "Binding strength of the ligand-recptor interaction (KbT)")
            ("[ligand - Virus] complex concentration,C", value<double>(&concentration)->default_value(9),
             "[ligand - Virus] complex concentration (nM)") // box size adjusteded for nanomolar conc.
            ("receptor spacing,w", value<double>(&wallSpacing)->default_value(100), "receptor spacing (nm)")
            ("number of vlp-ligand complexes,S", value<int>(&numberComplexes)->default_value(108),
             "number of vlp-ligand complexes")
            ("simulation time,T", value<double>(&timesteps)->default_value(275), "simulation time (milliseconds)")
            ("verbose,V", value<bool>(&verbose)->default_value(true), "verbose true: provides detailed output");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

    //add options for ligand - virus ratio, properties of ligand / vlp, free ligand in solution

    // GENERATE P22CD40L.in FILE!!!

    // generate atoms

    ofstream datafile(initFile);

    datafile << "#Script for P22-CD40L complex. To be read into LAMMPS template. " << endl;

    datafile << "#LAMMPS  data    file " << endl;

    double vlpDiameter = 1; //reduced units
    double ligandDiameter = 0.1; //reduced units
    double wallDiameter = 0.1; //reduced units

    double boxLength;
    double wallNumberX; //number of wall particles in x direction
    double atomNumber;
    double bondNumber;
    boxLength = pow((numberComplexes * 0.001 / (6.022E23)) / (concentration * 1E-9), (1.0 / 3.0)); //in meters

    wallSpacing = wallSpacing * 1E-9; // in meters
    wallNumberX = boxLength / wallSpacing; //calculate number of mesh points
    wallNumberX = trunc(wallNumberX); // truncate this value
    cout << "adjusted spacing is " << (boxLength / wallNumberX) * 1E9 << " nm" << endl; //this is the adjusted spacing
    atomNumber = (numberComplexes * 61) + (wallNumberX * wallNumberX);
    bondNumber = numberComplexes * 180;

    double sigma = 60e-9; //sigma value currently used, diameter of P22
    double sigmaHC = 0.12; // sigma hc in reduced units
    double massSI = 3.819E-20; // mass of P22 in kg
    double epsilonSI = (1.38E-23) * (298.15); // epsilon in J
    boxLength = boxLength / sigma; // now in reduced units
    wallSpacing = boxLength / wallNumberX; // now in reduced units

    //convert simulation time to reduced units
    double tau = sigma * sqrt(massSI / epsilonSI); //1 MD timestep in seconds
    timesteps = ceil(timesteps * 1e-3 / tau);
    cout << "lammps will run for " << timesteps << " MD timesteps" << endl;

    if (boxLength < (10 * vlpDiameter)) {
        cout << "Uh oh! The z-direction is too small!" << endl;
    }

    datafile << atomNumber << " atoms" << endl;

    datafile << bondNumber << " bonds" << endl;

    datafile << "3 atom types" << endl;

    datafile << "3 bond types" << endl;

    datafile << "0" << " " << boxLength << " xlo xhi" << endl; // in reduced units...

    datafile << "0" << " " << boxLength << " ylo yhi" << endl;

    datafile << "0" << " " << boxLength << " zlo zhi" << endl;

    datafile << endl;

    //atoms section

    datafile << "Atoms" << endl << endl;

    double atomType;
    double vlpX;
    double vlpY;
    double vlpZ;
    double index = 0;
    vector<VECTOR3D> vlpXYZ;
    gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);                    //setting up random seed
    //unsigned long int Seed = 23410982;
    gsl_rng_set(r, time(NULL));        //seed with time
    VECTOR3D dist;

    ///////////////////////////////...To give vlp random positions use these...////////////////////////////////
    bool add;

    while (vlpXYZ.size() < numberComplexes) {
        add = true;
        vlpX = (gsl_rng_uniform(r)) * boxLength; // guess some coordinates
        vlpY = (gsl_rng_uniform(r)) * boxLength;
        vlpZ = (gsl_rng_uniform(r)) * boxLength;

        if (vlpZ < 2.5) add = false; // if too close to the cell wall, reject the point
        if (vlpZ > (boxLength - (vlpDiameter + ligandDiameter)))
            add = false; // if overlaps with the upper z boundary reject

        if (vlpXYZ.size() == 0) {        //if no vlps have been added yet, add it
            vlpXYZ.push_back(VECTOR3D(vlpX, vlpY, vlpX));
            index += 1;
            datafile << index << " 1 " << "1" << " " << vlpX << " " << vlpY << " " << vlpZ << " " << endl;
            for (unsigned int m = 0; m < ligandCoordinates.size(); m++) {
                index += 1;
                x = vlpX + ligandCoordinates[m].x;
                y = vlpY + ligandCoordinates[m].y;
                z = vlpZ + ligandCoordinates[m].z;
                datafile << index << " 1 " << "2" << " " << x << " " << y << " " << z << " " << endl;
            }
            add = false;
        }

        for (int j = 0; j < vlpXYZ.size(); j++) { //make sure they don't overlap with other particles
            dist.x = vlpXYZ[j].x - vlpX;
            dist.y = vlpXYZ[j].y - vlpY;
            dist.z = vlpXYZ[j].z - vlpZ;
            if (dist.x > boxLength / 2) dist.x -= boxLength; //account for periodic boundaries in x & y
            if (dist.x < -boxLength / 2) dist.x += boxLength;
            if (dist.y > boxLength / 2) dist.y -= boxLength;
            if (dist.y < -boxLength / 2) dist.y += boxLength;
            //       if (dist.z>boxLength/2) dist.z -= boxLength; 	// z is not periodic
            //       if (dist.z<-boxLength/2) dist.z += boxLength;
            //   cout << "distance is " << dist.GetMagnitude() << " btw " << j << " ( " << vlpXYZ[j].x << " , " << vlpXYZ[j].y << " , " << vlpXYZ[j].z << " ) " << " and " << index/61 << " ( " << vlpX << " , " << vlpY << " , " << vlpZ << " ) " << endl;

            if (dist.GetMagnitude() < 2.5) {  //flag it if it intersects with anything
                //cout << "too close!" << endl;
                add = false;
                break;
            }
        }

        if (add == true) {        //if it doesn't intersect, add it and its ligands to the list
            vlpXYZ.push_back(VECTOR3D(vlpX, vlpY, vlpZ));
            index += 1;
            datafile << index << "  1 " << "1" << "  " << vlpX << "  " << vlpY << "  " << vlpZ << endl;
            for (unsigned int m = 0; m < ligandCoordinates.size(); m++) {
                index += 1;
                x = vlpX + ligandCoordinates[m].x;
                y = vlpY + ligandCoordinates[m].y;
                z = vlpZ + ligandCoordinates[m].z;
                datafile << index << "  1 " << "2" << "  " << x << "  " << y << "  " << z << endl;
            }
        } //else cout << "rejected!" << endl;

    }

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

//Making wall particles
    for (unsigned int i = 0; i < wallNumberX; i++) {
        for (unsigned int j = 0; j < wallNumberX; j++) {
            index += 1;
            datafile << index << " 1 " << "3" << "  " << i * wallSpacing << "  " << j * wallSpacing << "  " << "0"
                     << endl;
        }
    }

    cout << "Done making atoms!" << endl;

// Generate bonds

    vector<VECTOR3D> ligandBonds;
    int type;
    int b1;
    int b2;

    ifstream bonds;                                      //open coordinates file
    bonds.open("infiles/ligandBonds.coords");
    if (!bonds) {                                        //check to make sure file is there
        cerr << "ERR: FILE ligandBonds.coords NOT OPENED. Check directory and/or filename.";
        exit(1);
    }
    for (int i = 0; i < 180; i++) {
        bonds >> index >> type >> b1 >> b2;                // Add coordinates the the vector array
        ligandBonds.push_back(VECTOR3D(type, b1, b2));
    }

    datafile << endl << "Bonds" << endl << endl;

    index = 0;

    for (int i = 0; i < numberComplexes; i++) {
        for (int j = 0; j < ligandBonds.size(); j++) {
            index += 1;
            datafile << index << "     " << ligandBonds[j].x << "     " << ligandBonds[j].y + (i * 61) << "     "
                     << ligandBonds[j].z + (i * 61) << endl;
        }

    }

//bond type 1 length
    dist = VECTOR3D(0, 0, 0) - ligandCoordinates[0];
    double lengthType1 = dist.GetMagnitude();
//bond type 2 length
    dist = ligandCoordinates[0] - ligandCoordinates[1];
    double lengthType2 = dist.GetMagnitude();
//bond type 3 length
    dist = ligandCoordinates[0] - ligandCoordinates[20];
    double lengthType3 = dist.GetMagnitude();

    cout << "Done making bonds!" << endl;

/*************************Generating the LAMMPS file*************************/

    ofstream inputScript("in.lammps", ios::trunc);
    if (inputScript.is_open()) {

        /*Open the template file*/
        string line;
        ifstream inputTemplate("infiles/in.lammps.template", ios::in);
        if (inputTemplate.is_open()) {
            while (getline(inputTemplate, line)) {

                std::size_t found = line.find(permitivityText);
                if (found != std::string::npos)
                    line.replace(found, permitivityText.length(),  std::to_string(epsilon));

                found = line.find(totalTimeText);
                if (found != std::string::npos)
                    line.replace(found, totalTimeText.length(), std::to_string(int(timesteps)));

                found = line.find(initFileNameText);
                if (found != std::string::npos)
                    line.replace(found, initFileNameText.length(), initFile);

                inputScript << line << endl;
            }
            inputTemplate.close();
        } else cout << "Unable to open the template input script" << endl;
        inputScript.close();
    } else cout << "Unable create a input Script" << endl;

/*
//GENERATE LAMMPS INPUT SCRIPT

    ofstream outfile("lammps_script.in");

    outfile << "#Lammps input script for nanoparticle binding with multivalency #" << endl << endl;

    outfile << "units lj" << endl << "boundary p p f" << endl << "atom_style bond " << endl << "neighbor 0.3 bin"
            << endl << "neigh_modify every 1 delay 0 check yes" << endl << endl;

//Creating simulation box

    outfile << "## Create Simulation Box, Atoms ##" << endl;

    outfile << "read_data	   P22CD40L.in" << endl << endl;

    outfile << "group vlp type 1" << endl << "group ligand type 2" << endl << "group wall type 3" << endl << endl;

//Defining particle properties

    outfile << "## Defining Particle/Medium Properties ##" << endl;

    outfile << "mass	1	1	# reduced mass of vlp P22" << endl;

    outfile << "mass	2	0.002	# reduced mass of ligand " << endl;

    outfile << "mass	3	1	# mass of wall, irrelevant " << endl << endl;

    outfile << "## Ascribing Initial Velocities ##" << endl;

    outfile << "velocity	all	create	1.0	4928459	rot	yes	dist	gaussian	units	box" << endl
            << endl;

    outfile << "## Fixing Wall particles ##" << endl;

    outfile << "fix zwalls all wall/reflect zhi EDGE zlo EDGE" << endl << endl;

//Interparticle potentials

    outfile << "## Ascribing interparticle potentials: ##" << endl << endl;

    outfile << "pair_style      hybrid lj/expand        2.5     lj/cut 2.5" << endl;

    outfile << "pair_coeff      1       1       lj/cut           1      " << vlpDiameter << "	"
            << vlpDiameter * 1.12246 << "       # epsilon, sigma, cut-off  V-V" << endl;

    outfile << "pair_coeff      1       2       lj/expand        1      " << sigmaHC << "     "
            << (vlpDiameter + ligandDiameter) / 2 - sigmaHC << "	" << (vlpDiameter + ligandDiameter) / 2 * 1.12246
            << "  # epsilon, sigmaHC, delta_V-L, cut-off" << endl;

    outfile << "pair_coeff      2       2       lj/cut           1      " << ligandDiameter << "     "
            << ligandDiameter * 1.12246 << "       # L-L" << endl;

    outfile << "pair_coeff      1       3       lj/expand        1      " << sigmaHC << "     "
            << (vlpDiameter + wallDiameter) / 2 - sigmaHC << "	" << (vlpDiameter + wallDiameter) / 2 * 1.12246
            << "  # V-W" << endl;

    outfile << "pair_coeff      2       3       lj/cut       " << epsilon << "	   " << ligandDiameter << "	"
            << "2.5" << "  # L-W" << endl;

    outfile << "pair_coeff      3       3       lj/cut           0      0               # W-W" << endl << endl;

    outfile << "#pair_modify  shift   yes     # the additive e_LJ for repulsion-only" << endl << endl;

//Defining bond properties

    outfile << "## Making bonds ##" << endl;

    outfile << "bond_style harmonic" << endl;

    outfile << "bond_coeff 1    1000.0  " << lengthType1 << "  # type, K, distance" << endl;

    outfile << "bond_coeff 2    1000.0  " << lengthType2 << endl;

    outfile << "bond_coeff 3    1000.0  " << lengthType3 << endl << endl;

//Ensemble fixes

    outfile << "## Ensemble Fixes (+ for output) ##" << endl;

    outfile << "variable        myTStep equal   0.00005 # timestep where 1 MD step is "
            << sigma * sqrt((massSI) / (epsilonSI)) << " seconds" << endl;

    outfile << "timestep        ${myTStep}" << endl;

    outfile << "variable        myDStep equal   1000" << endl << endl;

    outfile << "fix     ens     vlp     nvt     temp    1.      1.      0.005" << endl;

    outfile << "fix     ens2    ligand  nvt     temp    1.      1.      0.005  # T_start, T_stop, T_damp=100*timestep"
            << endl;

    outfile << "fix_modify ens energy yes  # adds thermostat energy to potential" << endl;

    outfile << "fix_modify ens2 energy yes" << endl << endl;

//Output dump

    outfile << "## Initial Image Dump ##" << endl;

    outfile << "dump mymovie all custom 100 ovitomovie.melt id type x y z" << endl << endl;

    outfile << "print \"OUTPUT COLUMNS: SIMULATION STEP NUMBER | TEMPERATURE | NP POTENTIAL ENERGY | SYSTEM VOLUME\" "
            << endl << endl;

    outfile << "thermo_style   custom  step    temp   etotal  ke      pe     #print info to file" << endl;

    outfile << "thermo  50000" << endl << endl;

    outfile << fixed;

    outfile << "run     " << int(timesteps) << endl << endl;

    outfile << "## Defining Output Information ##" << endl;

    outfile
            << "dump    posD    all     custom  ${myDStep}      outfiles/dump.melt      id      type    x       y       z       # c_atomPot     c_atomKin"
            << endl << endl;

    outfile << " unfix   ens" << endl;

    outfile << "unfix   ens2" << endl;

    outfile << "undump  posD" << endl;

    outfile << "undump mymovie" << endl;

    outfile << "#shell          echo \"Lammps Simulation Ended\" " << endl;

*/

    gsl_rng_free(r);

    return 0;

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps Rot2 class to python
 * @author Andrew Melim
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include "gtsam/geometry/Rot2.h"

using namespace boost::python;
using namespace gtsam;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3)

void exportRot2(){

  class_<Rot2>("Rot2", init<>())
    .def(init<double>())
    .def("Expmap", &Rot2::Expmap)
    .staticmethod("Expmap")
    .def("Logmap", &Rot2::Logmap)
    .staticmethod("Logmap")
    .def("atan2", &Rot2::atan2)
    .staticmethod("atan2")
    .def("fromAngle", &Rot2::fromAngle)
    .staticmethod("fromAngle")
    .def("fromCosSin", &Rot2::fromCosSin)
    .staticmethod("fromCosSin")
    .def("fromDegrees", &Rot2::fromDegrees)
    .staticmethod("fromDegrees")
    .def("identity", &Rot2::identity)
    .staticmethod("identity")
    .def("relativeBearing", &Rot2::relativeBearing)
    .staticmethod("relativeBearing")
    .def("c", &Rot2::c)
    .def("degrees", &Rot2::degrees)
    .def("equals", &Rot2::equals, equals_overloads(args("q","tol")))
    .def("matrix", &Rot2::matrix)
    .def("print", &Rot2::print, print_overloads(args("s")))
    .def("rotate", &Rot2::rotate)
    .def("s", &Rot2::s)
    .def("theta", &Rot2::theta)
    .def("unrotate", &Rot2::unrotate)
    .def(self * self) // __mult__
  ;

}


#include <Statistical/3rdParty/EigenUtil.hpp>

#include <Eigen/Dense>

EigenUtil::EigenUtil()
{
    //ctor
}

EigenUtil::~EigenUtil()
{
    //dtor
}

Eigen::VectorXd EigenUtil::ConvertVector2Eigen( const EnjoLib::VecD & vec )
{
    const int nrows = vec.size();
    Eigen::VectorXd veceig(nrows);
    for ( int i = 0; i < nrows; ++i )
            veceig(i) = vec.at(i);

    return veceig;
}


//  Copyright 2015 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt
//  See http://www.boost.org/libs/chrono for documentation.

//#define BOOST_CHRONO_VERSION 1
#define BOOST_CHRONO_VERSION 2
#include <iostream>
#include <boost/rational.hpp>
#include <boost/chrono/chrono.hpp>
//#define BOOST_CHRONO_DONT_PROVIDES_DEPRECATED_IO_SINCE_V2_0_0
#include <boost/chrono/chrono_io.hpp>

int main()
{
  {
    typedef boost::chrono::duration<float> RationalSeconds;
    RationalSeconds d(0.5);
    std::cout << d << std::endl;
  }
  {
    typedef boost::chrono::duration<boost::rational<int> > RationalSeconds;
    RationalSeconds d;
    std::cout << d << std::endl;
  }
  return 0;
}


#ifndef KA_BASE64_HPP
#define KA_BASE64_HPP
#pragma once
#include <climits>
#include <algorithm>
#include <iterator>
#include <string>
#include <boost/iterator/iterator_facade.hpp>
#include "macro.hpp"
#include "macroregular.hpp"
#include "utility.hpp"

namespace ka {

namespace detail {
  // Precondition: 0 <= n < 64
  inline constexpr
  char base64_char(int n) noexcept {
    return "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"[n];
  }

  // Integral N
  template<typename N> constexpr
  N base64_bits(N non_encoded_byte_count) noexcept {
    return N(8) * non_encoded_byte_count;
  }

  // Distance in bits, sextets, and whether byte-distance and sextet-distance
  // are commensurable.
  // Integral N
  template<typename N>
  struct base64_distance_t {
    N bits;
    N sextets;
    bool commensurable;
  };

  // Measures the given distance with several units.
  //
  // Precondition: (0 <= offset0 < 8) && (0 <= offset1 < 8)
  // TODO: Make constexpr when above C++11.
  // Integral N, Integral M
  template<typename N, typename M = N>
  base64_distance_t<N> measure(N distance, M offset0 = M(0), M offset1 = M(0)) {
    auto const bits = base64_bits(distance) + offset1 - offset0;
    auto const sextets = bits / N(6);
    return {bits, sextets, bits == N(6)*sextets};
  }

  // Sextet-distance corresponding to the given byte-distance, starting sextet
  // offset and final sextet offset.
  //
  // Precondition: (0 <= offset0 < 8) && (0 <= offset1 < 8)
  // TODO: Make constexpr when above C++11.
  // Integral N, Integral M
  template<typename N, typename M>
  N distance_to(N distance, M offset0, M offset1) noexcept {
    auto const m = measure(distance, offset0, offset1);
    return m.sextets + (m.commensurable ? 0 : (m.sextets >= 0 ? 1 : -1));
  }

  // Data needed to advance the iterator by the given sextets and from the given
  // sextet offset. This data is composed of the byte-distance and the sextet
  // offset.
  //
  // Precondition: 0 <= offset < 8
  // TODO: Make constexpr when above C++11.
  // Integral N, Integral M
  template<typename N, typename M>
  std::pair<N, M> advance_data(N sextets, M offset) noexcept {
    // octets:  0-------1-------2-------
    //          012345670123456701234567
    // sextets: a-----b-----c-----d-----
    // offsets: 0     6     4     2
    auto const bits = offset + 6*sextets;
    if (bits >= 0) {
      return {static_cast<N>(bits / 8), static_cast<M>(bits % 8)};
    } else {
      // Return value is of the form
      //    {f(bits), g(sextets, offset)}
      // where:
      //   f(x) = -1, for x in [-8, -1]
      //          -2, for x in [-16, -9]
      //          ...
      //   g(-1, 6) = 0
      //   g(-2, 6) = 2
      //   g(-3, 6) = 4
      //   g(-4, 6) = 6
      //   ...
      //   g's implementation relies on the fact that successive offsets from
      //   higher to lower addresses are repeatedly: 0, 2, 4, 6...
      return {static_cast<N>(((bits + 1) / 8) - 1), static_cast<M>((offset - 2*sextets) % 8)};
    }
  }
} // namespace detail

/// Iterator that transforms binary data to base64-encoded data.
///
/// This iterator has the same iterator category (forward, bidirectional, random
/// access) than its underlying byte iterator.
///
/// Note: This iterator is read-only.
///
/// Note: Constructing the end iterator requires computing the offset of the
///   final sextet through the underlying byte iterators. This computation is
///   O(1) for random access iterators and O(n) otherwise. If this final offset
///   is already known, it can be specified through the `base64_iter`
///   constructor function, instead of `base64_end` to avoid the computation.
///
/// Note: This iterator is not suitable for situations requiring (super) high
///   performances, as it handles one byte at a time, does not leverage
///   vectorized operations, etc. It is nonetheless suitable for most situations
///   (no allocation, etc.).
///
/// Note: No padding is added to allow incremental encoding (e.g. chunk by chunk
///   encoding).
///
/// Example: Appending successive base64-encoded data *without* padding.
/// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// // `b64` is a `char` container, `b` and `e` are forward iterators on `char`.
/// ...
/// b64.insert(b64.end(), base64_begin(b, e), base64_end(b, e));
/// ...
/// b64.insert(b64.end(), base64_begin(b, e), base64_end(b, e));
/// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
///
/// Example: Finishing base64-encoding by padding.
/// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// // `b64` is a `char` container, `b` and `e` are forward iterators on `char`.
/// ...
/// b64.insert(b64.end(), base64_begin(b, e), base64_end(b, e));
/// ...
/// std::fill_n(
///   std::back_inserter(b64), base64_padding_byte_count(b64.size()), '=');
/// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
///
/// The following diagram represents how binary data is cut into sextets (6-bit
/// chunks). Each sextet then corresponds to one output character. Three bytes
/// (i.e. 24 bits) yield four output characters ((3 * 8) / 6 == 4).
///
/// octets:  0-------1-------2-------
///          012345670123456701234567
/// sextets: a-----b-----c-----d-----
/// offsets: 0     6     4     2
///
/// ForwardIterator<char> I
template<typename I>
struct base64_iter_t
  : boost::iterator_facade<
      base64_iter_t<I>,
      char,
      typename std::iterator_traits<I>::iterator_category,
      char,
      typename std::iterator_traits<I>::difference_type
    >
{
  I b, e;
  int offset; // Beginning of the sextet inside the current byte.

  explicit
  base64_iter_t(I b = {}, I e = {}, int offset = {}) : b(b), e(e), offset(offset) {
    static_assert(CHAR_BIT == 8, "base64_iter_t: A char must be 8-bit long.");
  }

  char dereference() const {
    return detail::base64_char([&]() -> unsigned int {
      // Sextets a and d (see diagram above) are fully inside one byte:
      if (offset <= 2) {
        return *b >> (2 - offset);
      }
      // Sextets b and c are overlapping two bytes:
      auto const l = 8 - offset; // Bit count of the left part.
      auto const r = 6 - l;      // Bit count of the right part.
      auto const i = std::next(b);
      auto const left = *b << r; // Shift to make space for right part.
      auto const right = (i == e
        ? 0                      // At end -> right part is 0.
        : *i >> (8 - r));        // Not at end -> shift out bits not in right part.
      return left | right;       // Assemble the two parts of the sextet.
    }() & 0x3F);                 // 0x3F forces 6-bit length.
  }
  bool equal(base64_iter_t const& x) const KA_NOEXCEPT_EXPR(b == x.b) {
    return b == x.b && e == x.e && offset == x.offset;
  }
  void increment() {
    advance(1);
  }
  void decrement() {
    advance(-1);
  }
  /// Integral N
  template<typename N>
  void advance(N sextets) {
    auto d = detail::advance_data(sextets, offset);
    std::advance(b, d.first);
    offset = d.second;
  }
  auto distance_to(base64_iter_t const& x) const -> decltype(std::distance(b, x.b)) {
    auto const d = std::distance(b, x.b);
    if (d >= 0) {
      return detail::distance_to(d, offset, x.offset);
    } else {
      return -detail::distance_to(-d, x.offset, offset);
    }
  }
};

/// Contructs a base64 iterator, performing argument type deduction.
///
/// Complexity: O(1)
/// Precondition: readable_bounded_range(b, e) && 0 <= offset < 8
///
/// ForwardIterator<char> I
template<typename I>
base64_iter_t<I> base64_iter(I b, I e, int offset = 0) {
  return base64_iter_t<I>{b, e, offset};
}

/// Contructs a base64 iterator pointing to the beginning of the range.
///
/// Complexity: O(1)
/// Precondition: readable_bounded_range(b, e)
///
/// ForwardIterator<char> I
template<typename I>
base64_iter_t<I> base64_begin(I b, I e) {
  return base64_iter(b, e, 0);
}

/// Contructs a base64 iterator pointing to the end of the range.
///
/// Complexity: O(1) if RandomAccessIterator(I), O(n) otherwise
/// Precondition: readable_bounded_range(b, e)
///
/// ForwardIterator<char> I
template<typename I>
base64_iter_t<I> base64_end(I b, I e) {
  auto m = detail::measure(std::distance(b, e));
  if (!m.commensurable) ++m.sextets;
  return base64_iter(e, e, detail::advance_data(m.sextets, 0).second);
}

/// Number of padding bytes to add to a base64-encoded string that is not
/// yet padded.
///
/// Integral N
template<typename N> constexpr
N base64_padding_byte_count(N encoded_bytes) noexcept {
  // When encoding, each sextet of binary data becomes a character. When
  // decoding, characters are processed by groups of four, because each such
  // group will correspond to three decoded bytes (the least common multiple of
  // 6 and 8 is 24). Therefore, padding's goal is to make the encoded string's
  // length a multiple of four.
  return (N(4) - (encoded_bytes % N(4))) % N(4);
}

/// Number of bytes of a base64-encoded string *without* padding, computed from
/// the number of bytes of a non-encoded string.
///
/// See `base64_encoded_with_padding_byte_count` for a version with padding.
/// TODO: Make constexpr when above C++11.
///
/// Integral N
template<typename N>
N base64_encoded_byte_count(N n) noexcept {
  auto const m = detail::measure(n);
  return m.sextets + N(m.commensurable ? 0 : 1);
}

/// Number of bytes of a base64-encoded string *with* padding, computed from
/// the number of bytes of a non-encoded string.
///
/// See `base64_encoded_byte_count` for a version without padding.
/// TODO: Make constexpr when above C++11.
///
/// Integral N
template<typename N>
N base64_encoded_with_padding_byte_count(N non_encoded_byte_count) noexcept {
  return base64_encoded_byte_count(non_encoded_byte_count)
    + base64_padding_byte_count(
        base64_encoded_byte_count(non_encoded_byte_count));
}

/// Base64-encodes a range of bytes, *without* padding.
///
/// This algorithm is suitable for incremental encoding, letting the caller add
/// padding when it is finished.
///
/// See `base64_encode_with_padding` for a version with padding.
///
/// Precondition: readable_bounded_range(b, e)
///   && writable_counted_range(o, base64_encoded_byte_count(std::distance(b, e)))
///
/// ForwardIterator<char> I, OutputIterator<char> O
template<typename I, typename O>
O base64_encode(I b, I e, O o) {
  return std::copy(base64_begin(b, e), base64_end(b, e), o);
}

namespace detail {

// Output iterator that counts how many times it is incremented.
//
// OutputIterator O
template<typename O>
struct counting_output_iter_t {
  O base;
  std::size_t count; // Output iterators have incomplete difference type.
// Regular:
  KA_GENERATE_FRIEND_REGULAR_OPS_2(counting_output_iter_t, base, count)
// OutputIterator:
  using value_type = typename std::iterator_traits<O>::value_type;
  using pointer = typename std::iterator_traits<O>::pointer;
  using reference = typename std::iterator_traits<O>::reference;
  using difference_type = typename std::iterator_traits<O>::difference_type;
  using iterator_category = typename std::iterator_traits<O>::iterator_category;
  auto operator*() -> decltype(*base) {
    return *base;
  }
  auto operator++() -> counting_output_iter_t& {
    ++base;
    ++count;
    return *this;
  }
  auto operator++(int) -> counting_output_iter_t {
    return {base++, count++};
  }
  // TODO: Remove operator+ after upgrading from VS2015.
  auto operator+(std::size_t n) const -> counting_output_iter_t {
    return {std::next(base, n), count + n};
  }
};

// OutputIterator O
template<typename O>
counting_output_iter_t<O> counting_output_iter(O o) {
  return {o, 0};
}

} // namespace detail

/// Base64-encodes a range of bytes, *with* padding.
///
/// See `base64_encode` for a version without padding.
///
/// Precondition: readable_bounded_range(b, e)
///   && writable_counted_range(o,
///       base64_encoded_with_padding_byte_count(std::distance(b, e)))
///
/// ForwardIterator<char> I, OutputIterator<char> O
template<typename I, typename O>
O base64_encode_with_padding(I b, I e, O o) {
  auto co = std::copy(
    base64_begin(b, e), base64_end(b, e), detail::counting_output_iter(o));
  return std::fill_n(co.base, base64_padding_byte_count(co.count), '=');
}

namespace detail {
  /// ForwardIterator<char> I
  template<typename I>
  void reserve_base64_encode(
      std::string& s, I const& b, I const& e, std::random_access_iterator_tag) {
    s.reserve(base64_encoded_byte_count(e - b));
  }

  /// ForwardIterator<char> I
  template<typename I>
  void reserve_base64_encode(
      std::string&, I const&, I const&, std::forward_iterator_tag) {
  }

  /// ForwardIterator<char> I
  template<typename I>
  void reserve_base64_encode_with_padding(
      std::string& s, I const& b, I const& e, std::random_access_iterator_tag) {
    s.reserve(base64_encoded_with_padding_byte_count(e - b));
  }

  /// ForwardIterator<char> I
  template<typename I>
  void reserve_base64_encode_with_padding(
      std::string&, I const&, I const&, std::forward_iterator_tag) {
  }
} // namespace detail

/// Base64-encodes a range of bytes, *without* padding, returning a string.
///
/// Precondition: readable_bounded_range(b, e)
///
/// ForwardIterator<char> I
template<typename I>
std::string base64_encode(I b, I e) {
  std::string s;
  detail::reserve_base64_encode(s, b, e,
    typename std::iterator_traits<I>::iterator_category{});
  base64_encode(b, e, std::back_inserter(s));
  return s;
}

/// Base64-encodes a range of bytes, *with* padding, returning a string.
///
/// Precondition: readable_bounded_range(b, e)
///
/// ForwardIterator<char> I
template<typename I>
std::string base64_encode_with_padding(I b, I e) {
  std::string s;
  detail::reserve_base64_encode_with_padding(s, b, e,
    typename std::iterator_traits<I>::iterator_category{});
  base64_encode_with_padding(b, e, std::back_inserter(s));
  return s;
}

} // namespace ka

#endif // KA_BASE64_HPP


/**
 * MIT License
 *
 * Copyright (c) 2018 Parsiad Azimzadeh
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef MFHOWARDS_CORE_HPP
#define MFHOWARDS_CORE_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>

#include <chrono>   // std::chrono
#include <cstddef>  // std::size_t
#include <iostream> // std::ostream
#include <numeric>  // std::accumulate
#include <vector>   // std::vector

namespace mfhowards {

namespace {

struct results {
  Eigen::VectorXd value;
  int outer_iters;
  std::vector<int> inner_iters;
  int status;
  double time_ms;

  results(const Eigen::VectorXd &value) : value(value) {}

  results(results &&r) {
    value = std::move(r.value);
    outer_iters = r.outer_iters;
    inner_iters = std::move(r.inner_iters);
    status = r.status;
  }

  results(const results &) = delete;
  results &operator=(const results &) = delete;
};

std::ostream &operator<<(std::ostream &os, const results &r) {
  const double avg_inner_iters =
      accumulate(r.inner_iters.begin(), r.inner_iters.end(), 0.) /
      r.inner_iters.size();
  os << "value = " << r.value.transpose() << std::endl
     << "[status = " << r.status << ", "
     << "outer iterations = " << r.outer_iters << ", "
     << "average inner iterations = " << avg_inner_iters << ", "
     << "time = " << r.time_ms << " ms]" << std::endl;
  return os;
}

} // namespace

template <typename T,
          typename S = Eigen::BiCGSTAB<T, Eigen::IdentityPreconditioner>>
results howards_alg(T &&beq, Eigen::VectorXd x0 = Eigen::VectorXd(0),
                    const double rtol = 1e-5, const double atol = 1e-8,
                    const int max_iters = 1000) {
  S solver;

  if (x0.size() == 0) {
    x0 = Eigen::VectorXd::Zero(beq.rows());
  }
  results r(x0);

  auto t0 = std::chrono::steady_clock::now();

  while (true) {
    // If maximum number of iterations reached, return with bad status
    if (r.outer_iters == max_iters) {
      r.status = 1;
      break;
    }

    // Policy improvement
    beq.improve(r.value);

    // Policy evaluation
    solver.compute(beq);
    x0 = r.value;
    r.value = solver.solve(beq.rhs());
    r.outer_iters += 1;
    r.inner_iters.push_back(solver.iterations());

    // If the solver fails, return with bad status
    if (solver.info() != Eigen::Success) {
      r.status = 255;
      break;
    }

    // If error tolerance met, return with good status
    const Eigen::VectorXd delta =
        (x0 - r.value).cwiseAbs() - rtol * r.value.cwiseAbs();
    const bool converged = (delta.array() < atol).all();
    if (converged) {
      r.status = 0;
      break;
    }
  }

  auto t1 = std::chrono::steady_clock::now();
  auto delta = t1 - t0;
  r.time_ms = std::chrono::duration<double, std::milli>(delta).count();

  return r;
}

} // namespace mfhowards

#endif


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
//using namespace std;

#include <boost/core/ignore_unused.hpp>
#include <boost/mpl/int.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/clear.hpp>

using namespace boost;
using namespace boost::mpl;

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

void case1()
{
    typedef mpl::vector<char, int , long,
        int_<8>, std::string> vec;

    assert((empty<vec>::value == false));
    assert((size<vec>::value == 5));

    assert((is_same<front<vec>::type, char>::value));
    assert((is_same<back<vec>::type, std::string>::value));
    assert((is_same<at_c<vec, 1>::type, int>::value));

    typedef push_front<vec, float>::type vec2;
    assert((size<vec2>::value == 6));
    assert((is_same<front<vec2>::type, float>::value));

    typedef clear<vec2>::type vec3;
    assert((empty<vec3>::value));

}

///////////////////////////////////////
#define BOOST_MPL_LIMIT_STRING_SIZE 24
#include <boost/mpl/string.hpp>

void case2()
{
    typedef mpl::string<'a','b','c'>    abc;
    typedef mpl::string<'Hell','o !!'> hello;
    //typedef mpl::string<'Hello !!'> hello2;
    //typedef mpl::string<"Hello !!"> hello3;

    boost::ignore_unused<abc>();

    assert((empty<hello>::value == false));
    assert((size<hello>::value == 8));

    assert((front<hello>::type::value == 'H'));
    assert((back<hello>::type::value == '!'));

    typedef push_back<hello, char_<'?'>>::type hello2;
    assert((c_str<hello2>::value ==
                std::string("Hello !!?") ));

}

///////////////////////////////////////
#include <boost/mpl/map.hpp>
#include <boost/mpl/count.hpp>

void case3()
{
    typedef mpl::map<
        mpl::pair<int, std::vector<int>>,
        mpl::pair<char,std::string>,
        mpl::pair<int_<3>, float[3]>
    > m;
    assert((empty<m>::value == false));
    assert((size<m>::value == 3));

    typedef front<m>::type p1;
    assert((is_same<first<p1>::type, int>::value));
    assert((is_same<p1::second,std::vector<int>>::value));

    assert((has_key<m, char>::value));
    assert((count<m, int_<0>>::value == 0));

    typedef at<m, char>::type mdata;
    assert((is_same<mdata, std::string>::value));

}

///////////////////////////////////////
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/distance.hpp>

void case4()
{
    typedef mpl::vector<char, int , long,
        int_<8>, std::string> vec;

    typedef begin<vec>::type i1;
    assert((is_same<deref<i1>::type, char>::value));

    typedef next<i1>::type i2;
    assert((is_same<deref<i2>::type, int>::value));
    assert((is_same<i1, prior<i2>::type>::value));

    typedef advance<i2, int_<4>>::type i3;
    assert((distance<i1, i3>::type::value == 5));
    assert((distance<i3, i1>::type::value == -5));
    assert((is_same<i3, mpl::end<vec>::type>::value));

    typedef insert<vec, i2, float>::type vec2;

    boost::ignore_unused<vec2>();
}
///////////////////////////////////////

int main()
{
    std::cout << "hello container" << std::endl;

    case1();
    case2();
    case3();
    case4();
}


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    std::vector<float> y = {1,2,3,4};
    std::vector<float> x = {1,2,3,4};
    std::vector<float> out = {3,6,9,12};
    axpy<float> (x,y,2); // y = 2*x + y;
    BOOST_CHECK (y == out);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));

    // checking axpy() operation
    auto row1 = make_row_vector<float> (cm,1);
    auto row2 = make_row_vector<float> (cm,2);
    axpy<float>(row1,row2,3); // row2 = 3*row1 + row2
    cm.to_rowmajor().save("./out_4x4");

    // checking whether the above operation successfully taken place
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

using boost::geometry::wkt;
using boost::geometry::model::point;
using boost::geometry::model::box;
using boost::geometry::cs::cartesian;
using boost::geometry::index::rtree;
using boost::geometry::index::quadratic;
using boost::geometry::index::intersects;

#include <cmath>
#include <chrono>
#include <vector>
#include <iostream>
#include <utility>

using std::vector;
using std::cerr;
using std::make_pair;
using std::pair;

void test_pack_method(){
    typedef point<size_t, 2, cartesian> point;

    vector<point> points;

    auto t1 = std::chrono::high_resolution_clock::now();
    for (size_t i=0; i<800; i++){
        for (size_t j=0; j<80; j++) {
            points.emplace_back(point {i,j});
        }
    }

    // Construct using "pack" method (bulk loading)
    rtree<point, quadratic<30> > tree(points);

    auto t2 = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    cerr << "Construction completed in: " << elapsed.count() << " milliseconds" << '\n';

    std::vector<point> result;

    for (size_t i=0; i<800; i++){
        for (size_t j=0; j<80; j++) {
            box q(point{i,j},point{i+10,j+10});

            tree.query(intersects(q), std::back_inserter(result));
        }
    }

    auto t3 = std::chrono::high_resolution_clock::now();
    auto elapsed2 = std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2);
    cerr << "Queries completed in: " << elapsed2.count() << " milliseconds" << '\n';
}

void test_dynamic_method(){
    typedef point<size_t, 2, cartesian> point;

    vector<point> points;

    // Construct using "pack" method (bulk loading)
    rtree<point, quadratic<30> > tree;

    auto t1 = std::chrono::high_resolution_clock::now();
    for (size_t i=0; i<800; i++){
        for (size_t j=0; j<80; j++) {
            tree.insert(point {i,j});
        }
    }

    auto t2 = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    cerr << "Construction completed in: " << elapsed.count() << " milliseconds" << '\n';

    std::vector<point> result;

    for (size_t i=0; i<800; i++){
        for (size_t j=0; j<80; j++) {
            box q(point{i,j},point{i+10,j+10});

            tree.query(intersects(q), std::back_inserter(result));
        }
    }

    auto t3 = std::chrono::high_resolution_clock::now();
    auto elapsed2 = std::chrono::duration_cast<std::chrono::milliseconds>(t3 - t2);
    cerr << "Queries completed in: " << elapsed2.count() << " milliseconds" << '\n';
}

void test_object_storage(){
    typedef point<size_t, 2, cartesian> xy_point;
    typedef pair<xy_point, size_t> xy_object;

    vector<xy_object> points;

    // Construct using "pack" method (bulk loading)
    rtree<xy_object, quadratic<30> > tree;

    for (size_t i=0; i<40; i++){
        for (size_t j=0; j<40; j++) {
            tree.insert(make_pair(xy_point {i,j}, i*j));
        }
    }

    cerr << '\n';
    for (size_t i=0; i<40; i++){
        for (size_t j=0; j<40; j++) {
            std::vector<xy_object> result;

            box q(xy_point{i,j},xy_point{i+2,j+2});

            tree.query(intersects(q), std::back_inserter(result));

//            for (const auto& item: result){
//                cerr << i << ' ' << j << " (" << item.first.get<0>() << ',' << item.first.get<1>() << ")=" << item.second << '\n';
//            }
//            cerr << '\n';
        }
    }

}

int main()
{
    test_pack_method();
    test_dynamic_method();
    test_object_storage();
    return 0;
}