/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     hypothesis_features.cpp
 * \author   Collin Johnson
 *
 * Definition of HypothesisFeatures.
 */

#include "hssh/local_topological/area_detection/labeling/hypothesis_features.h"
#include "core/float_comparison.h"
#include "hssh/local_topological/area_detection/gateways/gateway_utils.h"
#include "hssh/local_topological/area_detection/labeling/area_graph.h"
#include "hssh/local_topological/area_detection/labeling/boundary.h"
#include "hssh/local_topological/area_detection/labeling/hypothesis.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "hssh/local_topological/area_extent.h"
#include "hssh/local_topological/voronoi_skeleton_grid.h"
#include "math/covariance.h"
#include "math/moments_features.h"
#include "math/statistics.h"
#include "math/zernike_moments.h"
#include "utils/float_io.h"
#include "utils/visibility_graph.h"
#include "utils/visibility_graph_feature.h"
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <cmath>
#include <functional>
#include <sstream>

namespace vulcan
{
namespace hssh
{

const int kFeaturesVersion = 249;   // version number to assign to newly calculated features
const int kIsovistFeaturesVersion = 237;

// Keep a single instance so the pre-cached values can be used repeatedly, which will speed up the overall moments
// computation, which can be quite slow
static math::ZernikeMoments<5> zernike;

enum FeatureIndex
{
    kLoopDist,
    //     kUniqueNeighbors,
    kNumGateways,
    kEdgeLength,
    kAvgGatewayLength,
    kMaxGatewayLength,
    kGatewayRatio,
    //     kShapeArea,
    //     kShapePerimeter,
    kAxisRatioIndex,
    //     kWeightedAxisRatio,
    //     kMeanAreaComp,
    //     kStdAreaComp,
    //     kMeanPerimeterComp,
    //     kStdPerimeterComp,
    kShapeEccentricity,
    kShapeCircularity,
    //     kShapeCompactness,
    //     kDiscretizedCompactness,
    //     kAminCompactness,
    //     kAmaxCompactness,
    //     kPminCompactness,
    //     kPmaxCompactness,
    //     kShapeWaviness,
    //     kIsovistMeanArea,
    //     kIsovistStdArea,
    kIsovistVariationArea,
    //     kIsovistMeanPerimeter,
    //     kIsovistStdPerimeter,
    kIsovistVariationPerimeter,
    kIsovistMeanEccentricity,
    kIsovistStdEccentricity,
    kIsovistMeanCircularity,
    kIsovistStdCircularity,
    kIsovistMeanWaviness,
    kIsovistStdWaviness,
    kIsovistMeanCompactness,
    kIsovistStdCompactness,
    kIsovistMeanRayCompactness,
    kIsovistStdRayCompactness,
    kIsovistMeanShapeDistCompactness,
    kIsovistStdShapeDistCompactness,
    //     kIsovistMeanRelation,
    //     kIsovistStdRelation,
    kIsovistVariationRelation,
    //     kIsovistMeanDelta,
    //     kIsovistStdDelta,
    kIsovistVariationDelta,
    kVisGraphMeanClusterCoeff,
    kVisGraphStdClusterCoeff,
    kVisGraphMeanBetweenness,
    kVisGraphStdBetweenness,
    kVisGraphMeanPageRank,
    kVisGraphStdPageRank,
    kSkeletonMaxBetweenness,
    kSkeletonMeanBetweenness,
    kSkeletonStdBetweenness,
    //     kIsovistDistMinMean,
    //     kIsovistDistMinStd,
    //     kIsovistDistMinVariation,
    //     kIsovistDistMean,
    //     kIsovistDistStd,
    kIsovistDistVariation,
    //     kIsovistDistFromCenterMean,
    //     kIsovistDistFromCenterStd,
    kIsovistDistFromCenterVariation,
    kIsovistAngleBetweenMinDistsMean,
    kIsovistAngleBetweenMinDistsStd,
    //     kIsovistAreaBalanceMean,
    //     kIsovistAreaBalanceStd,
    kNumFixedFeatures
};

enum IsovistFeatureIndex
{
    kSingleEccentricity,
    kSingleCircularity,
    kSingleWaviness,
    kSingleCompactness,
    kSingleRayCompactness,
    kSingleShapeDistCompactness,
    //     kSingleDVariation,
    //     kSingleDeltaVariation,
    kSingleAvgDelta,
    kSingleStdDelta,
    kSingleAvgRelation,
    kSingleStdRelation,
    //     kSingleDistMin,
    kSingleDistMean,
    kSingleDistStd,
    //     kSingleDistSkewness,
    kSingleDistFromCenterMean,
    kSingleDistFromCenterStd,
    //     kSingleDistFromCenterVariation,
    kSingleAngleBetweenMinDists,
    //     kSingleMinHalfArea,
    //     kSingleAreaBalance,
    kSingleArea,
    kSinglePerimeter,
    kNumSingle,
};

struct CachedFeatures
{
    std::vector<cell_t> boundaryCells;
    HypothesisFeatures features;
};

static std::unordered_map<std::size_t, std::vector<CachedFeatures>> kFeaturesCache;

HypothesisFeatures* find_cached_features(const AreaHypothesis& hyp);
void add_features_to_cache(const AreaHypothesis& hyp, const HypothesisFeatures& features);

int current_hypothesis_features_version(void)
{
    return kFeaturesVersion;
}

////////////////////  Operators ///////////////////////////

std::ostream& operator<<(std::ostream& out, const HypothesisFeatures& features)
{
    out << features.version() << ' ' << features.numFeatures() << ' ';
    for (auto f : features) {
        utils::save_floating_point(out, f);
    }

    out << features.numIsovists() << ' ' << features.numIsovistFeatures() << ' ';
    for (auto f : features.isovistFeatures()) {
        utils::save_floating_point(out, f);
    }
    utils::save_floating_point(out, features.explorationAmount());
    return out;
}

std::istream& operator>>(std::istream& in, HypothesisFeatures& features)
{
    in >> features.version_;

    int numFeatures = 0;
    in >> numFeatures;

    features.features_.resize(numFeatures);
    for (int n = 0; n < numFeatures; ++n) {
        features.features_[n] = utils::load_floating_point(in);
    }

    int numIsovists = 0;
    int numIsovistFeatures = 0;
    in >> numIsovists >> numIsovistFeatures;
    features.isovistFeatures_.resize(numIsovistFeatures, numIsovists);
    for (auto& f : features.isovistFeatures_) {
        f = utils::load_floating_point(in);
    }
    features.exploration_ = utils::load_floating_point(in);
    return in;
}

bool operator==(const HypothesisFeatures& lhs, const HypothesisFeatures& rhs)
{
    if (lhs.numFeatures() != rhs.numFeatures()) {
        return false;
    }

    bool areSame = absolute_fuzzy_equal(lhs.explorationAmount(), rhs.explorationAmount());

    for (std::size_t n = 0; n < lhs.numFeatures(); ++n) {
        areSame = absolute_fuzzy_equal(lhs.featureAt(n), rhs.featureAt(n));

        if (!areSame) {
            break;
        }
    }

    return areSame;
}

bool operator!=(const HypothesisFeatures& lhs, const HypothesisFeatures& rhs)
{
    return !(lhs == rhs);
}

//////////////////// HypothesisFeatures /////////////////////////////

void HypothesisFeatures::ClearCache(void)
{
    kFeaturesCache.clear();
}

HypothesisFeatures::HypothesisFeatures(const AreaHypothesis& hypothesis,
                                       const AreaExtent& extent,
                                       const VoronoiSkeletonGrid& grid,
                                       const VoronoiIsovistField& isovistField,
                                       const utils::VisibilityGraph& visGraph,
                                       const utils::VisibilityGraph& skeletonGraph)
: HypothesisFeatures()
{
    //     HypothesisFeatures* cached = find_cached_features(hypothesis);
    //     if(cached)
    //     {
    //         *this = *cached;
    //     }
    //     else
    //     {
    assert(features_.size() == numFeatures());

    exploration_ = 1.0 - hypothesis.extent().frontierRatio();

    features_.fill(NAN);
    calculateStructureFeatures(hypothesis, grid);
    calculateAxisFeatures(hypothesis, grid);
    calculateIsovistFeatures(hypothesis, isovistField);
    calculateVisGraphFeatures(hypothesis, visGraph);
    calculateSkeletonGraphFeatures(hypothesis, skeletonGraph);
    calculateShapeFeatures(extent, grid);
    //         calculateZernikeFeature(extent);

    //         axisRatio_ = features_[kAxisRatioIndex];

    //         add_features_to_cache(hypothesis, *this);

    for (arma::uword idx = 0; idx < features_.n_elem; ++idx) {
        if (std::isnan(features_[idx])) {
            std::cout << "Found NAN in feature: " << featureName(idx)
                      << " Area:" << extent.rectangleBoundary(math::ReferenceFrame::GLOBAL) << '\n'
                      << " Num cells:" << hypothesis.numSkeleton() << '\n';
            assert(!std::isnan(features_[idx]));
        }
    }
    //     }
}

HypothesisFeatures::HypothesisFeatures(const Vector& features, double exploration, double axisRatio)
: version_(kFeaturesVersion)
, isovistVersion_(kIsovistFeaturesVersion)
, features_(features)
, exploration_(exploration)
, axisRatio_(axisRatio)
{
}

HypothesisFeatures::HypothesisFeatures(void)
: version_(kFeaturesVersion)
, isovistVersion_(kIsovistFeaturesVersion)
, features_(kNumFixedFeatures)
// , features_(kNumFixedFeatures + zernike.numMoments())
{
}

std::string HypothesisFeatures::featureName(int index)
{
    FeatureIndex feature = static_cast<FeatureIndex>(index);

    switch (feature) {
    case kLoopDist:
        return "loop dist";
        //         case kUniqueNeighbors:
        //             return "unique neighbors";
    case kNumGateways:
        return "num gateways";
    case kEdgeLength:
        return "edge length";
    case kAvgGatewayLength:
        return "avg gateway length";
    case kMaxGatewayLength:
        return "max gateway length";
    case kGatewayRatio:
        return "gateway ratio";
        //         case kShapeArea:
        //             return "shape area";
        //         case kShapePerimeter:
        //             return "shape perimeter";
    case kAxisRatioIndex:
        return "axis ratio";
        //         case kWeightedAxisRatio:
        //             return "weighted axis ratio";
        //         case kMeanAreaComp:
        //             return "area comp mean";
        //         case kStdAreaComp:
        //             return "area comp std";
        //         case kMeanPerimeterComp:
        //             return "perimeter comp mean";
        //         case kStdPerimeterComp:
        //             return "perimeter comp std";
    case kShapeEccentricity:
        return "shape eccentricity";
    case kShapeCircularity:
        return "shape circularity";
        //         case kShapeCompactness:
        //             return "shape compactness";
        //         case kDiscretizedCompactness:
        //             return "discretized compactness";
        //         case kAminCompactness:
        //             return "a_min compactness";
        //         case kAmaxCompactness:
        //             return "a_max compactness";
        //         case kPminCompactness:
        //             return "p_min compactness";
        //         case kPmaxCompactness:
        //             return "p_max compactness";
        //         case kShapeWaviness:
        //             return "shape waviness";
        //         case kIsovistMeanArea:
        //             return "area mean";
        //         case kIsovistStdArea:
        //             return "area std";
    case kIsovistVariationArea:
        return "area variation";
        //         case kIsovistMeanPerimeter:
        //             return "perimeter mean";
        //         case kIsovistStdPerimeter:
        //             return "perimeter std";
    case kIsovistVariationPerimeter:
        return "perimeter variation";
    case kIsovistMeanEccentricity:
        return "eccentricity mean";
    case kIsovistStdEccentricity:
        return "eccentricity std";
    case kIsovistMeanCircularity:
        return "circularity mean";
    case kIsovistStdCircularity:
        return "circularity std";
    case kIsovistMeanWaviness:
        return "waviness mean";
    case kIsovistStdWaviness:
        return "waviness std";
    case kIsovistMeanCompactness:
        return "compactness mean";
    case kIsovistStdCompactness:
        return "compactness std";
    case kIsovistMeanRayCompactness:
        return "ray compactness mean";
    case kIsovistStdRayCompactness:
        return "ray compactness std";
    case kIsovistMeanShapeDistCompactness:
        return "shape dist compactness mean";
    case kIsovistStdShapeDistCompactness:
        return "shape dist compactness std";
        //         case kIsovistMeanRelation:
        //             return "relation mean";
        //         case kIsovistStdRelation:
        //             return "relation std";
    case kIsovistVariationRelation:
        return "relation variation";
        //         case kIsovistMeanDelta:
        //             return "delta mean";
        //         case kIsovistStdDelta:
        //             return "delta std";
    case kIsovistVariationDelta:
        return "delta variation";
    case kVisGraphMeanClusterCoeff:
        return "cluster coeff mean";
    case kVisGraphStdClusterCoeff:
        return "cluster coeff std";
    case kVisGraphMeanBetweenness:
        return "betweenness mean";
    case kVisGraphStdBetweenness:
        return "betweenness std";
    case kVisGraphMeanPageRank:
        return "pagerank mean";
    case kVisGraphStdPageRank:
        return "pagerank std";
    case kSkeletonMaxBetweenness:
        return "skel betweenness max";
    case kSkeletonMeanBetweenness:
        return "skel betweenness mean";
    case kSkeletonStdBetweenness:
        return "skel betweenness std";
        //         case kIsovistDistMinMean:
        //             return "min dist mean";
        //         case kIsovistDistMinStd:
        //             return "min dist std";
        //         case kIsovistDistMinVariation:
        //             return "min dist variation";
        //         case kIsovistDistMean:
        //             return "dist mean";
        //         case kIsovistDistStd:
        //             return "dist std";
    case kIsovistDistVariation:
        return "dist variation";
        //         case kIsovistDistFromCenterMean:
        //             return "dist from center mean";
        //         case kIsovistDistFromCenterStd:
        //             return "dist from center std";
    case kIsovistDistFromCenterVariation:
        return "dist from center variation";
    case kIsovistAngleBetweenMinDistsMean:
        return "angle between min mean";
    case kIsovistAngleBetweenMinDistsStd:
        return "angle between min std";
        //         case kIsovistAreaBalanceMean:
        //             return "area balance mean";
        //         case kIsovistAreaBalanceStd:
        //             return "area balance std";
    case kNumFixedFeatures:
    default:
        break;
    }

    if ((index >= kNumFixedFeatures) && (index < static_cast<int>(kNumFixedFeatures + zernike.numMoments()))) {
        std::ostringstream str;
        str << "zernike: " << (index - kNumFixedFeatures);
        return str.str();
    }
    return "error";
}

std::string HypothesisFeatures::isovistFeatureName(int index)
{
    switch (index) {
    case kSingleEccentricity:
        return utils::Isovist::scalarName(utils::Isovist::kShapeEccentricity);
    case kSingleCircularity:
        return utils::Isovist::scalarName(utils::Isovist::kCircularity);
    case kSingleWaviness:
        return utils::Isovist::scalarName(utils::Isovist::kShapeWaviness);
    case kSingleCompactness:
        return utils::Isovist::scalarName(utils::Isovist::kShapeCompactness);
    case kSingleRayCompactness:
        return utils::Isovist::scalarName(utils::Isovist::kRayCompactness);
    case kSingleShapeDistCompactness:
        return utils::Isovist::scalarName(utils::Isovist::kShapeDistCompactness);
        //     case kSingleDVariation:
        //         return utils::Isovist::scalarName(utils::Isovist::kDVariation);
        //     case kSingleDeltaVariation:
        //         return utils::Isovist::scalarName(utils::Isovist::kDeltaVariation);
    case kSingleAvgDelta:
        return utils::Isovist::scalarName(utils::Isovist::kDeltaAvg);
    case kSingleStdDelta:
        return utils::Isovist::scalarName(utils::Isovist::kDeltaStd);
    case kSingleAvgRelation:
        return utils::Isovist::scalarName(utils::Isovist::kDistRelationAvg);
    case kSingleStdRelation:
        return utils::Isovist::scalarName(utils::Isovist::kDistRelationStd);
        //     case kSingleDistMin:
        //         return utils::Isovist::scalarName(utils::Isovist::kDmin);
    case kSingleDistMean:
        return utils::Isovist::scalarName(utils::Isovist::kDavg);
    case kSingleDistStd:
        return utils::Isovist::scalarName(utils::Isovist::kDstd);
        //     case kSingleDistSkewness:
        //         return utils::Isovist::scalarName(utils::Isovist::kDskewness);
    case kSingleDistFromCenterMean:
        return utils::Isovist::scalarName(utils::Isovist::kShapeDistAvg);
    case kSingleDistFromCenterStd:
        return utils::Isovist::scalarName(utils::Isovist::kShapeDistStd);
        //     case kSingleDistFromCenterVariation:
        //         return utils::Isovist::scalarName(utils::Isovist::kShapeDistVariation);
    case kSingleAngleBetweenMinDists:
        return utils::Isovist::scalarName(utils::Isovist::kAngleBetweenMinDists);
        //     case kSingleMinHalfArea:
        //         return utils::Isovist::scalarName(utils::Isovist::kMinHalfArea);
        //     case kSingleAreaBalance:
        //         return utils::Isovist::scalarName(utils::Isovist::kAreaBalance);
    case kSingleArea:
        return utils::Isovist::scalarName(utils::Isovist::kArea);
    case kSinglePerimeter:
        return utils::Isovist::scalarName(utils::Isovist::kPerimeter);
    }

    std::ostringstream str;
    str << "zernike: " << (index - kNumSingle);
    return str.str();
}

void HypothesisFeatures::calculateStructureFeatures(const AreaHypothesis& hypothesis, const VoronoiSkeletonGrid& grid)
{
    double totalGwyPerim = 0.0;

    for (auto& bnd : boost::make_iterator_range(hypothesis.beginBoundary(), hypothesis.endBoundary())) {
        totalGwyPerim += gateway_cell_perimeter(bnd->getGateway(), grid.metersPerCell());
    }

    if (hypothesis.extent().perimeter() > 0.0) {
        features_[kGatewayRatio] = totalGwyPerim / hypothesis.extent().perimeter();
    } else {
        features_[kGatewayRatio] = 0.0;
    }

    double minLoop = HUGE_VAL;
    for (auto& node : boost::make_iterator_range(hypothesis.beginNode(), hypothesis.endNode())) {
        minLoop = std::min(minLoop, node->getLoopDistance());
    }

    features_[kLoopDist] = minLoop;

    //     auto neighbors = hypothesis.findAdjacentHypotheses();
    //     double totalNeighbors = neighbors.size();
    //     std::sort(neighbors.begin(), neighbors.end());
    //     utils::erase_unique(neighbors);
    //
    //     features_[kUniqueNeighbors] = neighbors.empty() ? 0.0 : neighbors.size() / totalNeighbors;

    features_[kNumGateways] = hypothesis.numBoundaries();

    AreaGraph* graph = hypothesis.areaGraph();
    double totalLength = 0.0;
    int totalDists = 0;
    for (auto& node : boost::make_iterator_range(hypothesis.beginNode(), hypothesis.endNode())) {
        for (auto& otherNode : boost::make_iterator_range(hypothesis.beginNode(), hypothesis.endNode())) {
            totalLength += graph->distanceBetweenNodes(node, otherNode);
            ++totalDists;
        }
    }

    features_[kEdgeLength] = (totalDists > 0) ? totalLength / totalDists : 0.0;

    double totalGwyLength = 0.0;
    double maxGwyLength = 0.0;
    int numGwys = 0;

    for (auto& bnd : boost::make_iterator_range(hypothesis.beginBoundary(), hypothesis.endBoundary())) {
        totalGwyLength += bnd->length();
        maxGwyLength = std::max(bnd->length(), maxGwyLength);
    }

    features_[kAvgGatewayLength] = (numGwys > 0) ? totalGwyLength / numGwys : 0.0;
    features_[kMaxGatewayLength] = maxGwyLength;
}

void HypothesisFeatures::calculateAxisFeatures(const AreaHypothesis& hypothesis, const VoronoiSkeletonGrid& grid)
{
    std::vector<double> weights(hypothesis.numSkeleton(), 1.0);
    auto features =
      math::point2D_covariance_properties(hypothesis.beginSkeleton(), hypothesis.endSkeleton(), weights.begin());
    features_[kAxisRatioIndex] = features.eccentricity;

    //     std::transform(hypothesis.beginSkeleton(), hypothesis.endSkeleton(), weights.begin(), [&grid](cell_t c) {
    //         return grid.getMetricDistance(c.x, c.y);
    //     });
    //
    //     auto weightedFeatures = math::point2D_covariance_properties(hypothesis.beginSkeleton(),
    //                                                                 hypothesis.endSkeleton(),
    //                                                                 weights.begin());
    //     features_[kWeightedAxisRatio] = weightedFeatures.eccentricity;
}

void HypothesisFeatures::calculateShapeFeatures(const AreaExtent& extent, const VoronoiSkeletonGrid& grid)
{
    auto shapeFeatures = math::shape_features(extent.begin(), extent.end(), extent.area(), extent.center().toPoint());
    features_[kShapeEccentricity] = shapeFeatures[math::kShapeEccentricity];
    if (extent.perimeter() > 0.0) {
        features_[kShapeCircularity] = (4.0 * M_PI * extent.area()) / std::pow(extent.perimeter(), 2);
        //         features_[kShapeWaviness] = extent.hullPerimeter(grid) / extent.perimeter();
    } else {
        features_[kShapeCircularity] = 1.0;
        //         features_[kShapeWaviness] = 1.0;
    }

    //     features_[kShapeCompactness] = shapeFeatures[math::kShapeCompactness];
    //
    //     extent_compactness_t compactness = extent.compactness();
    //     features_[kShapeCircularity] = compactness.circularity;
    //     features_[kDiscretizedCompactness] = compactness.ndc;
    //     features_[kAminCompactness] = compactness.aMin;
    //     features_[kAmaxCompactness] = compactness.aMax;
    //     features_[kPminCompactness] = compactness.pMin;
    //     features_[kPmaxCompactness] = compactness.pMax;

    //     features_[kShapeArea] = extent.area();
    //     features_[kShapePerimeter] = extent.perimeter();
}

void HypothesisFeatures::calculateIsovistFeatures(const AreaHypothesis& hypothesis,
                                                  const VoronoiIsovistField& isovistField)
{
    using namespace boost::accumulators;
    using IsovistAcc = accumulator_set<double, stats<tag::mean, tag::max, tag::sum, tag::variance, tag::median>>;

    IsovistAcc eccentricityAcc;
    IsovistAcc circularityAcc;
    IsovistAcc compactnessAcc;
    IsovistAcc rayCompactnessAcc;
    IsovistAcc shapeDistCompactnessAcc;
    IsovistAcc wavinessAcc;
    IsovistAcc avgRelationAcc;
    IsovistAcc stdRelationAcc;
    IsovistAcc avgDeltaAcc;
    IsovistAcc stdDeltaAcc;
    IsovistAcc minDistAcc;
    IsovistAcc avgDistAcc;
    IsovistAcc stdDistAcc;
    IsovistAcc avgDistFromCenterAcc;
    IsovistAcc stdDistFromCenterAcc;
    IsovistAcc areaAcc;
    IsovistAcc perimAcc;
    IsovistAcc areaRatioAcc;
    IsovistAcc perimRatioAcc;
    IsovistAcc minDistAngleAcc;
    IsovistAcc areaBalanceAcc;

    double area = hypothesis.extent().area();
    double perimeter = hypothesis.extent().perimeter();

    //     int numIsovistMoments = (isovistField[0].numScalars() - utils::Isovist::kNumScalars);
    int numIsovistFeatures = kNumSingle;   // + numIsovistMoments;
    arma::colvec singleFeatures(numIsovistFeatures);
    isovistFeatures_.resize(numIsovistFeatures, std::distance(hypothesis.beginSkeleton(), hypothesis.endSkeleton()));
    int isovistCol = 0;

    for (auto& cell : boost::make_iterator_range(hypothesis.beginSkeleton(), hypothesis.endSkeleton())) {
        // Sum the variances to get the std for the full isovist field in the area
        const auto& isovist = isovistField[cell];
        eccentricityAcc(isovist.scalar(utils::Isovist::kShapeEccentricity));
        circularityAcc(isovist.scalar(utils::Isovist::kCircularity));
        wavinessAcc(isovist.scalar(utils::Isovist::kShapeWaviness));
        compactnessAcc(isovist.scalar(utils::Isovist::kShapeCompactness));
        rayCompactnessAcc(isovist.scalar(utils::Isovist::kRayCompactness));
        shapeDistCompactnessAcc(isovist.scalar(utils::Isovist::kShapeDistCompactness));
        avgRelationAcc(isovist.scalar(utils::Isovist::kDistRelationAvg));
        stdRelationAcc(std::pow(isovist.scalar(utils::Isovist::kDistRelationStd), 2.0));
        avgDeltaAcc(isovist.scalar(utils::Isovist::kDeltaAvg));
        stdDeltaAcc(std::pow(isovist.scalar(utils::Isovist::kDeltaStd), 2.0));
        minDistAcc(isovist.scalar(utils::Isovist::kDmin));
        avgDistAcc(isovist.scalar(utils::Isovist::kDavg));
        stdDistAcc(std::pow(isovist.scalar(utils::Isovist::kDstd), 2.0));
        areaAcc(isovist.scalar(utils::Isovist::kArea));
        perimAcc(isovist.scalar(utils::Isovist::kPerimeter));
        areaRatioAcc(isovist.scalar(utils::Isovist::kArea) / area);
        perimRatioAcc(isovist.scalar(utils::Isovist::kPerimeter) / perimeter);
        avgDistFromCenterAcc(isovist.scalar(utils::Isovist::kShapeDistAvg));
        stdDistFromCenterAcc(std::pow(isovist.scalar(utils::Isovist::kShapeDistStd), 2.0));
        minDistAngleAcc(isovist.scalar(utils::Isovist::kAngleBetweenMinDists));
        areaBalanceAcc(isovist.scalar(utils::Isovist::kAreaBalance));

        singleFeatures[kSingleEccentricity] = isovist.scalar(utils::Isovist::kShapeEccentricity);
        singleFeatures[kSingleCircularity] = isovist.scalar(utils::Isovist::kCircularity);
        singleFeatures[kSingleWaviness] = isovist.scalar(utils::Isovist::kShapeWaviness);
        singleFeatures[kSingleCompactness] = isovist.scalar(utils::Isovist::kShapeCompactness);
        singleFeatures[kSingleRayCompactness] = isovist.scalar(utils::Isovist::kRayCompactness);
        singleFeatures[kSingleShapeDistCompactness] = isovist.scalar(utils::Isovist::kShapeDistCompactness);
        //         singleFeatures[kSingleDistMin] = isovist.scalar(utils::Isovist::kDmin);
        singleFeatures[kSingleDistStd] = isovist.scalar(utils::Isovist::kDstd);
        singleFeatures[kSingleDistMean] = isovist.scalar(utils::Isovist::kDavg);
        //         singleFeatures[kSingleDVariation] = isovist.scalar(utils::Isovist::kDVariation);
        singleFeatures[kSingleDistFromCenterMean] = isovist.scalar(utils::Isovist::kShapeDistAvg);
        singleFeatures[kSingleDistFromCenterStd] = isovist.scalar(utils::Isovist::kShapeDistStd);
        //         singleFeatures[kSingleDistFromCenterVariation] = isovist.scalar(utils::Isovist::kShapeDistVariation);
        //         singleFeatures[kSingleDeltaVariation] = isovist.scalar(utils::Isovist::kDeltaVariation);
        singleFeatures[kSingleAvgRelation] = isovist.scalar(utils::Isovist::kDistRelationAvg);
        singleFeatures[kSingleStdRelation] = isovist.scalar(utils::Isovist::kDistRelationStd);
        singleFeatures[kSingleAvgDelta] = isovist.scalar(utils::Isovist::kDeltaAvg);
        singleFeatures[kSingleStdDelta] = isovist.scalar(utils::Isovist::kDeltaStd);
        //         singleFeatures[kSingleDistSkewness] = isovist.scalar(utils::Isovist::kDskewness);
        singleFeatures[kSingleArea] = isovist.scalar(utils::Isovist::kArea);
        singleFeatures[kSinglePerimeter] = isovist.scalar(utils::Isovist::kPerimeter);
        singleFeatures[kSingleAngleBetweenMinDists] = isovist.scalar(utils::Isovist::kAngleBetweenMinDists);
        //         singleFeatures[kSingleMinHalfArea] = isovist.scalar(utils::Isovist::kMinHalfArea);
        //         singleFeatures[kSingleAreaBalance] = isovist.scalar(utils::Isovist::kAreaBalance);

        //         for(int n = 0; n < numIsovistMoments; ++n)
        //         {
        //             singleFeatures[kNumSingle + n] =
        //             isovist.scalar(static_cast<utils::Isovist::Scalar>(utils::Isovist::kNumScalars + n));
        //         }

        singleFeatures[kSingleArea] = isovist.scalar(utils::Isovist::kArea);
        singleFeatures[kSinglePerimeter] = isovist.scalar(utils::Isovist::kPerimeter);
        isovistFeatures_.col(isovistCol++) = singleFeatures;
    }

    auto numCells = std::distance(hypothesis.beginSkeleton(), hypothesis.endSkeleton());
    assert(numCells > 1);

    //     features_[kIsovistMeanArea] = mean(areaAcc);
    //     features_[kIsovistStdArea] = std::sqrt(variance(areaAcc));
    features_[kIsovistVariationArea] = (mean(areaAcc) > 0.0) ? std::sqrt(variance(areaAcc)) / mean(areaAcc) : 1e7;
    //     features_[kMeanAreaComp] = (max(areaAcc) > 0.0) ? mean(areaAcc) / max(areaAcc) : 1e7;
    //     features_[kStdAreaComp] = std::sqrt(variance(areaRatioAcc));

    //     features_[kIsovistMeanPerimeter] = mean(perimAcc);
    //     features_[kIsovistStdPerimeter] = std::sqrt(variance(perimAcc));
    features_[kIsovistVariationPerimeter] =
      (mean(perimAcc) > 0.0) ? std::sqrt(variance(perimAcc)) / mean(perimAcc) : 1e7;
    //     features_[kMeanPerimeterComp] = (max(perimAcc) > 0.0) ? mean(perimAcc) / max(perimAcc) : 1e7;
    //     features_[kStdPerimeterComp] = std::sqrt(variance(perimRatioAcc));

    features_[kIsovistMeanEccentricity] = mean(eccentricityAcc);
    features_[kIsovistStdEccentricity] = std::sqrt(variance(eccentricityAcc));

    features_[kIsovistMeanCircularity] = mean(circularityAcc);
    features_[kIsovistStdCircularity] = std::sqrt(variance(circularityAcc));

    features_[kIsovistMeanWaviness] = mean(wavinessAcc);
    features_[kIsovistStdWaviness] = std::sqrt(variance(wavinessAcc));

    features_[kIsovistMeanCompactness] = mean(compactnessAcc);
    features_[kIsovistStdCompactness] = std::sqrt(variance(compactnessAcc));

    features_[kIsovistMeanRayCompactness] = mean(rayCompactnessAcc);
    features_[kIsovistStdRayCompactness] = std::sqrt(variance(rayCompactnessAcc));

    features_[kIsovistMeanShapeDistCompactness] = mean(shapeDistCompactnessAcc);
    features_[kIsovistStdShapeDistCompactness] = std::sqrt(variance(shapeDistCompactnessAcc));

    //     features_[kIsovistMeanRelation] = mean(avgRelationAcc);
    //     features_[kIsovistStdRelation] = std::sqrt(variance(avgRelationAcc));
    features_[kIsovistVariationRelation] =
      (mean(avgRelationAcc) > 0.0) ? std::sqrt(sum(stdRelationAcc)) / mean(avgRelationAcc) : 1e7;

    //     features_[kIsovistMeanDelta] = mean(avgDeltaAcc);
    //     features_[kIsovistStdDelta] = std::sqrt(variance(avgDeltaAcc));
    features_[kIsovistVariationDelta] =
      (mean(avgDeltaAcc) > 0.0) ? std::sqrt(sum(stdDeltaAcc)) / mean(avgDeltaAcc) : 1e7;

    //     features_[kIsovistDistMinMean] = mean(minDistAcc);
    //     features_[kIsovistDistMinStd] = std::sqrt(variance(minDistAcc));
    //     features_[kIsovistDistMinVariation] = (mean(minDistAcc) > 0.0) ?
    //         std::sqrt(variance(minDistAcc)) / mean(minDistAcc) : 1e7;

    //     features_[kIsovistDistMean] = mean(avgDistAcc);
    //     features_[kIsovistDistStd] = std::sqrt(sum(stdDistAcc));
    features_[kIsovistDistVariation] = (mean(avgDistAcc) > 0.0) ? std::sqrt(sum(stdDistAcc)) / mean(avgDistAcc) : 1e7;

    //     features_[kIsovistDistFromCenterMean] = mean(avgDistFromCenterAcc);
    //     features_[kIsovistDistFromCenterStd] = std::sqrt(sum(stdDistFromCenterAcc));
    features_[kIsovistDistFromCenterVariation] =
      (mean(avgDistFromCenterAcc) > 0.0) ? std::sqrt(sum(stdDistFromCenterAcc)) / mean(avgDistFromCenterAcc) : 1e7;

    features_[kIsovistAngleBetweenMinDistsMean] = mean(minDistAngleAcc);
    features_[kIsovistAngleBetweenMinDistsStd] = std::sqrt(variance(minDistAngleAcc));

    //     features_[kIsovistAreaBalanceMean] = mean(areaBalanceAcc);
    //     features_[kIsovistAreaBalanceStd] = std::sqrt(variance(areaBalanceAcc));
}

void HypothesisFeatures::calculateZernikeFeature(const AreaExtent& extent)
{
    if (extent.area() > 0.0) {
        auto boundary = extent.rectangleBoundary(math::ReferenceFrame::GLOBAL);
        auto zernikeFeatures = zernike.moments<double>(extent.begin(),
                                                       extent.end(),
                                                       extent.center().toPoint(),
                                                       std::max(boundary.width(), boundary.height()));
        std::copy(zernikeFeatures.begin(), zernikeFeatures.end(), features_.begin() + kNumFixedFeatures);
    } else {
        std::fill(features_.begin() + kNumFixedFeatures, features_.end(), 0.0);
    }
}

void HypothesisFeatures::calculateVisGraphFeatures(const AreaHypothesis& hypothesis,
                                                   const utils::VisibilityGraph& visGraph)
{
    auto clusterCoeffFeature = visGraph.calculateFeature(utils::VisibilityGraphFeatureType::clustering_coeff);
    auto clusterCoeffStats = clusterCoeffFeature.stats(hypothesis.beginVisVertices(), hypothesis.endVisVertices());
    features_[kVisGraphMeanClusterCoeff] = clusterCoeffStats.mean;
    features_[kVisGraphStdClusterCoeff] = clusterCoeffStats.std;

    auto betweennessFeature = visGraph.calculateFeature(utils::VisibilityGraphFeatureType::betweenness_centrality);
    auto betweennessStats = betweennessFeature.stats(hypothesis.beginVisVertices(), hypothesis.endVisVertices());
    features_[kVisGraphMeanBetweenness] = betweennessStats.mean;
    features_[kVisGraphStdBetweenness] = betweennessStats.std;

    auto pagerankFeature = visGraph.calculateFeature(utils::VisibilityGraphFeatureType::pagerank);
    auto pagerankStats = pagerankFeature.stats(hypothesis.beginVisVertices(), hypothesis.endVisVertices());
    features_[kVisGraphMeanPageRank] = pagerankStats.mean;
    features_[kVisGraphStdPageRank] = pagerankStats.std;
}

void HypothesisFeatures::calculateSkeletonGraphFeatures(const AreaHypothesis& hypothesis,
                                                        const utils::VisibilityGraph& skeletonGraph)
{
    auto betweennessFeature = skeletonGraph.calculateFeature(utils::VisibilityGraphFeatureType::betweenness_centrality);
    auto betweennessStats = betweennessFeature.stats(hypothesis.beginGraphVertices(), hypothesis.endGraphVertices());
    features_[kSkeletonMeanBetweenness] = betweennessStats.mean;
    features_[kSkeletonStdBetweenness] = betweennessStats.std;
    features_[kSkeletonMaxBetweenness] = betweennessStats.max;
}

HypothesisFeatures* find_cached_features(const AreaHypothesis& hyp)
{
    auto cacheIt = kFeaturesCache.find(hyp.numBoundaries());

    if (cacheIt == kFeaturesCache.end()) {
        return nullptr;
    }

    for (auto& cached : cacheIt->second) {
        bool matchesCache = true;
        for (auto& boundary : boost::make_iterator_range(hyp.beginBoundary(), hyp.endBoundary())) {
            if (std::find(cached.boundaryCells.begin(),
                          cached.boundaryCells.end(),
                          boundary->getGateway().skeletonCell())
                == cached.boundaryCells.end()) {
                matchesCache = false;
                break;
            }
        }

        if (matchesCache) {
            return &(cached.features);
        }
    }

    return nullptr;
}

void add_features_to_cache(const AreaHypothesis& hyp, const HypothesisFeatures& features)
{
    CachedFeatures cached;
    cached.features = features;
    cached.boundaryCells.reserve(hyp.numBoundaries());

    for (auto& boundary : boost::make_iterator_range(hyp.beginBoundary(), hyp.endBoundary())) {
        cached.boundaryCells.push_back(boundary->getGateway().skeletonCell());
    }

    kFeaturesCache[cached.boundaryCells.size()].push_back(std::move(cached));
}

}   // namespace hssh
}   // namespace vulcan


