#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sm/logging.hpp>
#include <aslam/cameras/MultipleTargetAprilGridDetector.hpp>

namespace aslam {
    namespace cameras {

//serialization constructor (don't use!)
        MultipleTargetAprilGridDetector::MultipleTargetAprilGridDetector() {}

        MultipleTargetAprilGridDetector::MultipleTargetAprilGridDetector(
                boost::shared_ptr <CameraGeometryBase> geometry,
                GridCalibrationTargetAprilgrid::Ptr target,
                const int numTargets,
                const GridDetector::GridDetectorOptions &options)
                : _geometry(geometry),
                  _target(target),
                  _numTargets(numTargets),
                  _options(options) {
            SM_ASSERT_TRUE(Exception, _geometry.get() != NULL,
                           "Unable to initialize with null camera geometry");
            SM_ASSERT_TRUE(Exception, _target.get() != NULL,
                           "Unable to initialize with null calibration target");

            initializeDetector();
        }

        void MultipleTargetAprilGridDetector::initializeDetector() {
            if (_options.plotCornerReprojection) {
                cv::namedWindow("Corner reprojection", cv::WINDOW_NORMAL);
            }
            if (_target->getOptions().showExtractionVideo) {

                cv::namedWindow("Aprilgrid: Tag detection", cv::WINDOW_NORMAL);
                cv::namedWindow("Aprilgrid: Tag corners", cv::WINDOW_NORMAL);
            }
        }

        MultipleTargetAprilGridDetector::~MultipleTargetAprilGridDetector() {

        }

        void MultipleTargetAprilGridDetector::initCameraGeometry(boost::shared_ptr <CameraGeometryBase> geometry) {
            SM_ASSERT_TRUE(Exception, geometry.get() != NULL, "Unable to initialize with null camera geometry");
            _geometry = geometry;
        }

        bool MultipleTargetAprilGridDetector::initCameraGeometryFromObservation(const cv::Mat &image) {
            boost::shared_ptr <std::vector<cv::Mat>> images_ptr = boost::make_shared < std::vector < cv::Mat >> ();
            images_ptr->push_back(image);

            return initCameraGeometryFromObservations(images_ptr);
        }

        bool MultipleTargetAprilGridDetector::initCameraGeometryFromObservations(
                boost::shared_ptr <std::vector<cv::Mat>> images_ptr) {

            std::vector <cv::Mat> &images = *images_ptr;

            SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
            SM_ASSERT_TRUE(Exception, images.size() != 0, "Need min. one image");

            std::vector <GridCalibrationTargetObservation> observations;

            for (unsigned int i = 0; i < images.size(); i++) {
                std::vector<GridCalibrationTargetObservation> obs;

                //detect calibration target
                bool success = findTargetNoTransformation(images[i], obs);

                if (obs.empty() || obs[0].targetId()!=0) continue;

                //delete image copy (save memory)
                obs[0].clearImage();

                //append
                if (success)
                    observations.push_back(obs[0]);
            }

            //initialize the intrinsics
            if (observations.size() > 0)
                return _geometry->initializeIntrinsics(observations);

            return false;
        }

        bool MultipleTargetAprilGridDetector::findTarget(const cv::Mat &image,
                                                         vector <GridCalibrationTargetObservation> &outObservation) const {
            return findTarget(image, aslam::Time(0, 0), outObservation);
        }

        bool MultipleTargetAprilGridDetector::findTargetNoTransformation(const cv::Mat &image, const aslam::Time &stamp,
                                                                         vector <GridCalibrationTargetObservation> &outObservation) const {
            bool success = true;

            // Set the image, target, and timestamp regardless of success.
            GridCalibrationTargetObservation tmp(_target, image);
            tmp.setTime(stamp);
            outObservation.resize(_numTargets, tmp);
            for(int i = 0; i < _numTargets; i++){
                outObservation[i].setTargetId(i);
            }

            // detect the tags
            std::vector <AprilTags::TagDetection> detections = _target->getTagDetector()->extractTags(image);

            /* handle the case in which a tag is identified but not all tag
             * corners are in the image (all data bits in image but border
             * outside). tagCorners should still be okay as apriltag-lib
             * extrapolates them, only the subpix refinement will fail
             */

            //min. distance [px] of tag corners from image border (tag is not used if violated)
            std::vector<AprilTags::TagDetection>::iterator iter = detections.begin();
            for (iter = detections.begin(); iter != detections.end();) {
                // check all four corners for violation
                bool remove = false;

                for (int j = 0; j < 4; j++) {
                    remove |= iter->p[j].first < _target->getOptions().minBorderDistance;
                    remove |= iter->p[j].first > (float) (image.cols) - _target->getOptions().minBorderDistance;  //width
                    remove |= iter->p[j].second < _target->getOptions().minBorderDistance;
                    remove |= iter->p[j].second > (float) (image.rows) - _target->getOptions().minBorderDistance;  //height
                }

                //also remove tags that are flagged as bad
                if (iter->good != 1)
                    remove |= true;

                //also remove if the tag ID is out-of-range for this grid (faulty detection)
                if (iter->id >= _numTargets * _target->size() / 4)
                    remove |= true;

                // delete flagged tags
                if (remove) {
                    SM_DEBUG_STREAM("Tag with ID " << iter->id
                                                   << " is only partially in image (corners outside) and will be removed from the TargetObservation.\n");

                    // delete the tag and advance in list
                    iter = detections.erase(iter);
                } else {
                    //advance in list
                    ++iter;
                }
            }

            //did we find enough tags?
            if (detections.size() < _target->getOptions().minTagsForValidObs) {
                success = false;

                //immediate exit if we dont need to show video for debugging...
                //if video is shown, exit after drawing video...
                if (!_target->getOptions().showExtractionVideo)
                    return success;
            }

            //sort detections by tagId
            std::sort(detections.begin(), detections.end(),
                      AprilTags::TagDetection::sortByIdCompare);

            // check for duplicate tagIds (--> if found: wild Apriltags in image not belonging to calibration target)
            // (only if we have more than 1 tag...)
            if (detections.size() > 1) {
                for (unsigned i = 0; i < detections.size() - 1; i++)
                    if (detections[i].id == detections[i + 1].id) {

                        cv::Mat imageCopy = image.clone();
                        cv::cvtColor(imageCopy, imageCopy, CV_GRAY2RGB);

                        int duplicated_id = detections[i].id;
                        //mark all duplicate tags in image
                        for (auto iter = detections.begin()+i; iter != detections.end();) {
                            if (iter->id == duplicated_id) {
                                iter->draw(imageCopy);
                                detections.erase(iter);
                            }
                            else{
                                iter++;
                            }
                        }

                        cv::putText(imageCopy, "Duplicate Apriltags detected. Hide them.",
                                    cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                                    CV_RGB(255, 0, 0), 2, 8, false);
                        std::string file_name = "Duplicate_Apriltags_"+std::to_string(stamp.toSec())+".jpg";
                        cv::imwrite(file_name, imageCopy);  // OpenCV call

                        SM_FATAL_STREAM(
                                "\n[ERROR]: Found apriltag not belonging to calibration board. "
                                "Has erased these duplicated tags, please check the image file " + file_name +
                                " for the tag and hide it.\n");

                    }
            }

            // convert corners to cv::Mat (4 consecutive corners form one tag)
            /// point ordering here
            ///          11-----10  15-----14
            ///          | TAG 2 |  | TAG 3 |
            ///          8-------9  12-----13
            ///          3-------2  7-------6
            ///    y     | TAG 0 |  | TAG 1 |
            ///   ^      0-------1  4-------5
            ///   |-->x
            cv::Mat tagCorners(4 * detections.size(), 2, CV_32F);

            for (unsigned i = 0; i < detections.size(); i++) {
                for (unsigned j = 0; j < 4; j++) {
                    tagCorners.at<float>(4 * i + j, 0) = detections[i].p[j].first;
                    tagCorners.at<float>(4 * i + j, 1) = detections[i].p[j].second;
                }
            }

            //store a copy of the corner list before subpix refinement
            cv::Mat tagCornersRaw = tagCorners.clone();

            //optional subpixel refinement on all tag corners (four corners each tag)
            if (_target->getOptions().doSubpixRefinement && success)
                cv::cornerSubPix(
                        image, tagCorners, cv::Size(2, 2), cv::Size(-1, -1),
                        cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

            if (_target->getOptions().showExtractionVideo) {
                //image with refined (blue) and raw corners (red)
                cv::Mat imageCopy1 = image.clone();
                cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB);
                for (unsigned i = 0; i < detections.size(); i++)
                    for (unsigned j = 0; j < 4; j++) {
                        //raw apriltag corners
                        //cv::circle(imageCopy1, cv::Point2f(detections[i].p[j].first, detections[i].p[j].second), 2, CV_RGB(255,0,0), 1);

                        //subpixel refined corners
                        cv::circle(
                                imageCopy1,
                                cv::Point2f(tagCorners.at<float>(4 * i + j, 0),
                                            tagCorners.at<float>(4 * i + j, 1)),
                                3, CV_RGB(0, 0, 255), 1);

                        if (!success)
                            cv::putText(imageCopy1, "Detection failed! (frame not used)",
                                        cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                                        CV_RGB(255, 0, 0), 3, 8, false);
                    }

                cv::imshow("Aprilgrid: Tag corners", imageCopy1);  // OpenCV call
                cv::waitKey(1);

                /* copy image for modification */
                cv::Mat imageCopy2 = image.clone();
                cv::cvtColor(imageCopy2, imageCopy2, CV_GRAY2RGB);
                /* highlight detected tags in image */
                for (unsigned i = 0; i < detections.size(); i++) {
                    detections[i].draw(imageCopy2);

                    if (!success)
                        cv::putText(imageCopy2, "Detection failed! (frame not used)",
                                    cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                                    CV_RGB(255, 0, 0), 3, 8, false);
                }

                cv::imshow("Aprilgrid: Tag detection", imageCopy2);  // OpenCV call
                cv::waitKey(1);

                //if success is false exit here (delayed exit if _options.showExtractionVideo=true for debugging)
                if (!success)
                    return success;
            }

            //insert the observed points into the correct location of the grid point array
            /// point ordering
            ///          12-----13  14-----15
            ///          | TAG 2 |  | TAG 3 |
            ///          8-------9  10-----11
            ///          4-------5  6-------7
            ///    y     | TAG 0 |  | TAG 1 |
            ///   ^      0-------1  2-------3
            ///   |-->x

            int tagsEachTarget = _target->size() / 4;
            unsigned int cols = _target->cols();
//            outCornerObserved.resize(size(), false);
//            outImagePoints.resize(size(), 2);

            for (unsigned int i = 0; i < detections.size(); i++) {
                // get the tag id
                unsigned int tagId = detections[i].id;
                int targetId = tagId / tagsEachTarget;
                tagId %= tagsEachTarget;
                // calculate the grid idx for all four tag corners given the tagId and cols
                unsigned int baseId = (int) (tagId / (cols / 2)) * cols * 2
                                      + (tagId % (cols / 2)) * 2;
                unsigned int pIdx[] = {baseId, baseId + 1, baseId + cols + 1, baseId + cols};

                // add four points per tag
                for (int j = 0; j < 4; j++) {
                    //refined corners
                    double corner_x = tagCorners.row(4 * i + j).at<float>(0);
                    double corner_y = tagCorners.row(4 * i + j).at<float>(1);

                    //raw corners
                    double cornerRaw_x = tagCornersRaw.row(4 * i + j).at<float>(0);
                    double cornerRaw_y = tagCornersRaw.row(4 * i + j).at<float>(1);

                    //only add point if the displacement in the subpixel refinement is below a given threshold
                    double subpix_displacement_squarred = (corner_x - cornerRaw_x)
                                                          * (corner_x - cornerRaw_x)
                                                          + (corner_y - cornerRaw_y) * (corner_y - cornerRaw_y);

                    if (subpix_displacement_squarred <= _target->getOptions().maxSubpixDisplacement2) {
                        outObservation[targetId].updateImagePoint(pIdx[j], Eigen::Vector2d(corner_x, corner_y));
                    } else {
                        SM_DEBUG_STREAM("Subpix refinement failed for point: " << pIdx[j] << " with displacement: "
                                                                               << sqrt(subpix_displacement_squarred)
                                                                               << "(point removed) \n");
                    }
                }
            }

            for(int i =0; i < outObservation.size();)
            {
                if(outObservation[i].numberSuccessfulObservation() < _target->getOptions().minTagsForValidObs)
                {
                    outObservation.erase(outObservation.begin() + i);
                }
                else
                {
                    i++;
                }
            }
            return outObservation.size();
        }