#pragma once
#include <boost/range/adaptor/reversed.hpp>

#include "applications/heat_equation.hpp"
#include "datastructures/double_tree_view.hpp"
#include "spacetime/linear_form.hpp"

namespace applications {
namespace ErrorEstimator {
using datastructures::DoubleTreeVector;
using space::HierarchicalBasisFn;
using spacetime::LinearFormBase;
using Time::ThreePointWaveletFn;

struct GlobalError {
  double error;         // e_\delta
  double error_Yprime;  // \eqsim ||g - Bu||_{Y'}
  double error_t0;      // ||\gamma_0u - u0||_L2
};

GlobalError ComputeGlobalError(const Eigen::VectorXd &g_min_Bu,
                               const Eigen::VectorXd &PY_g_min_Bu,
                               const Eigen::VectorXd &G_u_dd_dd,
                               const Eigen::VectorXd &u0, HeatEquation &heat,
                               const Eigen::VectorXd &u_dd_dd,
                               LinearFormBase<ThreePointWaveletFn> &u0_lf);

// Computes \|u_t - \gamma_t u_delta\|_{L_2(\Omega)} using interpolation on u_t.
double ComputeTraceError(
    double t, std::function<double(double, double)> u_t,
    DoubleTreeVector<ThreePointWaveletFn, HierarchicalBasisFn> *u_delta);

double ComputeLocalErrors(
    DoubleTreeVector<ThreePointWaveletFn, HierarchicalBasisFn> *residual_dd_dd,
    bool mean_zero = true);
}  // namespace ErrorEstimator
}  // namespace applications


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/fast.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( sqrt_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      return if_else_zero(a0, a0 * fast_(rsqrt)(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( sqrt_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( bs::assert_all(is_positive(a0))
                      , "sqrt integer domain is restricted to positive integer."
                      );
      return sqrt(a0);
    }
  };
} } }

#endif


#ifndef SURFACEBASEDREGISTRATION_INCLUDED
#define SURFACEBASEDREGISTRATION_INCLUDED

#include <Eigen/Dense>

Eigen::ArrayXi find_closest_points(const Eigen::MatrixXd& surface1, const Eigen::MatrixXd& surface2);

Eigen::MatrixXd reorder_points(const Eigen::MatrixXd& surface, const Eigen::ArrayXi& lookup_table);

Eigen::Matrix4d register_surfaces(const Eigen::MatrixXd& surface1, const Eigen::MatrixXd& surface2, const Eigen::Matrix4d& transform_init);

Eigen::Matrix4d register_surfaces(const Eigen::MatrixXd& surface1, const Eigen::MatrixXd& surface2);
#endif


// #include <boost/geometry.hpp>
// #include <boost/geometry/geometries/linestring.hpp>
// #include <boost/geometry/geometries/polygon.hpp>
// #include <boost/geometry/geometries/point_xy.hpp>
// #include <boost/assign/list_of.hpp>
// namespace bg = boost::geometry;
// namespace trans = bg::strategy::transform;
// typedef bg::model::d2::point_xy<double> point;
// typedef boost::geometry::model::segment<point> line;
// typedef bg::model::multi_point<point> mpoint;
// // namespace bg = boost::geometry;
// // namespace trans = bg::strategy::transform;
// // typedef bg::model::d2::point_xy<double> point;
// // typedef boost::geometry::model::segment<point> segment;
// // typedef boost::geometry::model::linestring<point> line;
// // typedef bg::model::polygon<point> polygon;
// // typedef bg::model::multi_point<point> mpoint;
// bool intersects(point a,point b,point c,point d){
//     return bg::intersects(line(a,b),line(c,d));
// }