        bool MultipleTargetAprilGridDetector::findTarget(const cv::Mat &image, const aslam::Time &stamp,
                                                         vector <GridCalibrationTargetObservation> &outObservation) const {
            sm::kinematics::Transformation trafo;

            // find calibration target corners
            bool success = findTargetNoTransformation(image, stamp, outObservation);

            if(success)
            {
                for(auto& outObs: outObservation)
                {
                    // also estimate the transformation:
                    if(_geometry->estimateTransformation(outObs, trafo))
                    {
                        success = true;
                        outObs.set_T_t_c(trafo);
                        //remove corners with a reprojection error above a threshold
                        //(remove detection outliers)
                        if (_options.filterCornerOutliers) {
                            //calculate reprojection errors
                            std::vector <cv::Point2f> corners_reproj;
                            std::vector <cv::Point2f> corners_detected;
                            outObs.getCornerReprojection(_geometry, corners_reproj);
                            unsigned int numCorners = outObs.getCornersImageFrame(corners_detected);

                            //calculate error norm
                            Eigen::MatrixXd reprojection_errors_norm = Eigen::MatrixXd::Zero(numCorners, 1);

                            for (unsigned int i = 0; i < numCorners; i++) {
                                cv::Point2f reprojection_err = corners_detected[i] - corners_reproj[i];

                                reprojection_errors_norm(i, 0) = sqrt(reprojection_err.x * reprojection_err.x +
                                                                      reprojection_err.y * reprojection_err.y);
                            }

                            //calculate statistics
                            double mean = reprojection_errors_norm.mean();
                            double std = 0.0;
                            for (unsigned int i = 0; i < numCorners; i++) {
                                double temp = reprojection_errors_norm(i, 0) - mean;
                                std += temp * temp;
                            }
                            std /= (double) numCorners;
                            std = sqrt(std);

                            //disable outlier corners
                            std::vector<unsigned int> cornerIdx;
                            outObs.getCornersIdx(cornerIdx);

                            unsigned int removeCount = 0;
                            for (unsigned int i = 0; i < corners_detected.size(); i++) {
                                if (reprojection_errors_norm(i, 0) > mean + _options.filterCornerSigmaThreshold * std &&
                                    reprojection_errors_norm(i, 0) > _options.filterCornerMinReprojError) {
                                    outObs.removeImagePoint(cornerIdx[i]);
                                    removeCount++;
                                    SM_DEBUG_STREAM(
                                            "removed target point with reprojection error of " << reprojection_errors_norm(i, 0)
                                                                                << " (mean: " << mean << ", std: "
                                                                                << std << ")\n";);
                                }
                            }

                            if (removeCount > 0)
                                SM_DEBUG_STREAM("removed " << removeCount << " of " << numCorners
                                                           << " calibration target corner outliers\n";);
                        }
                    }
                    else
                        SM_DEBUG_STREAM("estimateTransformation() failed");
                }
            }

            // show plot of reprojected corners
            if (_options.plotCornerReprojection) {
                cv::Mat imageCopy1 = image.clone();
                cv::cvtColor(imageCopy1, imageCopy1, CV_GRAY2RGB);

                if (success) {
                    for(const auto& outObs: outObservation) {
                        //calculate reprojection
                        std::vector <cv::Point2f> reprojs;
                        outObs.getCornerReprojection(_geometry, reprojs);

                        for (unsigned int i = 0; i < reprojs.size(); i++)
                            cv::circle(imageCopy1, reprojs[i], 3, CV_RGB(255, 0, 0), 1);
                    }

                } else {
                    cv::putText(imageCopy1, "Detection failed! (frame not used)",
                                cv::Point(50, 50), CV_FONT_HERSHEY_SIMPLEX, 0.8,
                                CV_RGB(255, 0, 0), 3, 8, false);
                }

                cv::imshow("Corner reprojection", imageCopy1);  // OpenCV call
                if (_options.imageStepping) {
                    cv::waitKey(0);
                } else {
                    cv::waitKey(1);
                }
            }

            return success;
        }

/// \brief Find the target but don't estimate the transformation.
        bool MultipleTargetAprilGridDetector::findTargetNoTransformation(const cv::Mat &image,
                                                                         vector <GridCalibrationTargetObservation> &outObservation) const {
            return findTargetNoTransformation(image, aslam::Time(0, 0), outObservation);
        }

    }  // namespace cameras
}  // namespace aslam

//export explicit instantions for all included archives
#include <sm/boost/serialization.hpp>
#include <boost/serialization/export.hpp>

BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::MultipleTargetAprilGridDetector);


// Copyright (c) 2020 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <avalanche/peermanager.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(peermanager_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(select_peer_linear) {
    // No peers.
    BOOST_CHECK_EQUAL(selectPeerImpl({}, 0, 0), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl({}, 1, 3), NO_PEER);

    // One peer
    const std::vector<Slot> oneslot = {{100, 100}};

    // Undershoot
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 0, 300), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 42, 300), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 99, 300), NO_PEER);

    // Nailed it
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 100, 300), 0);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 142, 300), 0);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 199, 300), 0);

    // Overshoot
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 200, 300), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 242, 300), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(oneslot, 299, 300), NO_PEER);

    // Two peers
    const std::vector<Slot> twoslots = {{100, 100}, {300, 100}};

    // Undershoot
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 0, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 42, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 99, 500), NO_PEER);

    // First entry
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 100, 500), 0);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 142, 500), 0);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 199, 500), 0);

    // In betwenn
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 200, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 242, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 299, 500), NO_PEER);

    // Second entry
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 300, 500), 1);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 342, 500), 1);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 399, 500), 1);

    // Overshoot
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 400, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 442, 500), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(twoslots, 499, 500), NO_PEER);
}

BOOST_AUTO_TEST_CASE(select_peer_dichotomic) {
    std::vector<Slot> slots;

    // 100 peers of size 1 with 1 empty element apart.
    uint64_t max = 1;
    for (int i = 0; i < 100; i++) {
        slots.emplace_back(max, 1);
        max += 2;
    }

    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 4, max), NO_PEER);

    // Check that we get what we expect.
    for (int i = 0; i < 100; i++) {
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 2 * i, max), NO_PEER);
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 2 * i + 1, max), i);
    }

    BOOST_CHECK_EQUAL(selectPeerImpl(slots, max, max), NO_PEER);

    // Update the slots to be heavily skewed toward the last element.
    slots[99] = slots[99].withScore(101);
    max = slots[99].getStop();
    BOOST_CHECK_EQUAL(max, 300);

    for (int i = 0; i < 100; i++) {
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 2 * i, max), NO_PEER);
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 2 * i + 1, max), i);
    }

    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 200, max), 99);
    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 256, max), 99);
    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 299, max), 99);
    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 300, max), NO_PEER);

    // Update the slots to be heavily skewed toward the first element.
    for (int i = 0; i < 100; i++) {
        slots[i] = Slot(slots[i].getStart() + 100, slots[i].getScore());
    }

    slots[0] = Slot(1, slots[0].getStop() - 1);
    slots[99] = slots[99].withScore(1);
    max = slots[99].getStop();
    BOOST_CHECK_EQUAL(max, 300);

    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 0, max), NO_PEER);
    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 1, max), 0);
    BOOST_CHECK_EQUAL(selectPeerImpl(slots, 42, max), 0);

    for (int i = 0; i < 100; i++) {
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 100 + 2 * i + 1, max), i);
        BOOST_CHECK_EQUAL(selectPeerImpl(slots, 100 + 2 * i + 2, max), NO_PEER);
    }
}

BOOST_AUTO_TEST_CASE(select_peer_random) {
    for (int c = 0; c < 1000; c++) {
        size_t size = InsecureRandBits(10) + 1;
        std::vector<Slot> slots;
        slots.reserve(size);

        uint64_t max = InsecureRandBits(3);
        auto next = [&]() {
            uint64_t r = max;
            max += InsecureRandBits(3);
            return r;
        };

        for (size_t i = 0; i < size; i++) {
            const uint64_t start = next();
            const uint32_t score = InsecureRandBits(3);
            max += score;
            slots.emplace_back(start, score);
        }

        for (int k = 0; k < 100; k++) {
            uint64_t s = InsecureRandRange(max);
            auto i = selectPeerImpl(slots, s, max);
            BOOST_CHECK(i == NO_PEER || slots[i].contains(s));
        }
    }
}

BOOST_AUTO_TEST_CASE(add_peer) {
    // No peers.
    PeerManager pm;
    BOOST_CHECK_EQUAL(pm.selectPeer(), NO_PEER);

    // One peer, we always return it.
    pm.addPeer(100);
    BOOST_CHECK_EQUAL(pm.selectPeer(), 0);

    // Two peers, verify ratio.
    pm.addPeer(200);

    std::array<int, 3> results = {};
    for (int i = 0; i < 10000; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p <= 1);
        results[p]++;
    }

    BOOST_CHECK(abs(2 * results[0] - results[1]) < 500);

    // Three peers, verify ratio.
    pm.addPeer(100);

    results = {};
    for (int i = 0; i < 10000; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p <= 2);
        results[p]++;
    }

    BOOST_CHECK(abs(results[0] - results[1] + results[2]) < 500);
}

BOOST_AUTO_TEST_CASE(remove_peer) {
    // No peers.
    PeerManager pm;
    BOOST_CHECK_EQUAL(pm.selectPeer(), NO_PEER);

    // Add 4 peers.
    for (int i = 0; i < 4; i++) {
        pm.addPeer(100);
    }

    for (int i = 0; i < 100; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p <= 4);
    }

    // Remove one peer, it nevers show up now.
    pm.removePeer(3);
    for (int i = 0; i < 100; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p < 4);
        BOOST_CHECK(p != 3);
    }

    // Add 4 more peers.
    for (int i = 0; i < 4; i++) {
        pm.addPeer(100);
    }

    pm.removePeer(0);
    pm.removePeer(7);
    for (int i = 0; i < 100; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p < 8);
        BOOST_CHECK(p != 0);
        BOOST_CHECK(p != 3);
        BOOST_CHECK(p != 7);
    }
}