#include <gtest/gtest.h>
#include <stan/math.hpp>
#include <stan/math/torsten/pmx_csda.hpp>
#include <test/unit/util.hpp>
#include <boost/range/numeric.hpp>
#include <iostream>
#include <limits>
#include <sstream>
#include <vector>

struct ComplexStepDerivativeScalTest : public ::testing::Test {
  struct Fexp {
    template <typename T>
    inline T operator()(const T &theta, const std::vector<double> &x_r,
                        const std::vector<int> &x_i, std::ostream *msgs) const {
      return exp(theta) / sqrt(theta) - 0.5 * exp(theta) * pow(theta, -1.5);
    }
  };

  struct Fv {
    template <typename T>
    inline std::vector<T> operator()(const std::vector<T> &y,
                                     const std::vector<double> &x_r,
                                     const std::vector<int> &x_i,
                                     std::ostream *msgs) const {
      T x = boost::inner_product(y, y, y[0]);
      return std::vector<T>(y.size(), x);
    }
  };

  void SetUp() {}
  Fexp f;
  Fv fv;
  const std::vector<double> x_r;
  const std::vector<int> x_i;
  std::ostream *msgs = nullptr;
};

TEST_F(ComplexStepDerivativeScalTest, func_exp_sqrt) {
  using torsten::pk_csda;
  using stan::math::var;

  /* f near x = 0 has very large derivative */
  var x = 0.01;
  var y = pk_csda(f, x, x_r, x_i, 1.e-20, msgs);

  ASSERT_FLOAT_EQ(y.val(), f(x.val(), x_r, x_i, msgs));

  std::vector<stan::math::var> xv{x};
  std::vector<double> g1, g;
  var y1 = f(x, x_r, x_i, msgs);
  stan::math::set_zero_all_adjoints();
  y1.grad(xv, g1);
  stan::math::set_zero_all_adjoints();
  y.grad(xv, g);
  ASSERT_FLOAT_EQ(g[0], g1[0]);
}

TEST_F(ComplexStepDerivativeScalTest, func_exp_sqrt_default_h) {
  using torsten::pk_csda;
  using stan::math::var;

  /* f near x = 0 has very large derivative */
  var x = 0.01;
  var y = pk_csda(f, x, x_r, x_i, msgs);

  ASSERT_FLOAT_EQ(y.val(), f(x.val(), x_r, x_i, msgs));

  std::vector<stan::math::var> xv{x};
  std::vector<double> g1, g;
  var y1 = f(x, x_r, x_i, msgs);
  stan::math::set_zero_all_adjoints();
  y1.grad(xv, g1);
  stan::math::set_zero_all_adjoints();
  y.grad(xv, g);
  ASSERT_FLOAT_EQ(g[0], g1[0]);
}