BOOST_AUTO_TEST_CASE(rescore_peer, *boost::unit_test::timeout(5)) {
    // No peers.
    PeerManager pm;
    BOOST_CHECK_EQUAL(pm.selectPeer(), NO_PEER);

    // Add 4 peers.
    for (int i = 0; i < 4; i++) {
        pm.addPeer(100);
    }

    BOOST_CHECK_EQUAL(pm.getSlotCount(), 400);
    BOOST_CHECK_EQUAL(pm.getFragmentation(), 0);

    for (int i = 0; i < 100; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p <= 4);
    }

    // Set one peer's score to 0, it nevers show up now.
    pm.rescorePeer(1, 0);
    BOOST_CHECK_EQUAL(pm.getSlotCount(), 400);
    BOOST_CHECK_EQUAL(pm.getFragmentation(), 100);

    for (int i = 0; i < 100; i++) {
        size_t p = pm.selectPeer();
        BOOST_CHECK(p < 4);
        BOOST_CHECK(p != 1);
    }

    // "resurrect" the peer.
    pm.rescorePeer(1, 100);
    BOOST_CHECK_EQUAL(pm.getSlotCount(), 400);
    BOOST_CHECK_EQUAL(pm.getFragmentation(), 0);

    while (true) {
        size_t p = pm.selectPeer();
        if (p == 1) {
            break;
        }
    }

    // Grow the peer to a point where it needs to be reallocated.
    pm.rescorePeer(1, 200);
    BOOST_CHECK_EQUAL(pm.getSlotCount(), 600);
    BOOST_CHECK_EQUAL(pm.getFragmentation(), 100);

    for (int i = 0; i < 25; i++) {
        while (true) {
            size_t p = pm.selectPeer();
            BOOST_CHECK(p < 5);
            BOOST_CHECK(p != 1);
            if (p == 4) {
                break;
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "consensus/validation.h"
#include "data/sighash.json.h"
#include "hash.h"
#include "validation.h" // For CheckTransaction
#include "random.h"
#include "script/interpreter.h"
#include "script/script.h"
#include "serialize.h"
#include "streams.h"
#include "test/test_simplecointest.h"
#include "util.h"
#include "utilstrencodings.h"
#include "version.h"

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <univalue.h>

extern UniValue read_json(const std::string& jsondata);

// Old script.cpp SignatureHash function
uint256 static SignatureHashOld(CScript scriptCode, const CTransaction& txTo, unsigned int nIn, int nHashType)
{
    static const uint256 one(uint256S("0000000000000000000000000000000000000000000000000000000000000001"));
    if (nIn >= txTo.vin.size())
    {
        printf("ERROR: SignatureHash(): nIn=%d out of range\n", nIn);
        return one;
    }
    CMutableTransaction txTmp(txTo);

    // In case concatenating two scripts ends up with two codeseparators,
    // or an extra one at the end, this prevents all those possible incompatibilities.
    scriptCode.FindAndDelete(CScript(OP_CODESEPARATOR));

    // Blank out other inputs' signatures
    for (unsigned int i = 0; i < txTmp.vin.size(); i++)
        txTmp.vin[i].scriptSig = CScript();
    txTmp.vin[nIn].scriptSig = scriptCode;

    // Blank out some of the outputs
    if ((nHashType & 0x1f) == SIGHASH_NONE)
    {
        // Wildcard payee
        txTmp.vout.clear();

        // Let the others update at will
        for (unsigned int i = 0; i < txTmp.vin.size(); i++)
            if (i != nIn)
                txTmp.vin[i].nSequence = 0;
    }
    else if ((nHashType & 0x1f) == SIGHASH_SINGLE)
    {
        // Only lock-in the txout payee at same index as txin
        unsigned int nOut = nIn;
        if (nOut >= txTmp.vout.size())
        {
            printf("ERROR: SignatureHash(): nOut=%d out of range\n", nOut);
            return one;
        }
        txTmp.vout.resize(nOut+1);
        for (unsigned int i = 0; i < nOut; i++)
            txTmp.vout[i].SetNull();

        // Let the others update at will
        for (unsigned int i = 0; i < txTmp.vin.size(); i++)
            if (i != nIn)
                txTmp.vin[i].nSequence = 0;
    }

    // Blank out other inputs completely, not recommended for open transactions
    if (nHashType & SIGHASH_ANYONECANPAY)
    {
        txTmp.vin[0] = txTmp.vin[nIn];
        txTmp.vin.resize(1);
    }

    // Serialize and hash
    CHashWriter ss(SER_GETHASH, 0);
    ss << txTmp << nHashType;
    return ss.GetHash();
}

void static RandomScript(CScript &script) {
    static const opcodetype oplist[] = {OP_FALSE, OP_1, OP_2, OP_3, OP_CHECKSIG, OP_IF, OP_VERIF, OP_RETURN, OP_CODESEPARATOR};
    script = CScript();
    int ops = (insecure_rand() % 10);
    for (int i=0; i<ops; i++)
        script << oplist[insecure_rand() % (sizeof(oplist)/sizeof(oplist[0]))];
}

void static RandomTransaction(CMutableTransaction &tx, bool fSingle) {
    tx.nVersion = insecure_rand();
    tx.vin.clear();
    tx.vout.clear();
    tx.nLockTime = (insecure_rand() % 2) ? insecure_rand() : 0;
    int ins = (insecure_rand() % 4) + 1;
    int outs = fSingle ? ins : (insecure_rand() % 4) + 1;
    for (int in = 0; in < ins; in++) {
        tx.vin.push_back(CTxIn());
        CTxIn &txin = tx.vin.back();
        txin.prevout.hash = GetRandHash();
        txin.prevout.n = insecure_rand() % 4;
        RandomScript(txin.scriptSig);
        txin.nSequence = (insecure_rand() % 2) ? insecure_rand() : (unsigned int)-1;
    }
    for (int out = 0; out < outs; out++) {
        tx.vout.push_back(CTxOut());
        CTxOut &txout = tx.vout.back();
        txout.nValue = insecure_rand() % 100000000;
        RandomScript(txout.scriptPubKey);
    }
}

BOOST_FIXTURE_TEST_SUITE(sighash_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(sighash_test)
{
    seed_insecure_rand(false);

    #if defined(PRINT_SIGHASH_JSON)
    std::cout << "[\n";
    std::cout << "\t[\"raw_transaction, script, input_index, hashType, signature_hash (result)\"],\n";
    #endif
    int nRandomTests = 50000;

    #if defined(PRINT_SIGHASH_JSON)
    nRandomTests = 500;
    #endif
    for (int i=0; i<nRandomTests; i++) {
        int nHashType = insecure_rand();
        CMutableTransaction txTo;
        RandomTransaction(txTo, (nHashType & 0x1f) == SIGHASH_SINGLE);
        CScript scriptCode;
        RandomScript(scriptCode);
        int nIn = insecure_rand() % txTo.vin.size();

        uint256 sh, sho;
        sho = SignatureHashOld(scriptCode, txTo, nIn, nHashType);
        sh = SignatureHash(scriptCode, txTo, nIn, nHashType);
        #if defined(PRINT_SIGHASH_JSON)
        CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
        ss << txTo;

        std::cout << "\t[\"" ;
        std::cout << HexStr(ss.begin(), ss.end()) << "\", \"";
        std::cout << HexStr(scriptCode) << "\", ";
        std::cout << nIn << ", ";
        std::cout << nHashType << ", \"";
        std::cout << sho.GetHex() << "\"]";
        if (i+1 != nRandomTests) {
          std::cout << ",";
        }
        std::cout << "\n";
        #endif
        BOOST_CHECK(sh == sho);
    }
    #if defined(PRINT_SIGHASH_JSON)
    std::cout << "]\n";
    #endif
}

// Goal: check that SignatureHash generates correct hash
BOOST_AUTO_TEST_CASE(sighash_from_data)
{
    UniValue tests = read_json(std::string(json_tests::sighash, json_tests::sighash + sizeof(json_tests::sighash)));

    for (unsigned int idx = 0; idx < tests.size(); idx++) {
        UniValue test = tests[idx];
        std::string strTest = test.write();
        if (test.size() < 1) // Allow for extra stuff (useful for comments)
        {
            BOOST_ERROR("Bad test: " << strTest);
            continue;
        }
        if (test.size() == 1) continue; // comment

        std::string raw_tx, raw_script, sigHashHex;
        int nIn, nHashType;
        uint256 sh;
        CTransaction tx;
        CScript scriptCode = CScript();

        try {
          // deserialize test data
          raw_tx = test[0].get_str();
          raw_script = test[1].get_str();
          nIn = test[2].get_int();
          nHashType = test[3].get_int();
          sigHashHex = test[4].get_str();

          uint256 sh;
          CDataStream stream(ParseHex(raw_tx), SER_NETWORK, PROTOCOL_VERSION);
          stream >> tx;

          CValidationState state;
          BOOST_CHECK_MESSAGE(CheckTransaction(tx, state), strTest);
          BOOST_CHECK(state.IsValid());

          std::vector<unsigned char> raw = ParseHex(raw_script);
          scriptCode.insert(scriptCode.end(), raw.begin(), raw.end());
        } catch (...) {
          BOOST_ERROR("Bad test, couldn't deserialize data: " << strTest);
          continue;
        }

        sh = SignatureHash(scriptCode, tx, nIn, nHashType);
        BOOST_CHECK_MESSAGE(sh.GetHex() == sigHashHex, strTest);
    }
}
BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file math.hpp
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * 3D mesh operations
 */

#include <numeric>

#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Tools/Decimater/DecimaterT.hh>
#include <OpenMesh/Tools/Decimater/ModQuadricT.hh>
#include <OpenMesh/Tools/Decimater/ModNormalFlippingT.hh>
#include <OpenMesh/Tools/Decimater/ModAspectRatioT.hh>
#include <OpenMesh/Tools/Decimater/ModEdgeLengthT.hh>
#include <boost/numeric/ublas/vector.hpp>

#include "math/math.hpp"

#include "meshop.hpp"
#include "detail/hybrid-decimater.hpp"

namespace geometry {

/* code moved from the window-mesh utility */
namespace ublas = boost::numeric::ublas;

namespace {

struct NormalTraits : public OpenMesh::DefaultTraits
{
  FaceAttributes( OpenMesh::Attributes::Normal );
  VertexTraits
  {
    int classLabel;
  };

};

typedef OpenMesh::TriMesh_ArrayKernelT<NormalTraits> OMMesh;
typedef OpenMesh::Decimater::DecimaterT<OMMesh> Decimator;
typedef OpenMesh::Decimater::ModQuadricT<OMMesh>::Handle HModQuadric;
typedef detail::ModQuadricHybrid<OMMesh>::Handle HModQuadricHybrid;
typedef OpenMesh::Decimater::ModNormalFlippingT<OMMesh>::Handle HModNormalFlippingT;
typedef OpenMesh::Decimater::ModAspectRatioT<OMMesh>::Handle HModAspectRatioT;
typedef OpenMesh::Decimater::ModEdgeLengthT<OMMesh>::Handle HModEdgeLengthT;

#if OM_VERSION >= 0x60000
typedef detail::ModQuadricConvexT<OMMesh>::Handle HModQuadricConvex;
typedef detail::ClassifyRestrictModT<OMMesh>::Handle HModClassRestrictT;
#endif

#ifdef GEOMETRY_HAS_OPENCV

/** Extension of ModQuadric multiplying the cost by generic 2D weight map
 */
template <class MeshT>
class ModWeightedQuadricT : public OpenMesh::Decimater::ModQuadricT<MeshT> {
public:
    // Defines the types Self, Handle, Base, Mesh, and CollapseInfo
    // and the memberfunction name()
    DECIMATING_MODULE(ModWeightedQuadricT, MeshT, WeightedQuadric);

public:
    ModWeightedQuadricT(MeshT& mesh)
        : OpenMesh::Decimater::ModQuadricT<MeshT>(mesh) {
        Base::mesh().add_property(weights_);
    }

    ~ModWeightedQuadricT() {
        Base::mesh().remove_property(weights_);
    }

    virtual float collapse_priority(const CollapseInfo& ci) {
        const float baseCost = OpenMesh::Decimater::ModQuadricT<MeshT>::collapse_priority(ci);
        if (baseCost != Base::ILLEGAL_COLLAPSE) {
            const double w = Base::mesh().property(weights_, ci.v1);
            return baseCost * w;
        } else {
            return baseCost;
        }
    }

    void setWeights(const std::vector<float>& weights) {
        MeshT& mesh = Base::mesh();
        int i = 0;
        for (auto iter = mesh.vertices_begin(); iter != mesh.vertices_end(); ++iter, ++i) {
            mesh.property(weights_, *iter) = weights[i];
        }
    }

private:
    OpenMesh::VPropHandleT<float> weights_;
};

typedef ModWeightedQuadricT<OMMesh>::Handle HModWeightedQuadric;

#endif

/** Convert mesh to OpenMesh data structure and return mesh extents (2D bounding
 *  box)
 */
math::Extents2 toOpenMesh(const geometry::Mesh &mesh, OMMesh& omMesh)
{
    math::Extents2 e(math::InvalidExtents{});

    // create OpenMesh vertices
    std::vector<OMMesh::VertexHandle> handles;
    handles.reserve(mesh.vertices.size());
    int i = 0;

    for (const auto& v : mesh.vertices) {
        handles.emplace_back(
            omMesh.add_vertex(OMMesh::Point(v(0), v(1), v(2))) );
        update(e, v);

        // copy classification data only if present
        if (mesh.vertecesClass.size()) {
            omMesh.data(handles.back()).classLabel = mesh.vertecesClass[i];
            ++i;
        }
    }

    // create OpenMesh faces
    for (const geometry::Face& face : mesh.faces)
    {
        OMMesh::VertexHandle face_handles[3] = {
            handles[face.a], handles[face.b], handles[face.c]
        };
        omMesh.add_face(face_handles, 3);
    }

    return e;
}

void fromOpenMesh(const OMMesh& omMesh, geometry::Mesh& mesh)
{
    // create our vertices
    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();
              ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        mesh.vertices.emplace_back(pt[0], pt[1], pt[2]);
    }

    // create our faces
    for (auto f_it = omMesh.faces_begin();
              f_it != omMesh.faces_end();
              ++f_it)
    {
        auto fv_it(omMesh.cfv_iter(f_it.handle()));
        int index[3], *p = index;
        for ( ; fv_it; ++fv_it) {
            *p++ = fv_it.handle().idx();
        }
        mesh.faces.emplace_back(index[0], index[1], index[2]);
    }
}

void lockBorder(OMMesh &omMesh, bool inner, bool outer){
    // get bounding box
    double box[2][2] = { {INFINITY, -INFINITY}, {INFINITY, -INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        for (int i = 0; i < 2; i++) {
            if (pt[i] < box[i][0]) box[i][0] = pt[i];
            if (pt[i] > box[i][1]) box[i][1] = pt[i];
        }
    }

    double eps = std::max(box[0][1]-box[0][0], box[1][1]-box[1][0])/(1<<10);
    double epsNormal = 0.001;

    for (auto e_it = omMesh.edges_begin();
              e_it != omMesh.edges_end();  ++e_it)
    {
        if(omMesh.is_boundary(e_it))
        {
            //LOG(info3)<<"--------";
            auto hfhandle(omMesh.halfedge_handle(e_it,0));

            auto from(omMesh.from_vertex_handle(hfhandle));
            auto to(omMesh.to_vertex_handle(hfhandle));

            //check if both points lie on the same border
            auto p1(omMesh.point(from));
            auto p2(omMesh.point(to));
            math::Point3 curNormal(
                math::normalize(math::Point3(p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2])));
            //check if previous edge or next is in the same direction
            bool dirChangeFrom(true);
            bool dirChangeTo(true);
            for(auto vih_it = omMesh.vih_iter(to); vih_it; ++vih_it) {
                if(omMesh.is_boundary(vih_it)){
                    //auto prevhf = omMesh.prev_halfedge_handle(hfhandle);
                    auto pfrom(omMesh.from_vertex_handle(vih_it));
                    auto pto(omMesh.to_vertex_handle(vih_it));
                    auto pp1(omMesh.point(pfrom));
                    auto pp2(omMesh.point(pto));
                    math::Point3 prevNormal(
                    math::normalize(math::Point3(pp2[0]-pp1[0], pp2[1]-pp1[1], pp2[2]-pp1[2])));
                    dirChangeTo = std::abs(ublas::inner_prod(prevNormal,curNormal))<(1.0f-epsNormal);
                }
            }
            for(auto voh_it = omMesh.voh_iter(from); voh_it; ++voh_it) {
                if(omMesh.is_boundary(voh_it)){
                    //auto nexthf = omMesh.next_halfedge_handle(hfhandle);
                    auto nfrom(omMesh.from_vertex_handle(voh_it));
                    auto nto(omMesh.to_vertex_handle(voh_it));
                    auto np1(omMesh.point(nfrom));
                    auto np2(omMesh.point(nto));
                    math::Point3 nextNormal(
                        math::normalize(math::Point3(np2[0]-np1[0], np2[1]-np1[1], np2[2]-np1[2])));
                    dirChangeFrom = std::abs(ublas::inner_prod(nextNormal,curNormal))<(1.0f-epsNormal);
                }
            }

            bool p1Border[4];
            bool p2Border[4];

            p1Border[0] = std::abs(p1[0]-box[0][0])<eps;
            p1Border[1] = std::abs(p1[0]-box[0][1])<eps;
            p1Border[2] = std::abs(p1[1]-box[1][0])<eps;
            p1Border[3] = std::abs(p1[1]-box[1][0])<eps;

            p2Border[0] = std::abs(p2[0]-box[0][0])<eps;
            p2Border[1] = std::abs(p2[0]-box[0][1])<eps;
            p2Border[2] = std::abs(p2[1]-box[1][0])<eps;
            p2Border[3] = std::abs(p2[1]-box[1][0])<eps;

            bool onBorder = false;
            for (int i = 0; i < 4; i++) {
                onBorder = (onBorder || (p1Border[i]==p2Border[i] && p1Border[i]));
            }

            if((inner && !onBorder) || (outer && onBorder)){
                //if they don't lie on the border, lock them
                if(dirChangeFrom){
                    omMesh.status(from).set_locked(true);
                }

                if(dirChangeTo){
                    omMesh.status(to).set_locked(true);
                }
            }
        }
    }
}

void lockCorners(OMMesh &omMesh)
{
    // get bounding box
    double box[2][2] = { {INFINITY, -INFINITY}, {INFINITY, -INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());
        for (int i = 0; i < 2; i++) {
            box[i][0] = std::min(double(pt[i]), box[i][0]);
            box[i][1] = std::max(double(pt[i]), box[i][1]);
        }
    }

    // calculate smallest distances to the four corners of the bounding box
    double dist[2][2] = { {INFINITY, INFINITY}, {INFINITY, INFINITY} };

    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());

        for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
        {
            double d = std::hypot(pt[0] - box[0][i], pt[1] - box[1][j]);
            dist[i][j] = std::min(d, dist[i][j]);
        }
    }

    omMesh.request_vertex_status();

    // lock vertices closest to the corners
    // (note: multiple vertices can have the same minimum distance to a corner)
    for (auto v_it = omMesh.vertices_begin();
              v_it != omMesh.vertices_end();  ++v_it)
    {
        const auto& pt = omMesh.point(v_it.handle());

        for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
        {
            double d = std::hypot(pt[0] - box[0][i], pt[1] - box[1][j]);
            if (std::abs(d - dist[i][j]) < 1e-12)
            {
                auto handle = v_it.handle();
                LOG(info1) << "Locking corner vertex " << handle.idx();
                omMesh.status(handle).set_locked(true);
            }
        }
    }
}

math::Extents2 prepareMeshImpl(OMMesh &omMesh, const Mesh& mesh
                               , const SimplifyOptions &options)
{
    math::Extents2 me;
    me = toOpenMesh(mesh, omMesh);
    omMesh.update_normals();

    // lock the corner and/or border vertices based on flags
    if (options.check(SimplifyOption::CORNERS)) {
        lockCorners(omMesh);
    }
    if (options.check(SimplifyOption::INNERBORDER)) {
        lockBorder(omMesh, true, false);
    }
    if (options.check(SimplifyOption::OUTERBORDER)) {
        lockBorder(omMesh, false, true);
    }
    // return accumulated extents
    return me;
}

math::Extents2 prepareMesh(OMMesh &omMesh, const Mesh& mesh
                           , const SimplifyOptions &options)
{
    // preprocess mesh - remove non-manifold edges
    if (options.check(SimplifyOption::RMNONMANIFOLDEDGES)) {
        auto pmesh(geometry::removeNonManifoldEdges(mesh));
        return prepareMeshImpl(omMesh, *pmesh, options);
    } else {
        return prepareMeshImpl(omMesh, mesh, options);
    }
}

math::Extents2 prepareMesh(OMMesh &omMesh, Mesh&& mesh
                           , const SimplifyOptions &options)
{
    // preprocess mesh - remove non-manifold edges
    if (options.check(SimplifyOption::RMNONMANIFOLDEDGES)) {
        auto pmesh(geometry::removeNonManifoldEdges(std::move(mesh)));
        return prepareMeshImpl(omMesh, *pmesh, options);
    } else {
        return prepareMeshImpl(omMesh, mesh, options);
    }
}

void prepareDecimator(Decimator &decimator
                      , const SimplifyOptions &options)
{
    if (options.alternativeVertices()) {
        if (options.concaveVertexModifier()) {
            LOGTHROW (err3, std::runtime_error) <<
                "Concave/Convex modification is not "
                "implemented for alternative verteces";
        }
        HModQuadricHybrid hModQuadricHybrid;
        decimator.add(hModQuadricHybrid);
        decimator.module(hModQuadricHybrid)
            .setAlternativeVertices(options.alternativeVertices());

        if (options.maxError()) {
            decimator.module(hModQuadricHybrid)
                .set_max_err(*options.maxError(), false);
        }
    } else if (options.concaveVertexModifier()){
#if OM_VERSION >= 0x60000
        // collapse priority based on vertex error quadric
        // adjusted by convexity of the vertex
        HModQuadricConvex hModQuadricConvex;
        decimator.add(hModQuadricConvex);
        decimator.module(hModQuadricConvex).
            set_concave_vertex_modifier(*options.concaveVertexModifier());
        if (options.maxError()) {
            decimator.module(hModQuadricConvex)
                .set_max_err(*options.maxError(), false);
        }
#else
        LOGTHROW (err3, std::runtime_error) <<
            "Concave/Convex decimater is not available on this system.";
#endif
    } else if (options.vertexWeights()) {
        HModWeightedQuadric hModWeightedQuadric;
        decimator.add(hModWeightedQuadric);
        decimator.module(hModWeightedQuadric).setWeights(*options.vertexWeights());
        if (options.maxError()) {
            decimator.module(hModWeightedQuadric).set_max_err(*options.maxError(), false);
        }
    } else {
        // collapse priority based on vertex error quadric
        HModQuadric hModQuadric;
        decimator.add(hModQuadric);
        if (options.maxError()) {
            decimator.module(hModQuadric)
                .set_max_err(*options.maxError(), false);
        }
    }

    // apply normal flipping prevention (if configured)
    if (options.check(SimplifyOption::PREVENTFACEFLIP)) {
        HModNormalFlippingT hModNormalFlippingT;
        decimator.add(hModNormalFlippingT);
        decimator.module(hModNormalFlippingT).set_max_normal_deviation(90);
    }

    // apply aspect ratio (if configured)
    if (options.minAspectRatio()) {
        HModAspectRatioT h;
        decimator.add(h);
        decimator.module(h).set_aspect_ratio(*options.minAspectRatio());
    }

    // apply max edge lenght (if configured)
    if (options.maxEdgeLength()) {
        HModEdgeLengthT h;
        decimator.add(h);
        decimator.module(h).set_edge_length(*options.maxEdgeLength());
    }
}

} // namespace

Mesh::pointer simplify(const Mesh &mesh, int faceCount
                       , const SimplifyOptions &options)
{
    OMMesh omMesh;
    prepareMesh(omMesh, mesh, options);

    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);
    decimator.initialize();

    decimator.decimate_to_faces(0, faceCount);
    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}

void simplifyInPlace(Mesh& mesh, int faceCount, const SimplifyOptions& options) {
    OMMesh omMesh;
    prepareMesh(omMesh, std::move(mesh), options);

    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);
    decimator.initialize();

    decimator.decimate_to_faces(0, faceCount);
    omMesh.garbage_collection();

    mesh.vertices.clear();
    mesh.faces.clear();
    fromOpenMesh(omMesh, mesh);
}