TEST_F(ComplexStepDerivativeScalTest, directional_derivative) {
  using torsten::pk_csda;
  using namespace std::placeholders;
  const double h = 1.e-8;
  std::vector<double> y{1.0, 0.1, 2.0};
  std::vector<double> dy{0.0, -1.0, -1.0};
  std::vector<double> g = pk_csda(fv, y, dy, x_r, x_i, msgs);
  auto fvv = std::bind(fv, _1, x_r, x_i, msgs);
  std::vector<double> g1 = pk_csda(fvv, y, dy);
  std::vector<double> f1 = fv(y, x_r, x_i, msgs);
  std::transform(y.begin(), y.end(), dy.begin(), y.begin(),
                 [&h](const double &x1, const double &x2) -> double {
                    return x1 + h * x2; });
  std::vector<double> f2 = fv(y, x_r, x_i, msgs);
  for (size_t i = 0 ; i < y.size(); ++i) {
    EXPECT_FLOAT_EQ(g[i], (f2[i] - f1[i]) / h);
    EXPECT_FLOAT_EQ(g1[i], g[i]);
  }

}


#include <algorithm>
#include <boost/bind.hpp>
#include <chrono>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <gazebo/gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <iostream>
#include <vector>

#define L 0.04 // distance between body center and wheel center
#define r 0.01905 // wheel radius

#define pi30 0.1047197551196597705355242034774843062905347323976457118988037109375 // long double thirty = 30; long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne) / thirty);
#define pi12 1.5707963267948965579989817342720925807952880859375 // long double two = 2; long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne) / two);
#define pi 3.141592653589793115997963468544185161590576171875 // long double mOne = -1; printf("%1.70Lf\n", (long double) acos(mOne));
#define pi2 6.28318530717958623199592693708837032318115234375 // long double two = 2; long double mOne = -1; printf("%1.70Lf\n", (long double) two * acos(mOne));
#define sqrt3 1.732050807568877193176604123436845839023590087890625 // long double three = 3; printf("%1.70Lf\n", (long double) sqrt(three));
#define sqrt32 0.8660254037844385965883020617184229195117950439453125 // long double two = 2; long double three = 3; printf("%1.70Lf\n", (sqrt(three) / two));
#define Vmax 1.0 // set by experiments (m/s)
#define omegamax 50.0 // set by experiments (rad/s)
#define P 5.0 // PID
#define I 0.0005 // PID
#define stopDistance 0.001
#define stopAngle pi30

double Vx;
double Vy;
double Vxm;
double Vym;
double Vxw;
double Vyw;
double VxAbs;
double VyAbs;
double VxmTarget;
double VymTarget;
double VxwTarget;
double VywTarget;
double omegap;
double Vback;
double Vleft;
double Vright;
double VbackTarget;
double VleftTarget;
double VrightTarget;
double omegapL;
double omegapLVxm2;
double sqrtVym2;
double Vxm2;
double Vback3;
double Vleft3;
double Vright3;
double VxTarget;
double VyTarget;
double omegapTarget;
double x = 0;
double y = 0;
double xm = 0;
double ym = 0;
double xOffset = 0;
double yOffset = 0;
double xTarget = 0;
double yTarget = 0;
double xw = 0;
double yw = 0;
double xError;
double yError;
double xErrorI;
double yErrorI;
double theta = 0;
double thetaError;
double thetaErrorI;
double thetaOffset = 0;
double thetaTarget = 0;
double timeElapsed = 0;
double timeLast;
double timeNow = 0;
double backAngleElapsed;
double backAngleLast;
double backAngleNow = 0;
double leftAngleElapsed;
double leftAngleLast;
double leftAngleNow = 0;
double rightAngleElapsed;
double rightAngleLast;
double rightAngleNow = 0;
double phi;
double Vlow;
double Vhigh;
double scale;

class Point {

    public: double x;

    public: double y;

    private: static double w;

    public: void offset(double x, double y) {
        this->x += x;
        this->y += y;
    }

    public: void rotate(double theta) {
              w = (std::cos(theta) * this->y) + (std::sin(theta) * this->x);
        this->x = (std::cos(theta) * this->x) - (std::sin(theta) * this->y);
        this->y = w;
    }

    public: Point() {}

    public: Point(const Point& p) {
        x = p.x;
        y = p.y;
    }

    public: Point(double x, double y) {
        this->x = x;
        this->y = y;
    }

    public: Point(Point p, double xOffset, double yOffset) {
        x = p.x + xOffset;
        y = p.y + yOffset;
    }
};

std::vector<Point> points;
std::vector<double> angles;

enum Movements {
    MOVEMENT_DIRECT,
    MOVEMENT_DIRECT_M,
    MOVEMENT_DIRECT_W,
    MOVEMENT_NONE,
    MOVEMENT_ABSOLUTE_M,
    MOVEMENT_ABSOLUTE_W,
    MOVEMENT_BEZIER_W
};
Movements movement = MOVEMENT_NONE;

long long binomialCoefficient_c;
long long binomialCoefficient_i;

/**
 * source:
 * https://en.wikipedia.org/wiki/Binomial_coefficient
 * @param n the number that goes above
 * @param k the number that goes below
 * @return
 */
long long binomialCoefficient(long long n, long long k) {
    if ((k < 0) || (k > n))
        return 0;
    if ((k == 0) || (k == n))
        return 1;
    k = std::min(k, n - k);// take advantage of symmetry
    binomialCoefficient_c = 1;
    for (long binomialCoefficient_i = 0;
     binomialCoefficient_i < k; binomialCoefficient_i++)
        binomialCoefficient_c = binomialCoefficient_c
         * (n - binomialCoefficient_i) / (binomialCoefficient_i + 1);
    return binomialCoefficient_c;
}

double Bezier_t;
double Bezier_step;
long long Bezier_nT;
long long Bezier_nR;
long long Bezier_i;
double Bezier_B;
double Bezier_pX;
double Bezier_pY;
double Bezier_pTheta;

double Bezier_B_(double Bezier_n) {
    return ((double) binomialCoefficient(Bezier_n, Bezier_i))
         * std::pow((double) Bezier_t, (double) Bezier_i)
         * std::pow(1.0 - Bezier_t, (double) (Bezier_n - Bezier_i));
}

void Bezier() {
    Bezier_pX = 0;
    Bezier_pY = 0;
    for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
        Bezier_B = Bezier_B_(Bezier_nT);
        Bezier_pX += points[Bezier_i].x * Bezier_B;
        Bezier_pY += points[Bezier_i].y * Bezier_B;
    }
    Bezier_pTheta = 0;
    for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
        Bezier_pTheta += angles[Bezier_i] * Bezier_B_(Bezier_nR);
    }
}

/**
 * Resets the error values used by the PID controller.
 *
 * Should be used after a movement command if another one is fired
 * before or immediately after the previous one
 * and a continuous movement is not desired.
 */
void resetErrors() {
    xErrorI = 0;
    yErrorI = 0;
    thetaErrorI = 0;
}

/**
 * Calculates the mobile platform's velocities
 * relative to the mobile platform's frame
 * from the wheels velocities.
 */
void forwardKinematicsMobile() {
    Vleft3 = Vleft / 3.0;
    Vback3 = Vback / 3.0;
    Vright3 = Vright / 3.0;
    Vxm = (2 * Vback3) - Vleft3 - Vright3;
    Vym = (sqrt3 * Vright3) - (sqrt3 * Vleft3);
    omegap = (Vleft3 + Vback3 + Vright3) / L;
}

/**
 * Calculates the mobile platform's velocities
 * relative to the world's frame
 * from the wheels velocities.
 *
 * Also runs forwardKinematicsMobile().
 */
void forwardKinematicsWorld() {
    forwardKinematicsMobile();
    Vxw = (std::cos(theta) * Vxm) - (std::sin(theta) * Vym);
    Vyw = (std::cos(theta) * Vym) + (std::sin(theta) * Vxm);
}

/**
 * Generates a pseudo random number between 0 and maximumValue, inclusive.
 * @param maximumValue
 * @return
 */
double getRandom(double maximumValue) {
    return (((double) std::rand()) / ((double) RAND_MAX)) * maximumValue;
}

/**
 * Calculates the wheels velocities
 * from the mobile platform's velocities
 * relative to the mobile platform's frame.
 */
void inverseKinematicsMobile() {
    omegapL = omegap * L;
    sqrtVym2 = sqrt32 * Vym;
    Vxm2 = Vxm / 2.0;
    omegapLVxm2 = omegapL - Vxm2;
    Vleft  = omegapLVxm2 - sqrtVym2;
    Vback  = omegapL + Vxm;
    Vright = omegapLVxm2 + sqrtVym2;
}

/**
 * Calculates the wheels velocities
 * from the mobile platform's velocities
 * relative to the mobile platform's frame.
 *
 * Also runs inverseKinematicsMobile().
 */
void inverseKinematicsWorld() {
    Vxm = (std::cos(theta) * Vxw) + (std::sin(theta) * Vyw);
    Vym = (std::cos(theta) * Vyw) - (std::sin(theta) * Vxw);
    inverseKinematicsMobile();
}

bool isStopPose() {
    return (std::abs(xError) < stopDistance)
     && (std::abs(yError) < stopDistance)
     && (std::abs(thetaError) < stopAngle);
}

double normalizeRadian(double radian) {
    radian = fmod(radian, pi2);
    if (radian < 0) radian += pi2;
    return radian;
}

namespace gazebo {

    class OmniPlatformPlugin : public ModelPlugin {

        private: physics::JointPtr backJoint;

        private: physics::JointPtr leftJoint;

        // Pointer to the model
        private: physics::ModelPtr model;

        private: physics::JointPtr rightJoint;

        // Pointer to the update event connection
        private: event::ConnectionPtr updateConnection;

        private: physics::WorldPtr world;

        /**
         * Movement for desired pose in mobile platform's frame.
         *
         * Translates the desired pose in the mobile platform's frame
         * to the world's frame and moves according to the world's frame.
         * Keep in mind that how the mobile platform reaches the target theta
         * will affect the platform's position in it's own frame,
         * and that position can't be predicted. Also remember
         * that you can change the value of the platform's pose in either frame.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteM(double x, double y, double theta) {
            // distance to be traversed in the mobile platform's frame
            xTarget = x - xm;
            yTarget = y - ym;
            // distance rotated to the direction
            // to be traversed in the world's frame
            x = (std::cos(theta) * xTarget) - (std::sin(theta) * yTarget);
            y = (std::cos(theta) * yTarget) + (std::sin(theta) * xTarget);
            // rotated distance added to current world's frame position
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Random movement for desired pose in mobile platform's frame
         *
         * At the end of the movement, the mobile platform's x, y and theta
         * will be the same as the parameters. However, the position
         * in the world frame will depend on how theta will vary
         * from it's current value to the target value, and can't be predicted.
         * It's preferred to use fireMovementPoseM(),
         * unless you know what you're doing.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteMRaw(double x, double y, double theta) {
            xTarget = x;
            yTarget = y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_M;
            updateIndicator();
        }

        /**
         * Movement for desired pose in world frame.
         *
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementAbsoluteW(double x, double y, double theta) {
            xTarget = x;
            yTarget = y;
            thetaTarget = theta;
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Incremental movement based on two Bézier curves
         * relative to the platform's frame.
         *
         * The curve may be translated such that the first control point
         * and angle coincides with the current platform's position and angle.
         *
         * The Bézier function's t variable, by default, iterates
         * between 0 and 1 to generate the curve. t's step value
         * is defined by the user, and influences the time the platform
         * will take to execute the movement, which is equal to the step value
         * divided by the iteration time. If the movement time is too short
         * (which depends on the movement length and complexity), the platform
         * will make it's best effort to execute the movement. Therefore,
         * the longer the movement time (the shorter the step value), the closer
         * the platform will be to the desired movement.
         *
         * The platform's angle is also controlled by a Bézier curve. However,
         * the algorithm has been modified to be one dimensional, which means
         * its input are control values instead of control points.
         * If a constant angle is desired, input a vector with a single value
         * equal to the angle.
         *
         * @param step Bézier funcion's t variable's increment value.
         * @param points control points for the translation movement
         * @param angles control angles for the rotation movement
         * @param offsetT whether to offset the control points to the starting platform's position
         * @param offsetR whether to offset the control angles to the starting platform's angle
         */
        public: void fireMovementBezierM(std::vector<Point> * points,
                std::vector<double> * angles, double step,
                bool offsetT, bool offsetR) {
            if (points->empty() || angles->empty()) return;
            ::points.clear();
            ::angles.clear();
            Bezier_nT = points->size() - 1;
            Bezier_nR = angles->size() - 1;
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points.push_back(Point(points->at(Bezier_i)));
                ::points[Bezier_i].offset(-xw, -yw);
                ::points[Bezier_i].rotate(theta);
                ::points[Bezier_i].offset(xw, yw);
            }
            if (offsetT) {
                xOffset = xw - ::points[0].x;
                yOffset = yw - ::points[0].y;
            } else {
                xOffset = 0;
                yOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points[Bezier_i].offset(xOffset, yOffset);
            }
            if (offsetR) {
                thetaOffset = pi - normalizeRadian((*angles)[0] + pi - theta);
            } else {
                thetaOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
                ::angles.push_back(angles->at(Bezier_i) + thetaOffset);
            }
            Bezier_t = 0;
            Bezier_step = step;
            movement = MOVEMENT_BEZIER_W;
            updateIndicator();
        }