#if OM_VERSION >= 0x60000

Mesh::pointer simplifyToError(const Mesh &mesh, double maxErr
                            , const SimplifyOptions &options)
{
    OMMesh omMesh;

    prepareMesh(omMesh, mesh, options);

    Decimator decimator(omMesh);
    HModQuadric hModQuadric; // collapse priority based on vertex error quadric
    decimator.add(hModQuadric);
    decimator.module( hModQuadric ).set_max_err(maxErr, false);

    if (options.classBasedPlanarisation()) {
        HModClassRestrictT hModClassRestrictT;
        decimator.add (hModClassRestrictT);
        // Some hardcoded classes allowed for decimation
        decimator.module(hModClassRestrictT).allow(13);
        decimator.module(hModClassRestrictT).allow(14);
        decimator.module(hModClassRestrictT).allow(15);
        decimator.module(hModClassRestrictT).allow(16);
        decimator.module(hModClassRestrictT).allow(18);
        decimator.module(hModClassRestrictT).allow(9);
    }
    decimator.initialize();
    decimator.decimate_to_faces(0, 0);
    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}
#endif

namespace {

inline std::size_t gridIndexImpl(const math::Size2ll &size
                                 , const math::Point2 origin
                                 , const math::Size2f tileSize
                                 , double x, double y)
{
    long long xx((x - origin(0)) / tileSize.width);
    long long yy((y - origin(1)) / tileSize.height);

    // clamp to grid (values can be outside of grid in case non-integer
    // tileSize
    xx = math::clamp<long long>(xx, 0LL, size.width - 1);
    yy = math::clamp<long long>(yy, 0LL, size.height -1);

    return xx + yy * size.width;
}

struct Tiling {
    math::Size2ll size;
    math::Point2 origin;
    math::Size2f tileSize;
    FacesPerCell::Functor facesPerCell;

    Tiling() : tileSize() {}

    Tiling(const math::Size2ll &size, const math::Point2 &origin
           , const math::Size2f &tileSize
           , const FacesPerCell::Functor &facesPerCell)
        : size(size), origin(origin), tileSize(tileSize)
        , facesPerCell(facesPerCell)
    {}

    inline std::size_t gridIndex(double x, double y) const {
        return gridIndexImpl(size, origin, tileSize, x, y);
    };

    inline math::Point2 tileLowerLeft(int index){
        int y = index / size.width;
        int x = index % size.width;

        return math::Point2(x * tileSize.width + origin(0)
                            , y * tileSize.height + origin(1));
    }