        /**
         * Incremental movement based on two Bézier curves relative to the world's frame.
         *
         * The curve may be translated such that the first control point
         * and angle coincides with the current platform's position and angle.
         *
         * The Bézier function's t variable, by default, iterates
         * between 0 and 1 to generate the curve. t's step value
         * is defined by the user, and influences the time the platform
         * will take to execute the movement, which is equal to the step value
         * divided by the iteration time. If the movement time is too short
         * (which depends on the movement length and complexity), the platform
         * will make it's best effort to execute the movement. Therefore,
         * the longer the movement time (the shorter the step value), the closer
         * the platform will be to the desired movement.
         *
         * The platform's angle is also controlled by a Bézier curve. However,
         * the algorithm has been modified to be one dimensional, which means
         * its input are control values instead of control points.
         * If a constant angle is desired, input a vector with a single value
         * equal to the angle.
         *
         * @param step Bézier funcion's t variable's increment value.
         * @param points control points for the translation movement
         * @param angles control angles for the rotation movement
         * @param offsetT whether to offset the control points to the starting platform's position
         * @param offsetR whether to offset the control angles to the starting platform's angle
         */
        public: void fireMovementBezierW(std::vector<Point> * points,
                std::vector<double> * angles, double step,
                bool offsetT, bool offsetR) {
            if (points->empty() || angles->empty()) return;
            ::points.clear();
            ::angles.clear();
            Bezier_nT = points->size() - 1;
            Bezier_nR = angles->size() - 1;
            if (offsetT) {
                xOffset = xw - (*points)[0].x;
                yOffset = yw - (*points)[0].y;
            } else {
                xOffset = 0;
                yOffset = 0;
            }
            if (offsetR) {
                thetaOffset = pi - normalizeRadian((*angles)[0] + pi - theta);
            } else {
                thetaOffset = 0;
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nT; Bezier_i++) {
                ::points.push_back(Point(points->at(Bezier_i), xOffset, yOffset));
            }
            for (Bezier_i = 0; Bezier_i <= Bezier_nR; Bezier_i++) {
                ::angles.push_back(angles->at(Bezier_i) + thetaOffset);
            }
            Bezier_t = 0;
            Bezier_step = step;
            movement = MOVEMENT_BEZIER_W;
            updateIndicator();
        }

        /**
         * Moves the platform with fixed translation and rotation speeds
         * relative to the mobile platform's frame but translated
         * to the world's frame. This results in a world's frame movement
         * rotated according to the initial angle between frames.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectHybrid(double Vxm, double Vym, double omegap) {
            VxTarget = (std::cos(theta) * Vxm) - (std::sin(theta) * Vym);
            VyTarget = (std::cos(theta) * Vym) + (std::sin(theta) * Vxm);
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_W;
        }

        /**
         * Moves the platform with fixed translation and rotation speeds,
         * relative to the mobile platform's frame. Does not transform
         * the speeds from the mobile platform's frame to the world frame.
         * This means that random changes in the platform's angle
         * will not change it's movement, as the mobile platform's frame
         * rotates with the platform.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectMobile(double Vxm, double Vym, double omegap) {
            VxTarget = Vxm;
            VyTarget = Vym;
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_M;
        }

        /**
         * Sets the wheels' speeds, in m/s.
         *
         * To be executed by the communications module.
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectWheel(double Vleft, double Vback, double Vright) {
            VleftTarget  = Vleft ;
            VbackTarget  = Vback ;
            VrightTarget = Vright;
            movement = MOVEMENT_DIRECT;
        }

        /**
         * Moves the platform with fixed translation and rotation speeds,
         * relative to the world's frame.
         *
         * To be executed by the communications module.
         *
         * @param Vleft
         * @param Vback
         * @param Vright
         */
        public: void fireMovementDirectWorld(double Vxw, double Vyw, double omegap) {
            VxTarget     = Vxw   ;
            VyTarget     = Vyw   ;
            omegapTarget = omegap;
            movement = MOVEMENT_DIRECT_W;
        }

        /**
         * Movement of a given distance and angle of rotation
         * in the mobile platform's frame
         *
         * Translates the movement in the mobile platform's frame
         * to the world's frame and moves according to the world's frame.
         * Keep in mind that how the mobile platform reaches the target theta
         * will affect the platform's position in it's own frame,
         * and that position can't be predicted. Also remember
         * that you can change the value of the platform's pose in either frame.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeM(double x, double y, double theta) {
            // distance to be traversed in the mobile platform's frame
            xTarget = x;
            yTarget = y;
            // distance rotated to the direction
            // to be traversed in the world's frame
            x = (std::cos(theta) * xTarget) - (std::sin(theta) * yTarget);
            y = (std::cos(theta) * yTarget) + (std::sin(theta) * xTarget);
            // rotated distance added to current world's frame position
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        /**
         * Random movement of a given distance and angle of rotation
         * in the mobile platform's frame
         *
         * At the end of the movement, the mobile platform will have traversed
         * in x and y and rotated by theta, relative to its frame.
         * However, the position in the world frame will depend
         * on how theta will vary from it's current value to the target value,
         * and can't be predicted. It's preferred to use fireMovementPoseM(),
         * unless you know what you're doing.
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeMRaw(double x, double y, double theta) {
            xTarget = xm + x;
            yTarget = ym + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_M;
            updateIndicator();
        }

        /**
         * Movement of a given distance and angle of rotation
         * relative to the world frame.
         *
         * To be executed by the communications module.
         *
         * @param x
         * @param y
         * @param theta
         */
        public: void fireMovementRelativeW(double x, double y, double theta) {
            xTarget = xw + x;
            yTarget = yw + y;
            thetaTarget = normalizeRadian(::theta + theta);
            movement = MOVEMENT_ABSOLUTE_W;
            updateIndicator();
        }

        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {
            // Store the pointer to the model
            this->model = _parent;

            this-> leftJoint = model->GetJoint("left_joint");
            this-> backJoint = model->GetJoint("back_joint");
            this->rightJoint = model->GetJoint("right_joint");

            this->world = _parent->GetWorld();
            //this->indicator = this->world->GetModel("wood_cube_2_5cm");
            //this->indicatorPose.reset(new math::Pose());
            //this->indicator->SetGravityMode(false);
            //this->indicator->Fini();

            std::srand(std::time(0));

            // Listen to the update event. This event is broadcast every
            // simulation iteration.
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                    boost::bind(&OmniPlatformPlugin::OnUpdate, this, _1));
        }

        private: void odometry() {
            timeLast = timeNow;
            timeNow = this->world->GetSimTime().Double();
            timeElapsed = timeNow - timeLast;
            Vleft  = this-> leftJoint->GetVelocity(0) * r;
            Vback  = this-> backJoint->GetVelocity(0) * r;
            Vright = this->rightJoint->GetVelocity(0) * r;
            theta = normalizeRadian(model->GetRelativePose().rot.GetYaw()); // simulation theta
            forwardKinematicsWorld();
            xm += Vxm * timeElapsed;
            ym += Vym * timeElapsed;
            xw += Vxw * timeElapsed;
            //xw = model->GetRelativePose().pos.x; // simulation x
            yw += Vyw * timeElapsed;
            //yw = model->GetRelativePose().pos.y; // simulation y
        }

        // Called by the world update start event
        public: void OnUpdate(const common::UpdateInfo & /*_info*/) {
            odometry();
            switch (movement) {
                case MOVEMENT_ABSOLUTE_M:
                    xError = xTarget - xm;
                    yError = yTarget - ym;
                    // method parameter: theta plus shift to place thetaTarget at pi
                    thetaError = pi - normalizeRadian(theta + pi - thetaTarget);
                    if (isStopPose()) {
                        movement = MOVEMENT_NONE;
                        break;
                    }
                    xErrorI += xError;
                    yErrorI += yError;
                    thetaErrorI += thetaError;
                    Vxm = (xError * P) + (xErrorI * I);
                    Vym = (yError * P) + (yErrorI * I);
                    VxAbs = std::abs(Vxm);
                    VyAbs = std::abs(Vym);
                    if ((VxAbs > Vmax) || (VyAbs > Vmax)) {
                        scale = Vmax / ((VxAbs > VyAbs) ? VxAbs : VyAbs);
                        Vxm *= scale;
                        Vym *= scale;
                    }
                    omegap = (thetaError * P) + (thetaErrorI * I);
                    if (omegap > omegamax) {
                        omegap = omegamax;
                    }
                    inverseKinematicsMobile();
                    break;
                case MOVEMENT_BEZIER_W:
                    if (Bezier_t <= 1) {
                        Bezier();
                        xTarget = Bezier_pX;
                        yTarget = Bezier_pY;
                        thetaTarget = normalizeRadian(Bezier_pTheta);
                        Bezier_t += Bezier_step;
                        updateIndicator();
                    } else {
                        movement = MOVEMENT_NONE;
                    }
                    // break; // fall through intended
                case MOVEMENT_ABSOLUTE_W:
                    xError = xTarget - xw;
                    yError = yTarget - yw;
                    // method parameter: theta plus shift to place thetaTarget at pi
                    thetaError = pi - normalizeRadian(theta + pi - thetaTarget);
                    if (isStopPose() && (movement == MOVEMENT_ABSOLUTE_W)) {
                        movement = MOVEMENT_NONE;
                        break;
                    }
                    xErrorI += xError;
                    yErrorI += yError;
                    thetaErrorI += thetaError;
                    Vxw = (xError * P) + (xErrorI * I);
                    Vyw = (yError * P) + (yErrorI * I);
                    VxAbs = std::abs(Vxw);
                    VyAbs = std::abs(Vyw);
                    if ((VxAbs > Vmax) || (VyAbs > Vmax)) {
                        scale = Vmax / ((VxAbs > VyAbs) ? VxAbs : VyAbs);
                        Vxw *= scale;
                        Vyw *= scale;
                    }
                    omegap = (thetaError * P) + (thetaErrorI * I);
                    if (omegap > omegamax) {
                        omegap = omegamax;
                    }
                    inverseKinematicsWorld();
                    break;
                case MOVEMENT_DIRECT:
                    Vleft  = VleftTarget ;
                    Vback  = VbackTarget ;
                    Vright = VrightTarget;
                    break;
                case MOVEMENT_DIRECT_M:
                    Vxm    = VxTarget    ;
                    Vym    = VyTarget    ;
                    omegap = omegapTarget;
                    inverseKinematicsMobile();
                    break;
                case MOVEMENT_DIRECT_W:
                    Vxw    = VxTarget    ;
                    Vyw    = VyTarget    ;
                    omegap = omegapTarget;
                    inverseKinematicsWorld();
                    break;
                default:
                    xErrorI = 0;
                    yErrorI = 0;
                    thetaErrorI = 0;
                    Vleft = 0;
                    Vback = 0;
                    Vright = 0;
            }
             leftJoint->SetVelocity(0, Vleft / r);
             backJoint->SetVelocity(0, Vback / r);
            rightJoint->SetVelocity(0, Vright / r);
        }

        /**
         * Simulation purposes only.
         */
        private: void updateIndicator() {
//            indicatorPose->Set(xTarget, yTarget, 0.05, 0, 0, thetaTarget);
//            indicator->SetRelativePose(*indicatorPose);
        }
    };

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

// C++ bug: when things are declared as 'static' or 'const',
// they lose their references, and it's only possible to read from them
// to restore their references, they must be decladed again like this:
double Point::w;


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2011: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_INFINITY_HPP_JOFA_100322
#define BOOST_ICL_TYPE_TRAITS_INFINITY_HPP_JOFA_100322

#include <string>
#include <boost/static_assert.hpp>
#include <boost/icl/type_traits/is_numeric.hpp>
#include <boost/icl/type_traits/rep_type_of.hpp>
#include <boost/icl/type_traits/size_type_of.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/if.hpp>

namespace boost{ namespace icl
{

template<class Type> struct has_std_infinity
{
    typedef has_std_infinity type;
    BOOST_STATIC_CONSTANT(bool,
        value = (     is_numeric<Type>::value
                   && std::numeric_limits<Type>::has_infinity
                )
       );
};

template<class Type> struct has_max_infinity
{
    typedef has_max_infinity type;
    BOOST_STATIC_CONSTANT(bool,
        value = (     is_numeric<Type>::value
                   && ! std::numeric_limits<Type>::has_infinity
                )
       );
};

//------------------------------------------------------------------------------
template <class Type, bool has_std_inf=false, bool has_std_max=false>
struct get_numeric_infinity;

template <class Type, bool has_std_max>
struct get_numeric_infinity<Type, true, has_std_max>
{
    typedef get_numeric_infinity type;
    static Type value()
    {
        return (std::numeric_limits<Type>::infinity)();
    }
};

template <class Type>
struct get_numeric_infinity<Type, false, true>
{
    typedef get_numeric_infinity type;
    static Type value()
    {
        return (std::numeric_limits<Type>::max)();
    }
};

template <class Type>
struct get_numeric_infinity<Type, false, false>
{
    typedef get_numeric_infinity type;
    static Type value()
    {
        return Type();
    }
};

template <class Type>
struct numeric_infinity
{
    typedef numeric_infinity type;
    static Type value()
    {
        return get_numeric_infinity< Type
                                   , has_std_infinity<Type>::value
                                   , has_max_infinity<Type>::value >::value();
    }
};

//------------------------------------------------------------------------------
template<class Type, bool has_numeric_inf, bool has_repr_inf, bool has_size, bool has_diff>
struct get_infinity;

template<class Type, bool has_repr_inf, bool has_size, bool has_diff>
struct get_infinity<Type, true, has_repr_inf, has_size, has_diff>
{
    typedef get_infinity type;

    static Type value()
    {
        return  numeric_infinity<Type>::value();
    }
};

template<class Type, bool has_size, bool has_diff>
struct get_infinity<Type, false, true, has_size, has_diff>
{
    typedef get_infinity type;

    static Type value()
    {
        return Type(numeric_infinity<typename Type::rep>::value());
    }
};

template<class Type, bool has_diff>
struct get_infinity<Type, false, false, true, has_diff>
{
    typedef get_infinity type;
    typedef typename Type::size_type size_type;

    static Type value()
    {
        return Type(numeric_infinity<size_type>::value());
    }
};

template<class Type>
struct get_infinity<Type, false, false, false, true>
{
    typedef get_infinity type;
    typedef typename Type::difference_type difference_type;

    static Type value()
    {
        return identity_element<difference_type>::value();
    }
};

template<class Type>
struct get_infinity<Type, false, false, false, false>
{
    typedef get_infinity type;

    static Type value()
    {
        return identity_element<Type>::value();
    }
};

template <class Type> struct infinity
{
    typedef infinity type;

    static Type value()
    {
        return
            get_infinity< Type
                        , is_numeric<Type>::value
                        , has_rep_type<Type>::value
                        , has_size_type<Type>::value
                        , has_difference_type<Type>::value
                        >::value();
    }
};

template <>
struct infinity<std::string>
{
    typedef infinity type;

    static std::string value()
    {
        return std::string();
    }
};

}} // namespace boost icl

#endif


#pragma once

#include <Eigen/Dense>

namespace pd::ecs::cmp::tabbedWindow
{
	// ---------------------------------------------------------------------------------------------------------
	class MovementActive
	{
	public:
		Eigen::Vector2i cursorInTabSpacePosition = { 0, 0 };
	};