    inline std::vector<std::size_t> intersectingCells(math::Point2 triangle[3])
        const
    {
        std::vector<std::size_t> result;

        for(uint x = 0; x < size.width; ++x){
            for(uint y = 0; y < size.height; ++y){
                math::Point2 offset(x * tileSize.width
                                    , y * tileSize.height);
                math::Point2 ll = origin + offset;
                math::Point2 ur = ll + math::Point2
                    (tileSize.width, tileSize.height);

                if(math::triangleRectangleCollision(triangle,ll,ur)){
                    result.push_back(x + y * size.width);
                }
            }
        }
        return result;
    };

    /** Calculate maximum face count in all cells of grid. If real value is
     *  lower than maximum, set maximum to real value.
     */
    std::vector<std::size_t> getMax(const std::vector<std::size_t> &current)
        const
    {
        FacesPerCell fpc(size, origin, tileSize);
        facesPerCell(fpc);

        // merge minimum values from current and expected face counts
        auto out(fpc.get());
        auto icurrent(current.begin());
        for (auto &value : out) {
            value = std::min(value, *icurrent++);
        }

        // done
        return out;
    }

    static OpenMesh::MPropHandleT<Tiling> Property;
};

OpenMesh::MPropHandleT<Tiling> Tiling::Property;

template <typename MeshType>
class ModGrid : public OpenMesh::Decimater::ModBaseT<MeshType>
{
public:
    DECIMATING_MODULE(ModGrid, MeshType, Grid);

    ModGrid(MeshType &mesh)
        : Base(mesh, true) // true -> binary module!
        , mesh_(mesh), tiling_()
    {
        mesh_.add_property(cells_);
    }

    ~ModGrid() {
        mesh_.remove_property(cells_);
        //stat();
    }

    virtual void initialize() UTILITY_OVERRIDE {
        auto &prop(mesh_.mproperty(Tiling::Property));
        if (!prop.n_elements()) {
            LOGTHROW(err2, std::runtime_error)
                << "Tiling property not set at mesh.";
        }
        tiling_ = &prop[0];

        calculateGrid();
    }

    virtual float collapse_priority(const CollapseInfo &ci) UTILITY_OVERRIDE {
        // can be both faces removed?
        if (canRemove(ci.fl) && canRemove(ci.fr)) {
            return Base::LEGAL_COLLAPSE;
        }

        return Base::ILLEGAL_COLLAPSE;
    }

    virtual void preprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE {
        removed(ci.fl);
        removed(ci.fr);
    }

    virtual void postprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE
    {
        // traverse all remaining faces of non-removed vertex
        for (auto fi(mesh_.vf_begin(ci.v1)), fe(mesh_.vf_end(ci.v1));
             fi != fe; ++fi)
        {
            const auto f(fi.handle());
            update(f, cell(f), barycenterCell(f));
        }
    }

    std::size_t desiredCount() const {
        return std::accumulate(max_.begin(), max_.end(), std::size_t(0));
    }

private:
    typedef typename MeshType::FaceHandle FaceHandle;
    typedef typename MeshType::Scalar Scalar;

    void stat() const {
        auto ioriginal(original_.begin());
        auto icurrent(current_.begin());
        for (auto max : max_) {
            LOG(info2) << *ioriginal++ << " -> " << *icurrent++
                       << " (" << max << ")";
        }
    }

    void calculateGrid() {
        current_.clear();
        current_.resize(area(tiling_->size));

        for (const auto &f : mesh_.faces()) {
            auto cellIndex(barycenterCell(f));

            // assign cell index to face
            cell(f) = cellIndex;

            // one more face at given cell
            ++current_[cellIndex];
        }

        // get face count limits
        max_ = tiling_->getMax(current_);

        // for statistics
        original_.assign(current_.begin(), current_.end());
    }

    inline std::size_t barycenterCell(FaceHandle f) const {
        // calculate barycenter of face (i.e. average x and y coordinate)
        math::Point2 p;
        int valence(0);
        for (auto vi(mesh_.cfv_iter(f)); vi; ++vi) {
            const auto &v(mesh_.point(vi.handle()));
            p(0) += v[0];
            p(1) += v[1];
            ++valence;
        }

        p(0) /= valence;
        p(1) /= valence;

        return tiling_->gridIndex(p(0), p(1));
    }

    void removed(FaceHandle fh) {
        if (fh.is_valid()) {
            --current_[cell(fh)];
        }
    }

    void update(FaceHandle fh, std::size_t oldCell, std::size_t newCell) {
        if (!fh.is_valid() || (oldCell == newCell)) {
            return;
        }

        // new cell index, update
        --current_[oldCell];
        ++current_[newCell];
        cell(fh) = newCell;
    }

    inline bool canRemove(FaceHandle fh) const {
        if (!fh.is_valid()) { return true; }

        auto index(cell(fh));
        return (current_[index] > max_[index]);
    }

    inline Scalar& cell(FaceHandle fh) {
        return mesh_.property(cells_, fh);
    }

    inline const Scalar& cell(FaceHandle fh) const {
        return mesh_.property(cells_, fh);
    }

    MeshType &mesh_;
    Tiling *tiling_;
    std::vector<std::size_t> current_;
    std::vector<std::size_t> max_;
    std::vector<std::size_t> original_;
    OpenMesh::FPropHandleT<Scalar> cells_;
};

 /*
 * OpenMesh module for decimation mesh in grid.
 * Each cell has defined maximal face count and
 * once this face count is reached, incident faces
 * can't be decimated.
 */
template <typename MeshType>
class ModGrid2 : public OpenMesh::Decimater::ModBaseT<MeshType>
{
public:
    DECIMATING_MODULE(ModGrid2, MeshType, Grid);

    ModGrid2(MeshType &mesh)
        : Base(mesh, true) // true -> binary module!
        , mesh_(mesh), tiling_()
    {
        mesh_.add_property(cells_);
    }

    ~ModGrid2() {
        mesh_.remove_property(cells_);
        //stat();
    }

    virtual void initialize() UTILITY_OVERRIDE {
        auto &prop(mesh_.mproperty(Tiling::Property));
        if (!prop.n_elements()) {
            LOGTHROW(err2, std::runtime_error)
                << "Tiling property not set at mesh.";
        }
        tiling_ = &prop[0];

        calculateGrid();
    }

    virtual float collapse_priority(const CollapseInfo &ci) UTILITY_OVERRIDE {
        // can be both faces removed?
        if (canRemove(ci.fl) && canRemove(ci.fr)) {
            return Base::LEGAL_COLLAPSE;
        }

        return Base::ILLEGAL_COLLAPSE;
    }

    virtual void preprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE {
        removed(ci.fl);
        removed(ci.fr);
    }

    virtual void postprocess_collapse(const CollapseInfo &ci) UTILITY_OVERRIDE
    {
        // traverse all remaining faces of non-removed vertex
        for (auto fi(mesh_.vf_begin(ci.v1)), fe(mesh_.vf_end(ci.v1));
             fi != fe; ++fi)
        {
            const auto f(fi.handle());
            update(f);
        }
    }

    std::size_t desiredCount() const {
        return std::accumulate(max_.begin(), max_.end(), std::size_t(0));
    }

private:
    typedef typename MeshType::FaceHandle FaceHandle;
    typedef typename MeshType::Scalar Scalar;
    typedef typename std::vector<std::size_t> CellList;
    typedef typename std::vector<std::set<FaceHandle>> CellFaces;

    void setCellVerticesAsFeatures(int cellIndex) const{
        for(const auto &face : cellFaces_[cellIndex]){
            for (auto vi(mesh_.cfv_iter(face)); vi; ++vi) {
                mesh_.status(vi.handle()).set_feature(true);
            }
        }
    }

    void printCells() const {
        LOG(info2) << "Cells count:";
        for (uint i=0; i<current_.size(); ++i) {
            LOG(info2) << tiling_->tileLowerLeft(i)<<" - "<<current_[i];
        }
    }

    void stat() const {
        auto ioriginal(original_.begin());
        auto icurrent(current_.begin());
        for (auto max : max_) {
            LOG(info2) << *ioriginal++ << " -> " << *icurrent++
                       << " (" << max << ")";
        }
        printCells();
    }

    void calculateGrid() {
        current_.clear();
        current_.resize(area(tiling_->size));
        cellFaces_.clear();
        cellFaces_.resize(area(tiling_->size));

        for (const auto &f : mesh_.faces()) {
            cellList(f).clear();
            update(f);
        }

        // get face count limits
        max_ = tiling_->getMax(current_);

        // for statistics
        original_.assign(current_.begin(), current_.end());
    }

    void removed(FaceHandle fh) {
        if (fh.is_valid()) {
            for(auto &cell : cellList(fh)){
                --current_[cell];
                cellFaces_[cell].erase(fh);
            }
        }
    }

    void update(FaceHandle fh) {
        if (!fh.is_valid()) {
            return;
        }

        // decrement facecount in old cells
        for(auto &cell : cellList(fh)) {
            --current_[cell];
            cellFaces_[cell].erase(fh);
        }
        // recalculate cells
        calculateCells(fh);

        // increment facecount in new cells
        for(auto &cell : cellList(fh)) {
            ++current_[cell];
            cellFaces_[cell].insert(fh);
        }
    }

    inline void calculateCells(FaceHandle fh){
        math::Point2 vertices[3];
        uint vc = 0;
        for (auto vi(mesh_.cfv_iter(fh)); vi; ++vi) {
            const auto &v(mesh_.point(vi.handle()));
            vertices[vc](0) =  v[0];
            vertices[vc](1) =  v[1];
            vc++;
        }
        cellList(fh) = tiling_->intersectingCells(vertices);
    }

    inline bool canRemove(FaceHandle fh) const {
        if (!fh.is_valid()) { return true; }
        for(auto &cell : cellList(fh)){
            if(current_[cell] <= max_[cell]){
                setCellVerticesAsFeatures(cell);
                return false;
            }
        }
        return true;
    }

    inline CellList& cellList(FaceHandle fh) {
        return mesh_.property(cells_, fh);
    }

    inline const CellList& cellList(FaceHandle fh) const {
        return mesh_.property(cells_, fh);
    }