	// ---------------------------------------------------------------------------------------------------------
	class MovementRequest
	{
	public:
		Eigen::Vector2i newWindowPos;
	};
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/cpp_int.hpp>

#include "test_arithmetic.hpp"

template <>
struct related_type<boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::checked_int256_t::backend_type> > >
{
   typedef boost::multiprecision::checked_int256_t type;
};

template <>
struct is_twos_complement_integer<boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::checked_int256_t::backend_type> > > : public std::false_type
{};

int main()
{
   test<boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::checked_int256_t::backend_type> > >();
   return boost::report_errors();
}


#include "GBPIdescriptor.h"
#include "cykTools.h"
//#include <armadillo>

GBPI::GBPI(){
	init(40);
}

GBPI::GBPI(cv::Mat pic, int ds){
	init(ds);
	loadImg(pic);
}

void GBPI::set_gp(std::string gpname, int ds){
	gpn = gpname;
	set_des(ds);
}

void GBPI::set_des(int ds){
	cyktools cyk;
	arma::mat gp_a = cyk.readMat(gpn.c_str());
	gp = cyk.arma2opencv(gp_a);
	for (int i=0; i<Nl; i++)
	{
		gp.ptr<double>(i)[0] = cvRound(gp.ptr<double>(i)[0] * ds);
		gp.ptr<double>(i)[1] = cvRound(gp.ptr<double>(i)[1] * ds);
	}
}

void GBPI::init(int ds){
    cyktools cyk;
	gpn = "../../common/gp_400.mat";
// 	gpn = "E:/VS_WS/common/gp.mat";
    arma::mat gp_a = cyk.readMat(gpn.c_str());

    gp = cyk.arma2opencv(gp_a);

//	matfile2Mat("E:\\Matlab_WS\\testGBPI\\gp.mat", "gp", gp);
	des_size = ds;
	Nl = gp.rows;
//    std::cout << Nl << std::endl;
	for (int i=0; i<Nl; i++)
	{
		gp.ptr<double>(i)[0] = cvRound(gp.ptr<double>(i)[0] * des_size);
		gp.ptr<double>(i)[1] = cvRound(gp.ptr<double>(i)[1] * des_size);
	}
}

void GBPI::loadImg(cv::Mat& pic){

	if (pic.empty())
	{
		std::cout<< "Empty image!" << std::endl;
		return ;
	}
	if (pic.channels()==3)
		cvtColor(pic,img, CV_RGB2GRAY);
	else
		img = pic.clone();

//	for (int sig=1; sig<=32; sig++)
//	{
//		cv::Mat B;
//		cv::GaussianBlur(img, B, cv::Size(2*sig+1,2*sig+1), sig*0.25);
//		imgs.push_back(B);
//	}
	return ;
}

void GBPI::getDes(cv::Point2i pt, arma::mat& des)const{

//    des = cv::Mat::zeros(1, Nl/2, CV_64FC1); // 1000/2 = 500
    des.set_size(1, Nl/2);

    if (img.empty())
    {
        std::cout<< "Empty image! ------> No descriptors!" << std::endl;
        //		return des;
    }

    cv::Mat pts = cv::Mat::zeros(Nl, 1, CV_32FC1);
    for (int i=0; i<Nl; i++)
    {
        cv::Point2i l1(pt.x+gp.ptr<double>(i)[0], pt.y+gp.ptr<double>(i)[1]);
        if ( l1.x>=img.cols || l1.y>=img.rows || l1.x<0 || l1.y<0 )
        {
//            std::cout << "Points range error."<< std::endl;
            continue;
        }
        // 		std::cout << gp.ptr<double>(i)[2] <<std::endl;
        pts.ptr<float>(i)[0] = img.ptr(l1.y)[l1.x];
        // 		pts.ptr<float>(i)[0] = imgs.at(0).ptr(l1.y)[l1.x];
    }
    for (int i=0; i<Nl/2; i++)
    {
        des(0,i) = (pts.ptr<float>(i)[0] - pts.ptr<float>(i+Nl/2)[0]);
    }
    // 	for (int i=0; i<Nl; i++)
    // 	{
    // 		for (int j=0; j<Nl; j++)
    // 		{
    // 			des.ptr<float>(0)[i*Nl+j] = pts.ptr<float>(i)[0] - pts.ptr<float>(j)[0];
    // 		}
    // 	}
    des = arma::normalise(des, 2, 1);
    //	return des;
}

void GBPI::getDes(cv::Point2i pt, cv::Mat& des)const{

    des = cv::Mat::zeros(1, Nl/2, CV_64FC1); // 1000/2 = 500

    if (img.empty())
    {
        std::cout<< "Empty image! ------> No descriptors!" << std::endl;
        //		return des;
    }

    cv::Mat pts = cv::Mat::zeros(Nl, 1, CV_32FC1);
    for (int i=0; i<Nl; i++)
    {
        cv::Point2i l1(pt.x+gp.ptr<double>(i)[0], pt.y+gp.ptr<double>(i)[1]);
        if ( l1.x>=img.cols || l1.y>=img.rows || l1.x<0 || l1.y<0 )
        {
            std::cout << "Points range error."<< std::endl;
            continue;
        }
        // 		std::cout << gp.ptr<double>(i)[2] <<std::endl;
        pts.ptr<float>(i)[0] = img.ptr(l1.y)[l1.x];
        // 		pts.ptr<float>(i)[0] = imgs.at(0).ptr(l1.y)[l1.x];
    }
    for (int i=0; i<Nl/2; i++)
    {
        des.ptr<double>(0)[i] = (pts.ptr<float>(i)[0] - pts.ptr<float>(i+Nl/2)[0]);
    }
    // 	for (int i=0; i<Nl; i++)
    // 	{
    // 		for (int j=0; j<Nl; j++)
    // 		{
    // 			des.ptr<float>(0)[i*Nl+j] = pts.ptr<float>(i)[0] - pts.ptr<float>(j)[0];
    // 		}
    // 	}
    normalize(des, des);
    //	return des;
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate value -0

    @par Semantic:

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

    is similar to:

    @code
    T r = -T(0);
    @endcode

    @ par Note:

    This is a special constant as it can be used and considered
    identical to zero, except that for floating point numbers,
    there is two different representation of zero with different bit of sign.

    The existence of the sign can be used in special circumstances as
    choosing between \f$+\infty\f$ and \f$-\infty\f$ instead of nan in computing 1/0.
    \par

    The sign of zero can be accessed through the @ref is_negative
    and @ref is_positive predicates or the @ref bitofsign functions.

    @return The Mzero constant for the proper type
  **/
  template<typename T> T Mzero();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant mzero.

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

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

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWSIM_UTIL_LINEPOLAR_HPP
#define RWSIM_UTIL_LINEPOLAR_HPP

#include "P2D.hpp"
#include "Pose2D.hpp"

#include <Eigen/Core>

namespace rwsim { namespace util {

    class LinePolar
    {
      public:
        // rho * (cos(theta), sin(theta)) is the point on the line nearest to origo.
        LinePolar (double rho = 0, double theta = 0);

        // 'pnt' is any point on the line, and theta is the usual angle.
        static LinePolar make (const P2D& pnt, double theta);

        // The line moving through the segment from 'start' to 'end'.
        static LinePolar make (const P2D& start, const P2D& end);

        double getRho () const { return _rho; }
        double getTheta () const { return _theta; }
        const P2D& getNormal () const { return _normal; }

        // The L_2 distance from 'pnt' to the line.
        double dist2 (const P2D& pnt);

        // The point for the projection of 'pnt' onto 'line'.
        static P2D projectionPoint (const LinePolar& line, const P2D& pnt);

        // A supporting point on the line (equal to rho * normal).
        static P2D linePoint (const LinePolar& line);

        // The vector for the projection of 'pnt' onto the normal of 'line'.
        static P2D normalProjectionVector (const LinePolar& line, const P2D& pnt);

        // Print the line to stdout.
        static void print (const LinePolar& line);

        /**
         * @brief get the line olar as a Eigen Vector
         * @return Eigen Vector
         */
        Eigen::Vector2d e ();

        // 'line' given relative to the coordinate frame of 'pose'.
        static LinePolar lineToLocal (const Pose2D& pose, const LinePolar& line);

      public:
        double _rho;
        double _theta;
        P2D _normal;
    };
}}    // namespace rwsim::util

#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <gtest/gtest.h>
#include <test/Helpers.h>
#include <boost/foreach.hpp>
#include <photk/BBoxIterator.h>

using namespace vw;
using namespace photk;

TEST(BBoxIterator, BoostForeach2i) {
  size_t element_count = 0;
  BBox2i box(150,-50,250,200);

  BOOST_FOREACH( Vector2i const& v, bbox_range(box) ) {
    EXPECT_TRUE( box.contains(v) ) << v;
    element_count++;
  }
  EXPECT_EQ( box.width()*box.height(), element_count );

  element_count = 0;
  BOOST_FOREACH( Vector2i v, bbox_range(box) ) {
    EXPECT_TRUE( box.contains(v) ) << v;
    element_count++;
  }
  EXPECT_EQ( box.width()*box.height(), element_count );
}

TEST(BBoxIterator, BoostForeach3i) {
  size_t element_count = 0;
  BBox3i box(40,-40,-15,50,50,50);

  BOOST_FOREACH( Vector3i const& v, bbox_range(box) ) {
    EXPECT_TRUE( box.contains(v) ) << v;
    element_count++;
  }
  EXPECT_EQ( box.width()*box.height()*box.depth(), element_count );
}

TEST(BBoxIterator, BoostForeach2u) {
  size_t element_count = 0;
  BBox<uint32,2> box(50,1627,88,99);

  typedef Vector<uint32,2> Vector2u;
  BOOST_FOREACH( Vector2u const& v, bbox_range(box) ) {
    EXPECT_TRUE( box.contains(v) ) << v;
    element_count++;
  }
  EXPECT_EQ( box.width()*box.height(), element_count );
}

TEST(BBoxIterator, VerifyPattern) {
  BBox2i box(150,-50,250,200);
  BBoxRange<BBox2i,int32,2>::iterator iter = bbox_range(box).begin();
  for ( int32 y = box.min().y(); y < box.max().y(); y++ ) {
    for ( int32 x = box.min().x(); x < box.max().x(); x++ ) {
      EXPECT_EQ( Vector2i(x,y), *iter );
      iter++;
    }
  }
}


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   main.cpp
 * Author: haoran
 *
 * Created on October 2, 2017, 9:09 AM
 */

#include <cstdlib>
#include <boost/program_options/options_description.hpp>
#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>      // std::istringstream
#include <unordered_map>
#include <ctime>// record time
#include <cmath>
#include <numeric>
#include <frovedis/ml/graph/graph.hpp>
// #include <frovedis/ml/graph/graph.cpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);

    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), " input data path. When using binary files, the path is the directory containing all files. When using string file, the path is the file path.")
        ("binary,b" , " use binary graph matrix file" )
        ("dfactor,d", value<double>(), "damping factor (default: 0.15)")
        ("epsilon,e", value<double>(), "convergence threshold (default: 1E-4)")
        ("iter_max,", value<size_t>(), "maximum iterations (default: 100)")
        ("prepare,p" , "if to generate the CRS matrix from original graph list file ")
        ("method,m" , value<std::string>(), "method (pagerank_v1, pagerank_v1_shrink, pagerank_v2, pagerank_v2_shrink")
        // ("cout" , " enable screen output for the search result" )
        // ("fout" , " enable file output for the search result" )
        ("matformat,f" , value<std::string>(), " matrix format (default: spMV: CRS(X86) HYB(SX)");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    bool if_prep = 0; // true if prepare data from raw dataset
    bool if_binary = 0;
    bool if_cout = 0;
    bool if_fout = 0;

    size_t iter_max = 100;
    double epsilon = 1e-4; //default convergence threshold
    double df = 0.15;

    std::string data_p;
    std::string method_name;
    std::string matformat;
// #ifndef _SX
#if !defined(_SX) && !defined(__ve__)
    matformat = "CRS";
#else
    matformat = "HYB";
#endif

//////////////////////////   command options   ///////////////////////////
    if(argmap.count("help")){
        std::cerr << opt << std::endl;
        exit(1);
    }

    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("binary")){
        if_binary = true;
    }
    if(argmap.count("prepare")){
        if_prep = true;
    }
    if(argmap.count("method")){
        method_name = argmap["method"].as<std::string>();
    }else{
      std::cerr << "input is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    // if(argmap.count("cout")){
    //     if_cout = true;
    // }
    // if(argmap.count("fout")){
    //     if_fout = true;
    // }
    if(argmap.count("matformat")){
       matformat = argmap["matformat"].as<std::string>();
    }
    if(argmap.count("dfactor")){
       df = argmap["dfactor"].as<double>();
    }
    if(argmap.count("epsilon")){
       epsilon = argmap["epsilon"].as<double>();
    }
    if(argmap.count("iter_max")){
       epsilon = argmap["iter_max"].as<size_t>();
    }
/////////////////////////////////////////////////////////////////

    graph* CCptr = new graph();
    CCptr->set_if_cout(if_cout);
    CCptr->set_if_fout(if_fout);

    if(if_prep == true){
        CCptr->prep_graph_crs_pagerank(data_p);
        data_p = data_p + "_graph_in_crs_pg";
    }
    //connected component
    CCptr->read_graph_pagerank(data_p, if_binary);

    if(method_name == "pagerank_v1"){
        CCptr->pagerank_v1(matformat, df, epsilon, iter_max);
    }
    else if(method_name == "pagerank_v1_shrink"){
        CCptr->pagerank_v1_shrink(matformat, df, epsilon, iter_max);
    }
    else if(method_name == "pagerank_v2"){
        CCptr->pagerank_v2(matformat, df, epsilon, iter_max);
    }
    else if(method_name == "pagerank_v2_shrink"){
        CCptr->pagerank_v2_shrink(matformat, df, epsilon, iter_max);
    }
    else{
        std::cerr<<"ERROR: Method "<<method_name<<" doesn't exist!"<<std::endl;
        std::cerr << opt << std::endl;
        exit(1);
    }
    return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseLengthConversion.cpp
//! \author Alex Robinson
//! \brief  The inverse length conversion unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_InverseAngstromUnit.hpp"
#include "Utility_InverseCentimeterUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse length units can be converted
TEUCHOS_UNIT_TEST( InverseLengthConversion, convert )
{
  quantity<InverseAngstrom> inverse_angstrom_q( 1.0*inverse_centimeter );

  quantity<InverseCentimeter> inverse_centimeter_q( 1.0*inverse_angstrom );

  TEST_EQUALITY_CONST( inverse_angstrom_q.value(), 1e-8 );
  TEST_EQUALITY_CONST( inverse_centimeter_q.value(), 1e8 );
}

//---------------------------------------------------------------------------//
// end tstInverseLengthConversion.cpp
//---------------------------------------------------------------------------//


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 5, 3> test_int_5_3(const Eigen::Matrix<int, 5, 3> & M)
{
	return M;
}
void export_int_5_3()
{
	boost::python::def("test_int_5_3",test_int_5_3);
}


/// @author (Enigmatisms:https://github.com/Enigmatisms) @copyright Qianyue He
#pragma once
#include <Eigen/Core>
#include <queue>

const double margin = 1e-6;

// 投影边
class Edge: public std::deque<Eigen::Vector3d> {
public:
    double min_dist = 1e9;
    bool valid = true;                      // 是否完全被遮挡
public:
    Edge() {reset();}

    bool angleInRange(double angle) const {
        if (this->front().z() > this->back().z())        // 跨越奇异
            return (angle > this->front().z() + margin) || (angle < this->back().z() - margin);
        else
            return (angle > this->front().z() + margin) && (angle < this->back().z() - margin);
    }

    bool angleInRange(double angle, int k, int k_1) const {
        const double k_ang = this->at(k).z(), k_ang_1 = this->at(k_1).z();
        if (k_ang > k_ang_1)        // 跨越奇异
            return (angle > k_ang + margin) || (angle < k_ang_1 - margin);
        else
            return (angle > k_ang + margin) && (angle < k_ang_1 - margin);
    }

    void reset() {
        clear();
        min_dist = 1e9;
        valid = true;
    }

    void initWithObs(Eigen::Vector2d obs);            // 根据观测点，初始化角度以及ids

    int rotatedBinarySearch(double angle) const;            // binary search acceleration

    Eigen::Vector2d getRayIntersect(const Eigen::Vector3d& ray, const Eigen::Vector2d& obs) const;
private:
    int binarySearch(std::pair<int, int> range, double angle) const;
};