    MeshType &mesh_;
    Tiling *tiling_;
    std::vector<std::size_t> original_;
    std::vector<std::size_t> current_;
    std::vector<std::size_t> max_;
    OpenMesh::FPropHandleT<CellList> cells_;
    CellFaces cellFaces_;
};

} // namespace

/** Converts coordinate to grid defined by reference point and cell size.
 *  Coordinates not on grid are rounded down.
 */
double gridExtentsDown(double value, double origin, double size)
{
    return std::floor((value - origin) / size) * size + origin;
}

/** Converts coordinate to grid defined by reference point and cell size.
 *  Coordinates not on grid are rounded up.
 */
double gridExtentsUp(double value, double origin, double size)
{
    return std::ceil((value - origin) / size) * size + origin;
}

math::Extents2 gridExtents(const math::Extents2 &extents
                           , const math::Point2 &alignment
                           , const math::Size2f &cellSize)
{
    return {
        gridExtentsDown(extents.ll(0), alignment(0), cellSize.width)
        , gridExtentsDown(extents.ll(1), alignment(1), cellSize.height)
        , gridExtentsUp(extents.ur(0), alignment(0), cellSize.width)
        , gridExtentsUp(extents.ur(1), alignment(1), cellSize.height)
    };
}

std::size_t FacesPerCell::cellIndex(double x, double y) const
{
    return gridIndexImpl(gridSize_, origin_, cellSize_, x, y);
}

math::Extents2 FacesPerCell::cellExtents(std::size_t index) const
{
    // convert linear index to grid index
    auto i(index % gridSize_.width);
    auto j(index / gridSize_.width);

    // return extents
    return math::Extents2(origin_(0) + i * cellSize_.width
                          , origin_(1) + j * cellSize_.height
                          , origin_(0) + (i + 1) * cellSize_.width
                          , origin_(1) + (j + 1) * cellSize_.height);
}

Mesh::pointer simplifyInGrid(const Mesh &mesh, const math::Point2 &alignment
                             , const math::Size2f &cellSize
                             , const FacesPerCell::Functor &facesPerCell
                             , const SimplifyOptions &options)
{
    LOG(info2) << "[simplify] alignment: "
               << std::setprecision(15) << alignment;
    LOG(info2) << "[simplify] cellSize: " << cellSize;

    // convert to openmesh structure
    OMMesh omMesh;
    math::Extents2 me(prepareMesh(omMesh, mesh, options));

    // calculate grid extents
    const auto ge(gridExtents(me, alignment, cellSize));

    // grid origin
    const auto gorigin(ge.ll);

    // grid size
    const math::Size2ll gsize
        ((long long) (std::round(((ge.ur(0) - ge.ll(0)) / cellSize.width)))
         , (long long)(std::round((ge.ur(1) - ge.ll(1)) / cellSize.width)));

    LOG(info2) << "[simplify] mesh extents: " << me;
    LOG(info2) << "[simplify] gridded mesh extents: "
               << std::setprecision(15) << ge;
    LOG(info2) << "[simplify] grid size: " << gsize;
    LOG(info2) << "[simplify] grid origin: " << gorigin;

    // create and add tiling as a mesh property to mesh
    omMesh.add_property(Tiling::Property);
    omMesh.mproperty(Tiling::Property).push_back();
    omMesh.mproperty(Tiling::Property)[0]
        = Tiling(gsize, gorigin, cellSize, facesPerCell);

    // create and prepare decimator
    Decimator decimator(omMesh);
    prepareDecimator(decimator, options);

    // add decimator grid module
    ModGrid<OMMesh>::Handle hModGrid;
    decimator.add(hModGrid);

    // initialized
    decimator.initialize();

    auto fc(decimator.module(hModGrid).desiredCount());
    LOG(info2) << "[simplify] Simplifying mesh to " << fc << " faces.";
    // simplifying to 0 faces, simplification will stop based on grid constraints
    decimator.decimate_to_faces(0, 0);

    omMesh.garbage_collection();

    auto newMesh(std::make_shared<geometry::Mesh>());
    fromOpenMesh(omMesh, *newMesh);
    return newMesh;
}

} // namespace geometry


///////////////////////////////////////////////////////////////////////////////
/// \file find_period.hpp
/// Calculate the first period with which a given run overlaps
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_NUMERIC_DETAIL_FIND_PERIOD_EAN_10_02_2006
#define BOOST_TIME_SERIES_NUMERIC_DETAIL_FIND_PERIOD_EAN_10_02_2006

#include <utility>
#include <boost/assert.hpp>
#include <boost/range_run_storage/offset.hpp>

namespace boost { namespace time_series
{

    namespace detail
    {
        namespace rrs = range_run_storage;

        // Find the lowest period of which this run is a part
        template<typename Run, typename Offset, typename Length>
        inline std::pair<Offset, Offset>
        find_period(Run const &run, Offset offset, Length length)
        {
            Offset o = static_cast<Offset>(rrs::offset(run));
            Offset start = o - (o - offset) % length;
            Offset stop = static_cast<Offset>(start + length);

            BOOST_ASSERT(start <= rrs::offset(run));
            BOOST_ASSERT(stop > rrs::offset(run));
            return std::make_pair(start, stop);
        }

    }

}}

#endif // BOOST_TIME_SERIES_NUMERIC_DETAIL_FIND_PERIOD_EAN_10_02_2006


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Python bindings for the StepperHead class
 */

#include "blmc_controllers/impedance_controller.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>

#include <boost/python.hpp>

namespace py = pybind11;

namespace blmc_controllers{

void bind_impedance_controller(py::module& module)
{
    py::class_<ImpedanceController>(module, "ImpedanceController")
        .def(py::init<>())

        // Public methods.
        .def("initialize",
             [](ImpedanceController& obj,
                py::object model,
                const std::string& root_frame_name,
                const std::string& end_frame_name)
             {
                // Need to use boost-python here as the python object was
                // created in pinocchio using boost-python.
                const pinocchio::Model& pinocchio_model =
                    boost::python::extract<const pinocchio::Model&>(model.ptr());
                 obj.initialize(pinocchio_model, root_frame_name, end_frame_name);
                 return;
             })
        .def("run", [](ImpedanceController& obj,
                       const Eigen::VectorXd& robot_configuration,
                       const Eigen::VectorXd& robot_velocity,
                       const ImpedanceController::Array6d& gain_proportional,
                       const ImpedanceController::Array6d& gain_derivative,
                       const double& gain_feed_forward_force,
                       py::object py_desired_end_frame_placement,
                       py::object py_desired_end_frame_velocity,
                       py::object py_feed_forward_force)
                    {
                        // Need to use boost-python here as the python object was
                        // created in pinocchio using boost-python.
                        const pinocchio::SE3& desired_end_frame_placement =
                            boost::python::extract<const pinocchio::SE3&>(py_desired_end_frame_placement.ptr());
                        const pinocchio::Motion& desired_end_frame_velocity =
                            boost::python::extract<const pinocchio::Motion&>(py_desired_end_frame_velocity.ptr());
                        const pinocchio::Force& feed_forward_force =
                            boost::python::extract<const pinocchio::Force&>(py_feed_forward_force.ptr());
                        obj.run(robot_configuration, robot_velocity,
                                gain_proportional, gain_derivative,
                                gain_feed_forward_force,
                                desired_end_frame_placement,
                                desired_end_frame_velocity,
                                feed_forward_force);
                        return;
                    })
        .def("get_torques",
             &ImpedanceController::get_torques,
             py::return_value_policy::reference)
        .def("get_impedance_force",
             &ImpedanceController::get_impedance_force,
             py::return_value_policy::reference);
}

} // blmc_controllers


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <iostream>
#include <cmath>

#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "tudat/math/basic/coordinateConversions.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/math/basic/mathematicalConstants.h"
#include "tudat/astro/basic_astro/celestialBodyConstants.h"

#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/astro/basic_astro/sphericalBodyShapeModel.h"
#include "tudat/astro/ephemerides/simpleRotationalEphemeris.h"
//#include "tudat/astro/reference_frames/referenceFrameTransformations.h"
#include "tudat/interface/spice/spiceRotationalEphemeris.h"

namespace tudat
{

namespace simulation_setup
{

using namespace ephemerides;
using namespace gravitation;
using namespace basic_astrodynamics;

void addAerodynamicCoefficientInterface(
        const SystemOfBodies& bodies, const std::string bodyName,
        const std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings )
{
    if( bodies.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting aerodynamic coefficients for body "+ bodyName + ", body is not found in system of bodies" );
    }
    bodies.at( bodyName )->setAerodynamicCoefficientInterface(
                createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, bodyName) );
}

void addRadiationPressureInterface(
        const SystemOfBodies& bodies, const std::string bodyName,
        const std::shared_ptr< RadiationPressureInterfaceSettings > radiationPressureSettings )
{
    if( bodies.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting radiation pressure interface for body "+ bodyName + ", body is not found in system of bodies" );
    }
    bodies.at( bodyName )->setRadiationPressureInterface(
                radiationPressureSettings->getSourceBody( ), createRadiationPressureInterface(
                    radiationPressureSettings, bodyName, bodies ) );
}

void setSimpleRotationSettingsFromSpice(
        const BodyListSettings& bodySettings, const std::string& bodyName, const double spiceEvaluationTime )
{
    if( bodySettings.count( bodyName ) == 0 )
    {
        throw std::runtime_error( "Error when setting simple rotation model settings for body " +
                                  bodyName + ", no settings found for this body." );
    }

    Eigen::Quaterniond rotationAtReferenceTime =
            spice_interface::computeRotationQuaternionBetweenFrames(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, spiceEvaluationTime );
    double rotationRateAtReferenceTime =
            spice_interface::getAngularVelocityVectorOfFrameInOriginalFrame(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, spiceEvaluationTime ).norm( );

    bodySettings.at( bodyName )->rotationModelSettings =
            std::make_shared< SimpleRotationModelSettings >(
                bodySettings.getFrameOrientation( ), "IAU_" + bodyName, rotationAtReferenceTime,
                spiceEvaluationTime, rotationRateAtReferenceTime );
}

void addEmptyTabulatedEphemeris(
        const SystemOfBodies& bodies, const std::string& bodyName, const std::string& ephemerisOrigin )
{
    if( bodies.count( bodyName ) ==  0 )
    {
        throw std::runtime_error( "Error when setting empty tabulated ephemeris for body " + bodyName + ", no such body found" );
    }
    std::string ephemerisOriginToUse = ( ephemerisOrigin == "" ) ? bodies.getFrameOrigin( ) : ephemerisOrigin;
    bodies.at( bodyName )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                            < double, Eigen::Vector6d > >( ), ephemerisOriginToUse, bodies.getFrameOrientation( ) ) );

    bodies.processBodyFrameDefinitions( );
}

void addEmptyTabulatedRotationalEphemeris(
        const SystemOfBodies& bodies, const std::string& bodyName, const std::string& bodyFixedFrameName )
{
    if( bodies.count( bodyName ) ==  0 )
    {
        throw std::runtime_error( "Error when setting empty tabulated rotational ephemeris for body " + bodyName + ", no such body found" );
    }
    std::string bodyFixedFrameNameToUse = ( bodyFixedFrameName == "" ) ? ( bodyName + "_fixed" ) : bodyFixedFrameName;

    bodies.at( bodyName )->setRotationalEphemeris( std::make_shared< ephemerides::TabulatedRotationalEphemeris< > >(
                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                            < double, Eigen::Vector7d > >( ), bodies.getFrameOrientation( ), bodyFixedFrameNameToUse ) );
}

//! Function that determines the order in which bodies are to be created
std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > determineBodyCreationOrder(
        const std::map< std::string, std::shared_ptr< BodySettings > >& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > outputVector;

    // Create vector of pairs (body name and body settings) that is to be created.
    for( std::map< std::string, std::shared_ptr< BodySettings > >::const_iterator bodyIterator
         = bodySettings.begin( );
         bodyIterator != bodySettings.end( ); bodyIterator ++ )
    {
        outputVector.push_back( std::make_pair( bodyIterator->first, bodyIterator->second ) );
    }

    return outputVector;
}

//! Function to create a map of bodies objects.
SystemOfBodies createSystemOfBodies(
        const BodyListSettings& bodySettings )
{
    std::vector< std::pair< std::string, std::shared_ptr< BodySettings > > > orderedBodySettings
            = determineBodyCreationOrder( bodySettings.getMap( ) );

    // Declare map of bodies that is to be returned.
    SystemOfBodies bodyList = SystemOfBodies(
                bodySettings.getFrameOrigin( ), bodySettings.getFrameOrientation( ) );

    // Create empty body objects.
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        bodyList.createEmptyBody( orderedBodySettings.at( i ).first, false );
    }

    // Define constant mass for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        const double constantMass = orderedBodySettings.at( i ).second->constantMass;
        if ( constantMass == constantMass )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setConstantBodyMass( constantMass );
        }
    }

    // Create ephemeris objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->ephemerisSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setEphemeris(
                        createBodyEphemeris( orderedBodySettings.at( i ).second->ephemerisSettings,
                                             orderedBodySettings.at( i ).first ) );
        }
    }

    // Create atmosphere model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->atmosphereSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setAtmosphereModel(
                        createAtmosphereModel( orderedBodySettings.at( i ).second->atmosphereSettings,
                                               orderedBodySettings.at( i ).first ) );
        }
    }

    // Create body shape model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->shapeModelSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setShapeModel(
                        createBodyShapeModel( orderedBodySettings.at( i ).second->shapeModelSettings,
                                              orderedBodySettings.at( i ).first ) );
        }
    }

    // Create rotation model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->rotationModelSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setRotationalEphemeris(
                        createRotationModel( orderedBodySettings.at( i ).second->rotationModelSettings,
                                             orderedBodySettings.at( i ).first, bodyList ) );
        }
    }

    // Create gravity field model objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setGravityFieldModel(
                        createGravityFieldModel( orderedBodySettings.at( i ).second->gravityFieldSettings,
                                                 orderedBodySettings.at( i ).first, bodyList,
                                                 orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->gravityFieldVariationSettings.size( ) > 0 )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setGravityFieldVariationSet(
                        createGravityFieldModelVariationsSet(
                            orderedBodySettings.at( i ).first, bodyList,
                            orderedBodySettings.at( i ).second->gravityFieldVariationSettings ) );
        }
    }

    // Create aerodynamic coefficient interface objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        if( orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings != nullptr )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setAerodynamicCoefficientInterface(
                        createAerodynamicCoefficientInterface(
                            orderedBodySettings.at( i ).second->aerodynamicCoefficientSettings,
                            orderedBodySettings.at( i ).first ) );
        }
    }

    // Create radiation pressure coefficient objects for each body (if required).
    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >
                radiationPressureSettings
                = orderedBodySettings.at( i ).second->radiationPressureSettings;
        for( std::map< std::string, std::shared_ptr< RadiationPressureInterfaceSettings > >::iterator
             radiationPressureSettingsIterator = radiationPressureSettings.begin( );
             radiationPressureSettingsIterator != radiationPressureSettings.end( );
             radiationPressureSettingsIterator++ )
        {
            bodyList.at( orderedBodySettings.at( i ).first )->setRadiationPressureInterface(
                        radiationPressureSettingsIterator->first,
                        createRadiationPressureInterface(
                            radiationPressureSettingsIterator->second,
                            orderedBodySettings.at( i ).first, bodyList ) );
        }

    }

    for( unsigned int i = 0; i < orderedBodySettings.size( ); i++ )
    {
        for( unsigned int j = 0; j < orderedBodySettings.at( i ).second->groundStationSettings.size( ); j++ )
        {
            createGroundStation( bodyList.at( orderedBodySettings.at( i ).first ), orderedBodySettings.at( i ).first,
                     orderedBodySettings.at( i ).second->groundStationSettings.at( j ) );
        }
    }

    bodyList.processBodyFrameDefinitions( );

    return bodyList;

}

//! Function to create a simplified system of bodies
simulation_setup::SystemOfBodies createSimplifiedSystemOfBodies(const double secondsSinceJ2000)
{
    using namespace ephemerides;
    using namespace gravitation;

    // Creation of bodies
    SystemOfBodies bodies("SSB","ECLIPJ2000");
    bodies.createEmptyBody( "Sun" );
    bodies.createEmptyBody( "Mercury" );
    bodies.createEmptyBody( "Venus" );
    bodies.createEmptyBody( "Earth" );
    bodies.createEmptyBody( "Mars" );
    bodies.createEmptyBody( "Jupiter" );
    bodies.createEmptyBody( "Saturn" );
    bodies.createEmptyBody( "Uranus" );
    bodies.createEmptyBody( "Neptune" );
    bodies.createEmptyBody( "Pluto" );

    // Ephemerides
    bodies.getBody( "Sun" )->setEphemeris( std::make_shared< ConstantEphemeris >( Eigen::Vector6d::Zero( )) );
    bodies.getBody( "Mercury" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Mercury") );
    bodies.getBody( "Venus" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Venus" ) );
    bodies.getBody( "Earth" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Earth" ) );
    bodies.getBody( "Mars" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Mars" ) );
    bodies.getBody( "Jupiter" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Jupiter" ) );
    bodies.getBody( "Saturn" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Saturn" ) );
    bodies.getBody( "Uranus" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Uranus" ) );
    bodies.getBody( "Neptune" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Neptune" ) );
    bodies.getBody( "Pluto" )->setEphemeris( std::make_shared< ApproximateGtopEphemeris >("Pluto" ) );

    // Gravity field
    bodies.getBody( "Sun" )->setGravityFieldModel( std::make_shared< GravityFieldModel >(celestial_body_constants::SUN_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Mercury" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::MERCURY_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Venus" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::VENUS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Earth" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::EARTH_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Mars" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::MARS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Jupiter" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::JUPITER_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Saturn" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::SATURN_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Uranus" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::URANUS_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Neptune" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::NEPTUNE_GRAVITATIONAL_PARAMETER ) );
    bodies.getBody( "Pluto" )->setGravityFieldModel( std::make_shared< GravityFieldModel >( celestial_body_constants::PLUTO_GRAVITATIONAL_PARAMETER ) );

    // Earth's shape model
    bodies.getBody( "Earth" )->setShapeModel( std::make_shared< SphericalBodyShapeModel >(celestial_body_constants::EARTH_EQUATORIAL_RADIUS ) );

    // Calculate position of rotation axis at initialTime, with respect to J2000 frame. Values from:
    // "Report of the IAU Working Group on Cartographic Coordinates and Rotational Elements: 2009", B.A. Archinal et al.(2011)
    const double daysSinceJ2000 = secondsSinceJ2000 / 86400;
    const double centuriesSinceJ2000 = daysSinceJ2000 / 36525;
    const double poleRightAscension = (0 - 0.641 * centuriesSinceJ2000) * mathematical_constants::PI/180;
    const double poleDeclination = (90 - 0.557 * centuriesSinceJ2000) * mathematical_constants::PI/180;
    const double initialRotationAngle = 190.147 * mathematical_constants::PI/180;
    const double rotationRate = 360.985235 * mathematical_constants::PI/180 / physical_constants::JULIAN_DAY;

    Eigen::Matrix3d J2000toPlanetocentricMatrix = reference_frames::getInertialToPlanetocentricFrameTransformationMatrix (
            poleDeclination, poleRightAscension, initialRotationAngle);
    Eigen::Matrix3d ECLIPJ2000toJ2000Matrix = reference_frames::getECLIPJ2000toJ2000TransformationMatrix();
    Eigen::Matrix3d ECLIPJ2000toPlanetocentricMatrix =  J2000toPlanetocentricMatrix * ECLIPJ2000toJ2000Matrix;

    bodies.getBody( "Earth" )->setRotationalEphemeris(std::make_shared< SimpleRotationalEphemeris >(
                    Eigen::Quaterniond(ECLIPJ2000toPlanetocentricMatrix),
                    rotationRate, secondsSinceJ2000, "ECLIPJ2000", "Earth_Fixed" ) );

    return bodies;
}

} // namespace simulation_setup

} // namespace tudat