// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <boost/yap/expression.hpp>

#include <boost/test/minimal.hpp>

#include <sstream>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

namespace yap = boost::yap;

int test_main(int, char * [])
{
    {
        term<double> unity{1.0};

        {
            auto expr =
                unity + unity + unity + unity + unity + unity + unity + unity;
            double result = boost::yap::evaluate(expr);
            BOOST_CHECK(result == 8.0);
        }

        {
            auto expr =
                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity +

                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity +
                unity + unity + unity + unity + unity + unity + unity + unity;
            double result = boost::yap::evaluate(expr);
            BOOST_CHECK(result == 64.0);
        }

#if 0 // This makes the Travis VMs barf.
    {
        auto expr =
            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity +

            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity +
            unity + unity + unity + unity +    unity + unity + unity + unity;
        double result = boost::yap::evaluate(
            expr + expr + expr + expr +    expr + expr + expr + expr +
            expr + expr + expr + expr +    expr + expr + expr + expr +
            expr + expr + expr + expr +    expr + expr + expr + expr +
            expr + expr + expr + expr +    expr + expr + expr + expr
        );
        BOOST_CHECK(result == 64.0 * 32.0);
    }
#endif
    }

    return 0;
}


// Copyright 2020 Tier IV, Inc.
//
// 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 LOCALIZATION_ERROR_MONITOR__NODE_HPP_
#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_

#include <Eigen/Dense>

#include "rclcpp/rclcpp.hpp"

#include "diagnostic_msgs/msg/diagnostic_array.hpp"
#include "diagnostic_updater/diagnostic_updater.hpp"

#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

struct Ellipse
{
  double long_radius;
  double short_radius;
  double yaw;
  Eigen::Matrix2d P;
  double size_lateral_direction;
};

class LocalizationErrorMonitor : public rclcpp::Node
{
private:
  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_cov_sub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr ellipse_marker_pub_;

  rclcpp::TimerBase::SharedPtr timer_;
  double scale_;
  double error_ellipse_size_;
  double warn_ellipse_size_;
  double error_ellipse_size_lateral_direction_;
  double warn_ellipse_size_lateral_direction_;
  Ellipse ellipse_;
  diagnostic_updater::Updater updater_;

  void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat);
  void checkLocalizationAccuracyLateralDirection(
    diagnostic_updater::DiagnosticStatusWrapper & stat);
  void onPoseWithCovariance(
    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr input_msg);
  visualization_msgs::msg::Marker createEllipseMarker(
    const Ellipse & ellipse,
    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_with_cov);
  double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta);
  void onTimer();

public:
  LocalizationErrorMonitor();
  ~LocalizationErrorMonitor() = default;
};
#endif  // LOCALIZATION_ERROR_MONITOR__NODE_HPP_


/*
 * base64.cpp
 *
 *  Created on: Jan 16, 2019
 *      Author: iconmaster
 */

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/algorithm/string.hpp>

#include "base64.hpp"

using namespace std;
using namespace boost::archive::iterators;
using namespace boost::algorithm;

namespace iconus {
//	std::string base64decode(const std::string& val) {
//		using It = transform_width<binary_from_base64<std::string::const_iterator>, 8, 6>;
//		return trim_right_copy_if(std::string(It(std::begin(val)), It(std::end(val))), [](char c) {
//			return c == '\0';
//		});
//	}

	std::string base64encode(const Base64Buffer& buf) {
		using It = base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>;
		auto tmp = std::string(It(buf.b), It(buf.b+buf.n));
		return tmp.append((3 - buf.n % 3) % 3, '=');
	}
}


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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.
 */

#include <algorithm>
#include <cstring>
#include <filesystem>
#include <iostream>
#include <string>
#include <vector>

#include <benchmark/benchmark.h>

#include <Eigen/Dense>

#include <petscsys.h>

#include <solvers/CuSparseSolver.hpp>
#include <solvers/PetscSolver.hpp>
#include <solvers/Solver.hpp>
#include <solvers/SparseSystem.hpp>
#include <solvers/SuiteSparseSolver.hpp>
#include <solvers/SuperLUSolver.hpp>
#include <solvers/ViennaCLSolver.hpp>

namespace fs = std::filesystem;

std::vector<fs::path> getDatasetPaths(const fs::path& dataset_dir)
{
    std::vector<fs::path> paths;
    for (const fs::path& entry : fs::directory_iterator(dataset_dir)) {
        if (entry.extension() == ".npz") {
            paths.emplace_back(entry);
        }
    }
    return paths;
}

bool endsWith(std::string const& value, std::string const& ending)
{
    if (ending.size() > value.size()) {
        return false;
    }
    return std::equal(ending.rbegin(), ending.rend(), value.rbegin());
}

void benchmarkSolver(benchmark::State& state,
                     const solvers::Solver& solver,
                     const std::vector<fs::path>& dataset)
{
    double error_sum = 0.;
    for (auto _ : state) {
        int64_t it = state.items_processed();
        solvers::SparseSystem system =
            solvers::SparseSystem::loadNpz(dataset[it]);
        double duration;
        Eigen::VectorXd result = solver.solve(system, duration);
        state.SetIterationTime(duration);
        Eigen::VectorXd solution = system.getEigenSolution();
        double error = (result - solution).norm() / solution.norm();
        error_sum += error;
        state.SetItemsProcessed(it + 1);
    }
    state.counters["Average Relative Error"] =
        benchmark::Counter(error_sum, benchmark::Counter::kAvgIterations);
}

std::vector<benchmark::internal::Benchmark*> registerCuSparseBenchmarks(
    const std::vector<fs::path>& dataset)
{
    std::map<std::string,
             std::pair<solvers::CuSparseMethod, solvers::CuSparseReorder>>
        configs = {
            {"GPUCuSparse/Qr",
             {solvers::CuSparseMethod::QR, solvers::CuSparseReorder::None}},
            {"GPUCuSparse/QrSymRCM",
             {solvers::CuSparseMethod::QR, solvers::CuSparseReorder::SymRCM}},
            {"GPUCuSparse/QrSymAMD",
             {solvers::CuSparseMethod::QR, solvers::CuSparseReorder::SymAMD}},
            {"GPUCuSparse/QrMetis",
             {solvers::CuSparseMethod::QR, solvers::CuSparseReorder::METIS}},
            {"GPUCuSparse/Cholesky",
             {solvers::CuSparseMethod::Cholesky,
              solvers::CuSparseReorder::None}},
            {"GPUCuSparse/CholeskySymRCM",
             {solvers::CuSparseMethod::Cholesky,
              solvers::CuSparseReorder::SymRCM}},
            {"GPUCuSparse/CholeskySymAMD",
             {solvers::CuSparseMethod::Cholesky,
              solvers::CuSparseReorder::SymAMD}},
            {"GPUCuSparse/CholeskyMetis",
             {solvers::CuSparseMethod::Cholesky,
              solvers::CuSparseReorder::METIS}}};
    std::vector<benchmark::internal::Benchmark*> benchmarks;
    benchmarks.reserve(configs.size());
    for (const auto& [name, config] : configs) {
        solvers::CuSparseSolver solver(config.first, config.second);
        benchmarks.emplace_back(benchmark::RegisterBenchmark(
            name.c_str(), benchmarkSolver, solver, dataset));
    }
    return benchmarks;
}

std::vector<benchmark::internal::Benchmark*> registerPetscBenchmarks(
    const std::vector<fs::path>& dataset)
{
    std::map<std::string,
             std::pair<solvers::PetscMethod, solvers::PetscPreconditioner>>
        configs = {
            {"Petsc/Cg",
             {solvers::PetscMethod::CG, solvers::PetscPreconditioner::None}},
            {"Petsc/CgJacobi",
             {solvers::PetscMethod::CG, solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/CgSor",
             {solvers::PetscMethod::CG, solvers::PetscPreconditioner::SOR}},
            {"Petsc/CgEisenstat",
             {solvers::PetscMethod::CG,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/CgIlu",
             {solvers::PetscMethod::CG, solvers::PetscPreconditioner::ILU}},
            {"Petsc/CgIcc",
             {solvers::PetscMethod::CG, solvers::PetscPreconditioner::ICC}},
            {"Petsc/Gmres",
             {solvers::PetscMethod::GMRES, solvers::PetscPreconditioner::None}},
            {"Petsc/GmresJacobi",
             {solvers::PetscMethod::GMRES,
              solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/GmresSor",
             {solvers::PetscMethod::GMRES, solvers::PetscPreconditioner::SOR}},
            {"Petsc/GmresEisenstat",
             {solvers::PetscMethod::GMRES,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/GmresIlu",
             {solvers::PetscMethod::GMRES, solvers::PetscPreconditioner::ILU}},
            {"Petsc/GmresIcc",
             {solvers::PetscMethod::GMRES, solvers::PetscPreconditioner::ICC}},
            {"Petsc/FlexibleGmres",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::None}},
            {"Petsc/FlexibleGmresJacobi",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/FlexibleGmresSor",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::SOR}},
            {"Petsc/FlexibleGmresEisenstat",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/FlexibleGmresIlu",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::ILU}},
            {"Petsc/FlexibleGmresIcc",
             {solvers::PetscMethod::FlexibleGMRES,
              solvers::PetscPreconditioner::ICC}},
            {"Petsc/Qmr",
             {solvers::PetscMethod::QMR, solvers::PetscPreconditioner::None}},
            {"Petsc/QmrJacobi",
             {solvers::PetscMethod::QMR, solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/QmrSor",
             {solvers::PetscMethod::QMR, solvers::PetscPreconditioner::SOR}},
            {"Petsc/QmrEisenstat",
             {solvers::PetscMethod::QMR,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/QmrIlu",
             {solvers::PetscMethod::QMR, solvers::PetscPreconditioner::ILU}},
            {"Petsc/QmrIcc",
             {solvers::PetscMethod::QMR, solvers::PetscPreconditioner::ICC}},
            {"Petsc/Cr",
             {solvers::PetscMethod::CR, solvers::PetscPreconditioner::None}},
            {"Petsc/CrJacobi",
             {solvers::PetscMethod::CR, solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/CrSor",
             {solvers::PetscMethod::CR, solvers::PetscPreconditioner::SOR}},
            {"Petsc/CrEisenstat",
             {solvers::PetscMethod::CR,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/CrIlu",
             {solvers::PetscMethod::CR, solvers::PetscPreconditioner::ILU}},
            {"Petsc/CrIcc",
             {solvers::PetscMethod::CR, solvers::PetscPreconditioner::ICC}},
            {"Petsc/Minres",
             {solvers::PetscMethod::MinRes,
              solvers::PetscPreconditioner::None}},
            {"Petsc/MinresJacobi",
             {solvers::PetscMethod::MinRes,
              solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/MinresSor",
             {solvers::PetscMethod::MinRes, solvers::PetscPreconditioner::SOR}},
            {"Petsc/MinresEisenstat",
             {solvers::PetscMethod::MinRes,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/MinresIlu",
             {solvers::PetscMethod::MinRes, solvers::PetscPreconditioner::ILU}},
            {"Petsc/MinresIcc",
             {solvers::PetscMethod::MinRes, solvers::PetscPreconditioner::ICC}},
            {"Petsc/Symmlq",
             {solvers::PetscMethod::SymmLQ,
              solvers::PetscPreconditioner::None}},
            {"Petsc/SymmlqJacobi",
             {solvers::PetscMethod::SymmLQ,
              solvers::PetscPreconditioner::Jacobi}},
            {"Petsc/SymmlqSor",
             {solvers::PetscMethod::SymmLQ, solvers::PetscPreconditioner::SOR}},
            {"Petsc/SymmlqEisenstat",
             {solvers::PetscMethod::SymmLQ,
              solvers::PetscPreconditioner::Eisenstat}},
            {"Petsc/SymmlqIlu",
             {solvers::PetscMethod::SymmLQ, solvers::PetscPreconditioner::ILU}},
            {"Petsc/SymmlqIcc",
             {solvers::PetscMethod::SymmLQ, solvers::PetscPreconditioner::ICC}},
            {"Petsc/Cholesky",
             {solvers::PetscMethod::Cholesky,
              solvers::PetscPreconditioner::None}},
            {"Petsc/Lu",
             {solvers::PetscMethod::LU, solvers::PetscPreconditioner::None}},
            {"Petsc/Qr",
             {solvers::PetscMethod::QR, solvers::PetscPreconditioner::None}}};
    std::vector<benchmark::internal::Benchmark*> benchmarks;
    benchmarks.reserve(configs.size());
    for (const auto& [name, config] : configs) {
        solvers::PetscSolver solver(config.first, config.second);
        benchmarks.emplace_back(benchmark::RegisterBenchmark(
            name.c_str(), benchmarkSolver, solver, dataset));
    }
    return benchmarks;
}

std::vector<benchmark::internal::Benchmark*> registerSuiteSparseBenchmarks(
    const std::vector<fs::path>& dataset)
{
    std::map<std::string, solvers::SuiteSparseMethod> configs = {
        {"SuiteSparse/SimplicialLLT",
         solvers::SuiteSparseMethod::SimplicialLLT},
        {"SuiteSparse/SimplicialLDLT",
         solvers::SuiteSparseMethod::SimplicialLDLT},
        {"SuiteSparse/SupernodalLLT",
         solvers::SuiteSparseMethod::SupernodalLLT},
        {"SuiteSparse/LU", solvers::SuiteSparseMethod::LU},
        {"SuiteSparse/KLU", solvers::SuiteSparseMethod::KLU}};
    std::vector<benchmark::internal::Benchmark*> benchmarks;
    benchmarks.reserve(configs.size());
    constexpr bool gpu = true;
    for (const auto& [name, config] : configs) {
        solvers::SuiteSparseSolver solver(config);
        benchmarks.emplace_back(benchmark::RegisterBenchmark(
            name.c_str(), benchmarkSolver, solver, dataset));
        if (!endsWith(name, "LU")) {
            solvers::SuiteSparseSolver gpu_solver(config, gpu);
            benchmarks.emplace_back(benchmark::RegisterBenchmark(
                ("GPU" + name).c_str(), benchmarkSolver, solver, dataset));
        }
    }
    return benchmarks;
}

std::vector<benchmark::internal::Benchmark*> registerSuperLUBenchmarks(
    const std::vector<fs::path>& dataset)
{
    std::map<std::string, solvers::SuperLUReorder> configs = {
        {"GPUSuperLU/Lu", solvers::SuperLUReorder::None},
        {"GPUSuperLU/LuMinimumDegree", solvers::SuperLUReorder::MinimumDegree},
        {"GPUSuperLU/LuColamd", solvers::SuperLUReorder::ColAMD},
        {"GPUSuperLU/LuMetis", solvers::SuperLUReorder::METIS}};
    std::vector<benchmark::internal::Benchmark*> benchmarks;
    benchmarks.reserve(configs.size());
    for (const auto& [name, config] : configs) {
        solvers::SuperLUSolver solver(config);
        benchmarks.emplace_back(benchmark::RegisterBenchmark(
            name.c_str(), benchmarkSolver, solver, dataset));
    }
    return benchmarks;
}

std::vector<benchmark::internal::Benchmark*> registerViennaCLBenchmarks(
    const std::vector<fs::path>& dataset,
    const double tolerance = 1e-6,
    const int iterations = 1000)
{
    std::map<std::string, std::pair<solvers::ViennaCLMethod,
                                    solvers::ViennaCLPreconditioner>>
        configs = {
            {"GPUViennaCL/Cg",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::None}},
            {"GPUViennaCL/CgChowPatel",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::ChowPatel}},
            {"GPUViennaCL/CgIlu0",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::ILU0}},
            {"GPUViennaCL/CgIchol0",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::IChol0}},
            {"GPUViennaCL/CgBlockIlu0",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::BlockILU0}},
            {"GPUViennaCL/CgJacobi",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::Jacobi}},
            {"GPUViennaCL/CgRowScaling",
             {solvers::ViennaCLMethod::CG,
              solvers::ViennaCLPreconditioner::RowScaling}},
            {"GPUViennaCL/Gmres",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::None}},
            {"GPUViennaCL/GmresChowPatel",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::ChowPatel}},
            {"GPUViennaCL/GmresIlu0",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::ILU0}},
            {"GPUViennaCL/GmresIchol0",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::IChol0}},
            {"GPUViennaCL/GmresBlockIlu0",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::BlockILU0}},
            {"GPUViennaCL/GmresJacobi",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::Jacobi}},
            {"GPUViennaCL/GmresRowScaling",
             {solvers::ViennaCLMethod::GMRES,
              solvers::ViennaCLPreconditioner::RowScaling}},
        };
    std::vector<benchmark::internal::Benchmark*> benchmarks;
    benchmarks.reserve(configs.size());
    for (const auto& [name, config] : configs) {
        solvers::ViennaCLSolver solver(config.first, config.second, tolerance,
                                       iterations);
        benchmarks.emplace_back(benchmark::RegisterBenchmark(
            name.c_str(), benchmarkSolver, solver, dataset));
    }
    return benchmarks;
}

int main(int argc, char** argv)
{
    fs::path dataset_dir;

    const char* dataset_option = "--dataset=";
    std::vector<char*> benchmark_arguments;
    benchmark_arguments.reserve(argc);
    for (int i = 0; i < argc; ++i) {
        if (strncmp(dataset_option, argv[i], strlen(dataset_option)) == 0) {
            dataset_dir.assign(argv[i] + strlen(dataset_option));
        }
        else {
            benchmark_arguments.emplace_back(argv[i]);
        }
    }
    if (dataset_dir.empty()) {
        std::cout << "The required argument --dataset is missing, use "
                     "--dataset=<dataset_directory>"
                  << std::endl;
        return 1;
    }
    if (!fs::exists(dataset_dir)) {
        std::cout << "No such dataset directory: " << dataset_dir << std::endl;
        return 1;
    }

    PetscInitialize(nullptr, nullptr, PETSC_CONFIG, nullptr);
    std::vector<fs::path> dataset = getDatasetPaths(dataset_dir);

    std::vector<std::vector<benchmark::internal::Benchmark*>> benchmarks = {
        registerCuSparseBenchmarks(dataset), registerPetscBenchmarks(dataset),
        registerSuiteSparseBenchmarks(dataset),
        registerViennaCLBenchmarks(dataset),
        registerSuperLUBenchmarks(dataset)};
    for (const auto& lib_benchmarks : benchmarks) {
        for (const auto& benchmark : lib_benchmarks) {
            benchmark->UseManualTime()
                ->Unit(benchmark::kMillisecond)
                ->Iterations(dataset.size());
        }
    }

    argc = static_cast<int>(benchmark_arguments.size());
    argv = benchmark_arguments.data();
    ::benchmark::Initialize(&argc, argv);
    if (::benchmark::ReportUnrecognizedArguments(argc, argv)) {
        return 1;
    }
    ::benchmark::RunSpecifiedBenchmarks();
    ::benchmark::Shutdown();
    PetscFinalize();
    return 0;
}


/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_POINTLATLON_HPP
#define UTILITIES_GEOMETRY_POINTLATLON_HPP

#include "../UtilitiesAPI.hpp"
#include "../data/Vector.hpp"
#include "../core/Logger.hpp"

#include <vector>
#include <boost/optional.hpp>

// do not include anything from GeographicLib in header or expose in the API
namespace GeographicLib{
  class LocalCartesian;
  class UTMUPS;
}

namespace openstudio{

  class Point3d;

  /* PointLatLon represents a point on the Earth's (reference ellipsoid) surface, an optional height parameter is available
  ** for points above or below the geoid surface.  Currently only the WGS84 ellipsoid is supported,
  ** other systems may be added in the future. When one PointLatLon is taken as an origin, it can be used to
  ** translate other PointLatLon's into either a local Cartesian coordinate system (centered on the origin, with z up and y oriented North)
  ** or the UTM (Universal Transverse Mercator) zone which contains the origin. The origin PointLatLon can be used to
  ** translate Point3d's in either local Cartesian or UTM coordinate systems back to lat/lon.
  */
  class UTILITIES_API PointLatLon{
  public:

    /// default constructor creates point at 0, 0, 0
    PointLatLon();

    /// constructor with lat, lon, and height
    PointLatLon(double lat, double lon, double height = 0);

    /// copy constructor
    PointLatLon(const PointLatLon& other);

    /// destructor
    ~PointLatLon();

    /// get lat
    double lat() const;

    /// get lon
    double lon() const;

    /// get height
    double height() const;

    PointLatLon& operator=(const openstudio::PointLatLon& other);

    /// check equality
    bool operator==(const PointLatLon& other) const;

    /// point minus another point is distance along geodesic line
    double operator-(const PointLatLon& other) const;

    /// converts a PointLatLon to a Point3d in a Local Cartesian frame centered on this with y oriented North and z oriented up
    Point3d toLocalCartesian(const PointLatLon& point) const;
    std::vector<Point3d> toLocalCartesian(const std::vector<PointLatLon>& points) const;

    /// converts a Point3d in Local Cartesian coordinate system centered on this to LatLon
    PointLatLon fromLocalCartesian(const Point3d& point) const;
    std::vector<PointLatLon> fromLocalCartesian(const std::vector<Point3d>& points) const;

    /// gets the UTM zone for this point
    int utmZone() const;

    /// converts a PointLatLon to a Point3d in UTM centered on this
    Point3d toUTM(const PointLatLon& point) const;
    std::vector<Point3d> toUTM(const std::vector<PointLatLon>& points) const;

    /// converts a Point3d in UTM centered on this to LatLon
    PointLatLon fromUTM(const Point3d& point) const;
    std::vector<PointLatLon> fromUTM(const std::vector<Point3d>& points) const;

  private:

    REGISTER_LOGGER("utilities.PointLatLon");
    Vector m_storage;

    void initLocalCartesianConverter() const;

    mutable std::unique_ptr<GeographicLib::LocalCartesian> m_localCartesianConverter;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const PointLatLon& point);

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const std::vector<PointLatLon>& pointVector);

  // optional PointLatLon
  typedef boost::optional<PointLatLon> OptionalPointLatLon;

  // vector of PointLatLon
  typedef std::vector<PointLatLon> PointLatLonVector;

} // openstudio

#endif //UTILITIES_GEOMETRY_POINTLATLON_HPP


#include <urdf_editor/utils/convex_hull_generator.h>
#include <assimp/postprocess.h>
#include <ros/console.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/surface/convex_hull.h>
#include <boost/algorithm/string.hpp>
#include <string>
#include <fstream>
#include <boost/assign.hpp>

static const std::string STL_BIN_FORMAT_ID = "stlb";
static const std::map<std::string,std::string> OUTPUT_EXTENSION_MAP = boost::assign::map_list_of ("stl","stlb");

namespace urdf_editor
{
namespace utils
{

ConvexHullGenerator::ConvexHullGenerator():
    scene_(NULL)
{

}

ConvexHullGenerator::~ConvexHullGenerator()
{

}

bool ConvexHullGenerator::generate(const std::string& file_path)
{
  using namespace Assimp;

  const aiScene* scene = importer_.ReadFile(file_path,
                                            aiProcess_Triangulate|
                                            aiProcess_JoinIdenticalVertices|
                                            aiProcess_GenNormals);

  if(!scene)
  {
    ROS_ERROR_STREAM("Read Error: " <<importer_.GetErrorString());
    return false;
  }

  if(!scene->HasMeshes())
  {
    ROS_ERROR_STREAM("No meshes were found in file "<<file_path);
    return false;
  }

  // copying scene
  aiCopyScene(scene,&scene_);

  return generateConvexHull(scene_);
}

bool ConvexHullGenerator::save(const std::string& file_path)
{
  using namespace Assimp;
  Exporter exporter;

  // register STL binary exporter
  unsigned int steps = aiProcess_Triangulate | aiProcess_SortByPType | aiProcess_GenSmoothNormals |
      aiProcess_JoinIdenticalVertices | aiProcess_FixInfacingNormals | aiProcess_FindInvalidData ;

  Exporter::ExportFormatEntry format_entry(STL_BIN_FORMAT_ID.c_str(),"Stereolithography (binary)",
                                           "stl",&assimp::STLExporter::ExportSceneSTLBinary,
                                           steps);
  if(exporter.RegisterExporter(format_entry)!= aiReturn_SUCCESS)
  {
    ROS_ERROR_STREAM("Failed to register Binary STL exporter: "<<exporter.GetErrorString());
    return false;
  }

  if(!chull_mesh_)
  {
    ROS_ERROR_STREAM("Mesh file has not been loaded");
    return false;
  }

  // check extensions
  std::string ext = file_path.substr(file_path.find_last_of('.')+1);
  boost::algorithm::to_lower(ext);
  if(OUTPUT_EXTENSION_MAP.count(ext) <= 0)
  {
    ROS_ERROR("Extension %s is not supported",ext.c_str());
    return false;
  }

  // veryfing support for selected format
  int ext_count = exporter.GetExportFormatCount();
  std::string ext_id = OUTPUT_EXTENSION_MAP.at(ext);
  bool supported = false;
  for(unsigned int i = 0u; i < ext_count; i++)
  {
    const aiExportFormatDesc* desc = exporter.GetExportFormatDescription(i);
    if(std::string(desc->id).compare(ext_id) == 0) // find binary
    {
      supported = true;
      break;
    }
  }

  if(!supported)
  {
    ROS_ERROR("Assimp exporter does not support extension %s",ext.c_str());
    return false;
  }

  aiReturn res = exporter.Export(scene_,ext_id,file_path,steps);
  if(res == aiReturn_OUTOFMEMORY)
  {
    ROS_ERROR("Mesh export failed due to out-of-memory error");
    return false;
  }

  return res == aiReturn_SUCCESS;
}

bool ConvexHullGenerator::generateConvexHull(const aiScene* scene)
{
  using namespace pcl;

  const aiMesh* mesh = scene->mMeshes[0];
  if(mesh->mNumVertices == 0)
  {
    ROS_ERROR("Mesh geometry is empty");
    return false;
  }

  PointCloud<PointXYZ>::Ptr mesh_points(new PointCloud<PointXYZ>()), chull_points(new PointCloud<PointXYZ>());
  PointXYZ p;
  for(std::size_t i = 0; i < mesh->mNumVertices; i++)
  {
    p.x = mesh->mVertices[i].x;
    p.y = mesh->mVertices[i].y;
    p.z = mesh->mVertices[i].z;
    mesh_points->points.push_back(p);
  }

  // generate chull
  std::vector< Vertices > faces;
  ConvexHull<PointXYZ> chull;
  chull.setInputCloud(mesh_points);
  chull.reconstruct(*chull_points,faces);

  // creating assimp mesh from pcl convex-hull
  std::size_t vertices_per_face = 3;
  std::size_t num_vertices = chull_points->points.size();
  chull_mesh_.reset(new aiMesh());
  chull_mesh_->mMaterialIndex = 0;
  chull_mesh_->mVertices = new aiVector3D[num_vertices];
  chull_mesh_->mNumVertices = num_vertices;
  chull_mesh_->mFaces = new aiFace[faces.size()];
  chull_mesh_->mNumFaces = faces.size();
  chull_mesh_->mName = "convex-hull";

  for(std::size_t f = 0; f < faces.size();f++)
  {
    std::vector<unsigned int>& vertices = faces[f].vertices;

    aiFace& face = chull_mesh_->mFaces[f];
    face.mIndices = new unsigned int[vertices_per_face];
    face.mNumIndices = vertices_per_face;

    std::size_t start_index = f*vertices_per_face;
    std::size_t vertex_index;
    for(std::size_t v = 0; v < vertices.size() ; v++)
    {
      vertex_index = vertices[v];
      PointXYZ& p = chull_points->points[vertex_index];
      chull_mesh_->mVertices[vertex_index] = aiVector3D(p.x,p.y,p.z);
      face.mIndices[v] = vertex_index;
    }
  }

  // deleting old scene data
  delete scene_->mRootNode;
  delete scene_->mMeshes;

  // populating scene with new mesh
  scene_->mRootNode = new aiNode();

  scene_->mMeshes = new aiMesh*[1];
  scene_->mMeshes[0] = chull_mesh_.get();
  scene_->mMeshes[ 0 ]->mMaterialIndex = 0;
  scene_->mNumMeshes = 1;

  scene_->mRootNode->mMeshes = new unsigned int[ 1 ];
  scene_->mRootNode->mMeshes[ 0 ] = 0;
  scene_->mRootNode->mNumMeshes = 1;

  return !chull_points->points.empty();

}

} /* namespace utils */
} /* namespace urdf_editor */


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/contains.hpp>
#include <boost/hana/permutations.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unpack.hpp>

#include <laws/base.hpp>
namespace hana = boost::hana;
using hana::test::ct_eq;

int main() {
    hana::test::_injection<0> f{};

    auto check = [=](auto ...xs) {
        auto arg_perms = hana::permutations(hana::make_tuple(xs...));
        auto possible_results = hana::transform(arg_perms, [=](auto args) {
            return hana::unpack(args, f);
        });

        BOOST_HANA_CONSTANT_CHECK(hana::contains(
            possible_results,
            hana::unpack(hana::make_set(xs...), f)
        ));
    };

    check();
    check(ct_eq<1>{});
    check(ct_eq<1>{}, ct_eq<2>{});
    check(ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{});
    check(ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{}, ct_eq<4>{});
}


/*
 * Copyright 2015, Fetch Robotics Inc.
 * Copyright 2013-2014, Unbounded Robotics Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Unbounded Robotics, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

// Author: Michael Ferguson

#include <Eigen/Eigen>
#include <ros/ros.h>
#include <simple_grasping/shape_grasp_planner.h>

using shape_msgs::SolidPrimitive;

namespace simple_grasping
{

moveit_msgs::GripperTranslation makeGripperTranslation(
  std::string frame,
  double min,
  double desired,
  double x_axis = 1.0,
  double y_axis = 0.0,
  double z_axis = 0.0)
{
  moveit_msgs::GripperTranslation translation;
  translation.direction.vector.x = x_axis;
  translation.direction.vector.y = y_axis;
  translation.direction.vector.z = z_axis;
  translation.direction.header.frame_id = frame;
  translation.min_distance = min;
  translation.desired_distance = desired;
  return translation;
}

Eigen::Quaterniond quaternionFromEuler(float yaw, float pitch, float roll)
{
  float sy = sin(yaw*0.5);
  float cy = cos(yaw*0.5);
  float sp = sin(pitch*0.5);
  float cp = cos(pitch*0.5);
  float sr = sin(roll*0.5);
  float cr = cos(roll*0.5);
  float w = cr*cp*cy + sr*sp*sy;
  float x = sr*cp*cy - cr*sp*sy;
  float y = cr*sp*cy + sr*cp*sy;
  float z = cr*cp*sy - sr*sp*cy;
  return Eigen::Quaterniond(w,x,y,z);
}

double ShapeGraspPlanner::calc_grip_angle(double x)
{
	float dist = x*1000.0;
	float tmp = (0.5*dist-30.9476)/-87.0932;
	float a = asinf(tmp)+0.627445866;

    if (0.5*dist > 30.9476)
    {
        a+=0.00599;
    }
    else
    {
        a-=0.1;
    }

    return (a);
}

ShapeGraspPlanner::ShapeGraspPlanner(ros::NodeHandle& nh)
{
  /*
   * Gripper model is based on having two fingers, and assumes
   * that the robot is using the moveit_simple_controller_manager
   * gripper interface, with "parallel" parameter set to true.
   */
  left_joint_.resize(2);
  right_joint_.resize(2);
  max_opening_.resize(2);
  max_effort_.resize(2);
  grasp_duration_.resize(2);
  tool_offset_.resize(2);
  finger_depth_.resize(2);
  gripper_tolerance_.resize(2);
  approach_frame_.resize(2);
  approach_min_translation_.resize(2);
  approach_desired_translation_.resize(2);
  grasps_.resize(2);

  // retreat model
  retreat_frame_.resize(2);
  retreat_min_translation_.resize(2);
  retreat_desired_translation_.resize(2);

  nh.param<std::string>("left_gripper/left_joint", left_joint_[0], "left_gripper_finger1_joint");
  nh.param<std::string>("left_gripper/right_joint", right_joint_[0], "left_gripper_finger2_joint");
  nh.param("left_gripper/max_opening", max_opening_[0], 0.165);
  nh.param("left_gripper/max_effort", max_effort_[0], 25.0);
  nh.param("left_gripper/finger_depth", finger_depth_[0], 0.02);
  nh.param("left_gripper/grasp_duration", grasp_duration_[0], 2.0);
  nh.param("left_gripper/gripper_tolerance", gripper_tolerance_[0], 0.02);

  /*
   * Approach is usually aligned with wrist_roll
   */
  nh.param<std::string>("left_gripper/approach/frame", approach_frame_[0], "left_ee_link");
  nh.param("left_gripper/approach/min", approach_min_translation_[0], 0.1);
  nh.param("left_gripper/approach/desired", approach_desired_translation_[0], 0.15);

  /*
   * Retreat is usually aligned with wrist_roll
   */
  nh.param<std::string>("left_gripper/retreat/frame", retreat_frame_[0], "left_ee_link");
  nh.param("left_gripper/retreat/min", retreat_min_translation_[0], 0.1);
  nh.param("left_gripper/retreat/desired", retreat_desired_translation_[0], 0.15);

  // Distance from tool point to planning frame
  nh.param("left_gripper/tool_to_planning_frame", tool_offset_[0], 0.0);

  nh.param<std::string>("right_gripper/left_joint", left_joint_[1], "right_gripper_finger1_joint");
  nh.param<std::string>("right_gripper/right_joint", right_joint_[1], "right_gripper_finger2_joint");
  nh.param("right_gripper/max_opening", max_opening_[1], 0.185);
  nh.param("right_gripper/max_effort", max_effort_[1], 50.0);
  nh.param("right_gripper/finger_depth", finger_depth_[1], 0.02);
  nh.param("right_gripper/grasp_duration", grasp_duration_[1], 2.0);
  nh.param("right_gripper/gripper_tolerance", gripper_tolerance_[1], 0.02);

  /*
   * Approach is usually aligned with wrist_roll
   */
  nh.param<std::string>("right_gripper/approach/frame", approach_frame_[1], "right_ee_link");
  nh.param("right_gripper/approach/min", approach_min_translation_[1], 0.1);
  nh.param("right_gripper/approach/desired", approach_desired_translation_[1], 0.15);

  /*
   * Retreat is usually aligned with wrist_roll
   */
  nh.param<std::string>("right_gripper/retreat/frame", retreat_frame_[1], "right_ee_link");
  nh.param("right_gripper/retreat/min", retreat_min_translation_[1], 0.1);
  nh.param("right_gripper/retreat/desired", retreat_desired_translation_[1], 0.15);

  // Distance from tool point to planning frame
  nh.param("right_gripper/tool_to_planning_frame", tool_offset_[1], 0.0);

  nh.param("simulated_kinova", is_sim_, true);

}

int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
                                   double gripper_opening,
                                   double gripper_pitch,
                                   double x_offset,
                                   double z_offset,
                                   double quality,
								                   int g)
{
  moveit_msgs::Grasp grasp;
  grasp.grasp_pose = pose;

  // defaults
  if (true == is_sim_){
    grasp.pre_grasp_posture = makeGraspPosture(calc_grip_angle(max_opening_[g]),g);
    grasp.grasp_posture = makeGraspPosture(calc_grip_angle(gripper_opening),g);
  }else{
    grasp.pre_grasp_posture = makeGraspPosture(max_opening_[g],g);
    grasp.grasp_posture = makeGraspPosture(gripper_opening,g);
  }

  grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_[g],
                                                    approach_min_translation_[g],
                                                    approach_desired_translation_[g],
													                          1.0,0.0,0.0); // 0.0,0.0,-1.0);

  // TODO Change vector?
  // TODO Change translation for object height
  grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_[g],
                                                    retreat_min_translation_[g],
                                                    retreat_desired_translation_[g],
                                                    -1.0,0.0,0.0);  // 0.0,0.0,1.0); retreat is in positive z direction

  // initial pose
  Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
                                           pose.pose.position.y,
                                           pose.pose.position.z) *
                        Eigen::Quaterniond(pose.pose.orientation.w,
                                           pose.pose.orientation.x,
                                           pose.pose.orientation.y,
                                           pose.pose.orientation.z);
  // translate by x_offset, 0, z_offset
  p = p * Eigen::Translation3d(x_offset, 0, z_offset);
  // rotate by 0, pitch, 0
  p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
  // apply grasp point -> planning frame offset
  p = p * Eigen::Translation3d(-tool_offset_[g], 0, 0);

  grasp.grasp_pose.pose.position.x = p.translation().x();
  grasp.grasp_pose.pose.position.y = p.translation().y();
  grasp.grasp_pose.pose.position.z = p.translation().z();
  Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
  grasp.grasp_pose.pose.orientation.x = q.x();
  grasp.grasp_pose.pose.orientation.y = q.y();
  grasp.grasp_pose.pose.orientation.z = q.z();
  grasp.grasp_pose.pose.orientation.w = q.w();

  grasp.grasp_quality = quality;

  grasps_[g].push_back(grasp);
  return 1;
}

// Create the grasps going in one direction around an object
// starts with gripper level, rotates it up
// this works for boxes and cylinders
int ShapeGraspPlanner::createGraspSeries(
  const geometry_msgs::PoseStamped& pose,
  double depth, double width, double height, int g,
  bool use_vertical)
{
  int count = 0;

  // Gripper opening is limited
  if (width >= (max_opening_[g]*0.9))
    return count;

  double closed = std::max(width - gripper_tolerance_[g], 0.0);

  // Grasp horizontally along side of box
  // TODO Change z offset so we start higher than table height (the step argument)
  for (double step = 0.0; step < (height/2.0 - finger_depth_[g]); step += 0.01)
  {
    count += createGrasp(pose, closed, 0.0, 0.0, step, 0.8 - 0.1*step, g);  // horizontal
    count += createGrasp(pose, closed, 0.1, 0.0, step, 0.7 - 0.1*step, g);  // angled
    count += createGrasp(pose, closed, 0.2, 0.0, step, 0.6 - 0.1*step, g);  // angled_more
    // count += createGrasp(pose, closed, 0.0, 0.0, -step, 0.5 - 0.1*step, g);  // horizontal
    // count += createGrasp(pose, closed, 0.1, 0.0, -step, 0.4 - 0.1*step, g);  // angled
    // count += createGrasp(pose, closed, 0.2, 0.0, -step, 0.3 - 0.1*step, g);  // angled_more

  }

  return count;
}

int ShapeGraspPlanner::plan(const grasp_msgs::Object& object,
                            std::vector<moveit_msgs::Grasp>& grasps, int g)
{
  ROS_INFO("shape grasp planning starting...");

  // Need a shape primitive
  if (object.primitives.size() == 0)
  {
    // Shape grasp planner can only plan for objects
    //  with SolidPrimitive bounding boxes
    return -1;
  }

  if (object.primitive_poses.size() != object.primitives.size())
  {
    // Invalid object
    return -1;
  }

  // Clear out internal vector
  grasps_[g].clear();

  // Setup Pose
  geometry_msgs::PoseStamped grasp_pose;
  grasp_pose.header = object.header;
  grasp_pose.pose = object.primitive_poses[0];

  // Setup object orientation
  Eigen::Quaterniond q(object.primitive_poses[0].orientation.w,
                       object.primitive_poses[0].orientation.x,
                       object.primitive_poses[0].orientation.y,
                       object.primitive_poses[0].orientation.z);

  // Start by rotating 90 deg
  q = q * quaternionFromEuler(-1.571, 0.0, 0.0);

  // Setup object dimensions
  double x, y, z;
  if (object.primitives[0].type == SolidPrimitive::BOX)
  {
    x = object.primitives[0].dimensions[SolidPrimitive::BOX_X];
    y = object.primitives[0].dimensions[SolidPrimitive::BOX_Y];
    z = object.primitives[0].dimensions[SolidPrimitive::BOX_Z];
  }
  else if (object.primitives[0].type == SolidPrimitive::CYLINDER)
  {
    x = y = 2.0 * object.primitives[0].dimensions[SolidPrimitive::CYLINDER_RADIUS];
    z = object.primitives[0].dimensions[SolidPrimitive::CYLINDER_HEIGHT];
  }

  // Generate grasps
  for (int i = 0; i < 4; ++i)
  {
    grasp_pose.pose.orientation.x = q.x();
    grasp_pose.pose.orientation.y = q.y();
    grasp_pose.pose.orientation.z = q.z();
    grasp_pose.pose.orientation.w = q.w();

    createGraspSeries(grasp_pose, x, y, z, g);

    // Next iteration, rotate 45 degrees about Z axis
    q = q * quaternionFromEuler(-0.7854, 0.0, 0.0);
    std::swap(x, y);
  }

  ROS_INFO("shape grasp planning done.");

  grasps = grasps_[g];
  return grasps.size();  // num of grasps
}

trajectory_msgs::JointTrajectory
ShapeGraspPlanner::makeGraspPosture(double pose, int g)
{
  trajectory_msgs::JointTrajectory trajectory;
  trajectory.joint_names.push_back(left_joint_[g]);
  trajectory.joint_names.push_back(right_joint_[g]);
  trajectory_msgs::JointTrajectoryPoint point;
  point.positions.push_back(pose);
  point.positions.push_back(pose);
  point.effort.push_back(max_effort_[g]);
  point.effort.push_back(max_effort_[g]);
  point.time_from_start = ros::Duration(grasp_duration_[g]);
  trajectory.points.push_back(point);
  return trajectory;
}

}  // namespace simple_grasping


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/sgtbx/space_group_type.h>
#include <cctbx/sgtbx/space_group_hash.h>
#include <boost/python/tuple.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include <scitbx/boost_python/utils.h>
#include <boost_adaptbx/hash.h>

namespace cctbx { namespace sgtbx { namespace boost_python {

namespace {

  struct space_group_wrappers : boost::python::pickle_suite,
                                boost_adaptbx::py_hashable<space_group>
  {
    typedef space_group w_t;

    static rt_mx
    getitem(w_t const& o, std::size_t i_op)
    {
      if (i_op >= o.order_z()) scitbx::boost_python::raise_index_error();
      return o(i_op);
    }

    static rt_mx
    call_3(w_t const& o,
           std::size_t i_ltr, std::size_t i_inv, std::size_t i_smx)
    {
      return o(i_ltr, i_inv, i_smx);
    }

    static boost::python::tuple
    getinitargs(w_t const& o)
    {
      return boost::python::make_tuple(o.type().hall_symbol());
    }
/* Must tidy the spacegroup order of operations because
   asu_mappings.find_i_sym(i_seq, rt_mx) rely on the order being retained.
   We do this in a setstate method that ignores its arguments and only calls make_tidy()
   on itself. Likewise the corresponding getstate() is a dummy returning an empty tuple.
*/
    static boost::python::tuple
      getstate(w_t const& o)
    {
      return boost::python::tuple();
    }

    static void
      setstate(w_t& o, boost::python::tuple state)
    {
      o.make_tidy();
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("space_group")
        .def(init<parse_string&, optional<bool, bool, bool, int> >((
          arg("hall_symbol"),
          arg("pedantic")=false,
          arg("no_centring_type_symbol")=false,
          arg("no_expand")=false,
          arg("t_den")=sg_t_den)))
        .def(init<std::string const&, optional<bool, bool, bool, int> >((
          arg("hall_symbol"),
          arg("pedantic")=false,
          arg("no_centring_type_symbol")=false,
          arg("no_expand")=false,
          arg("t_den")=sg_t_den)))
        .def(init<space_group_symbols const&, optional<int> >((
          arg("space_group_symbols"),
          arg("t_den")=sg_t_den)))
        .def(init<space_group const&>((arg("other"))))
        .def("reset", &w_t::reset, (arg("t_den")=sg_t_den))
        .def("expand_ltr", &w_t::expand_ltr, return_self<>(), (arg("new_t")))
        .def("expand_inv", &w_t::expand_inv,
          return_self<>(), (arg("new_inv_t")))
        .def("expand_smx", (space_group&(w_t::*)(rt_mx const&))
          &w_t::expand_smx, return_self<>(), (arg("new_smx")))
        .def("expand_smx", (space_group&(w_t::*)(std::string const&))
          &w_t::expand_smx, return_self<>(), (arg("smx_symbol")))
        .def("expand_conventional_centring_type",
          &w_t::expand_conventional_centring_type, (arg("symbol")))
        .def("parse_hall_symbol", &w_t::parse_hall_symbol, (
          arg("hall_symbol"),
          arg("pedantic")=false,
          arg("no_centring_type_symbol")=false))
        .def("change_basis", &w_t::change_basis, (arg("cb_op")))
        .def("change_of_origin_realising_origin_centricity",
             &w_t::change_of_origin_realising_origin_centricity)
        .def("r_den", &w_t::r_den)
        .def("t_den", &w_t::t_den)
        .def("order_p", &w_t::order_p)
        .def("order_z", &w_t::order_z)
        .def("__len__", &w_t::order_z)
        .def("n_equivalent_positions", &w_t::n_equivalent_positions)
        .def("n_ltr", &w_t::n_ltr)
        .def("is_centric", (bool(w_t::*)() const) &w_t::is_centric)
        .def("is_origin_centric", &w_t::is_origin_centric)
        .def("f_inv", &w_t::f_inv)
        .def("n_smx", &w_t::n_smx)
        .def("__call__", call_3, (
          arg("i_ltr"),
          arg("i_inv"),
          arg("i_smx")))
        .def("__call__", getitem, (arg("i_op")))
        .def("__getitem__", getitem)
        .def("make_tidy", &w_t::make_tidy, return_self<>())
        .def("is_tidy", &w_t::is_tidy)
        .def("contains", &w_t::contains, (arg("smx")))
        .def("__contains__", &w_t::contains)
        .def("__eq__", &w_t::operator==)
        .def("__ne__", &w_t::operator!=)
        .def("__hash__", py_hash)
        .def("conventional_centring_type_symbol",
          &w_t::conventional_centring_type_symbol)
        .def("z2p_op", &w_t::z2p_op, (
          arg("r_den")=cb_r_den,
          arg("t_den")=cb_t_den))
        .def("construct_z2p_op", &w_t::construct_z2p_op, (
          arg("r_den")=cb_r_den,
          arg("t_den")=cb_t_den))
        .def("is_chiral", &w_t::is_chiral)
        .def("is_sys_absent",
          (bool(w_t::*)(miller::index<> const&) const)
          &w_t::is_sys_absent, (arg("miller_index")))
        .def("is_sys_absent",
          (af::shared<bool>(w_t::*)
            (af::const_ref<miller::index<> > const&) const)
          &w_t::is_sys_absent, (arg("miller_indices")))
        .def("is_centric",
          (bool(w_t::*)(miller::index<> const&) const)
          &w_t::is_centric, (arg("miller_index")))
        .def("is_centric",
          (af::shared<bool>(w_t::*)
            (af::const_ref<miller::index<> > const&) const)
          &w_t::is_centric, (arg("miller_indices")))
        .def("phase_restriction", &w_t::phase_restriction, (
          arg("miller_index")))
        .def("is_valid_phase", &w_t::is_valid_phase, (
          arg("miller_index"),
          arg("phi"),
          arg("deg")=false,
          arg("tolerance")=1e-5))
        .def("nearest_valid_phases", &w_t::nearest_valid_phases, (
          arg("miller_indices"),
          arg("phases"),
          arg("deg")=false))
        .def("multiplicity",
          (int(w_t::*)(miller::index<> const&, bool) const)
          &w_t::multiplicity, (arg("miller_index")))
        .def("multiplicity",
          (af::shared<int>(w_t::*)
            (af::const_ref<miller::index<> > const&, bool) const)
          &w_t::multiplicity, (arg("miller_indices")))
        .def("epsilon",
          (int(w_t::*)(miller::index<> const&) const)
          &w_t::epsilon, (arg("miller_index")))
        .def("epsilon",
          (af::shared<int>(w_t::*)
            (af::const_ref<miller::index<> > const&) const)
          &w_t::epsilon, (arg("miller_indices")))
        .def("multiplicity",
          (int(w_t::*)(vec3_rat const&) const)
            &w_t::multiplicity,
              (arg("site")))
        .def("average_unit_cell", &w_t::average_unit_cell, (
          arg("unit_cell")))
        .def("is_compatible_unit_cell", &w_t::is_compatible_unit_cell, (
          arg("unit_cell"),
          arg("relative_length_tolerance")=0.01,
          arg("absolute_angle_tolerance")=1.))
        .def("average_u_star",
          (scitbx::sym_mat3<double>(w_t::*)(
            scitbx::sym_mat3<double> const&) const) &w_t::average_u_star, (
          arg("u_star")))
        .def("build_derived_acentric_group",
          &w_t::build_derived_acentric_group)
        .def("build_derived_group",
           &w_t::build_derived_group)
        .def("build_derived_reflection_intensity_group",
          &w_t::build_derived_reflection_intensity_group, (
           arg("anomalous_flag")))
        .def("build_derived_patterson_group",
          &w_t::build_derived_patterson_group)
        .def("build_derived_point_group",
          &w_t::build_derived_point_group)
        .def("build_derived_laue_group",
          &w_t::build_derived_laue_group)
        .def("point_group_type", &w_t::point_group_type)
        .def("laue_group_type", &w_t::laue_group_type)
        .def("crystal_system", &w_t::crystal_system)
        .def("match_tabulated_settings", &w_t::match_tabulated_settings)
        .def("gridding", &w_t::gridding)
        .def("refine_gridding",
          (sg_vec3(w_t::*)(sg_vec3 const&) const)
          &w_t::refine_gridding, (arg("grid")))
        .def("all_ops", &w_t::all_ops, (arg("mod")=0, arg("cancel")=false))
        .def("unique", &w_t::unique, (arg("special_op")))
        .def("type", &w_t::type)
        .def_pickle(space_group_wrappers())
      ;
    }
  };

} // namespace <anoymous>

  void wrap_space_group()
  {
    space_group_wrappers::wrap();
  }

}}} // namespace cctbx::sgtbx::boost_python


// Compile with cmake (CMakeLists.txt is provided) or with the following lines in bash:
// g++ -c -fPIC libautosim.cpp -o libautosim.o
// g++ -shared -Wl,-soname,libautosim.so -o libautosim.so libautosim.o

#include <string>
#include <sstream>
#include <iostream>
#include <list>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <algorithm>
#include <float.h>
#include <limits>
#include <cmath>
#include <limits>

#include <bitset>
#include <boost/math/distributions/binomial.hpp>
#include "cpp_libs/libOrsa/orsa_fundamental.hpp"
#include "cpp_libs/libOrsa/orsa_homography.hpp"

#define FullDescDim 6272
#define VecDescDim 128 // as in (x,x,128) output of the network
#define SameKPThres 4

struct TargetNode
{
  const int TargetIdx; float sim_with_query;
  TargetNode(int Idx, float sim):TargetIdx(Idx){ this->sim_with_query = sim; };
  bool operator >(const TargetNode & kp) {return ( this->sim_with_query>kp.sim_with_query );};
  bool operator ==(const TargetNode & kp) {return ( this->sim_with_query==kp.sim_with_query );};
};

struct QueryNode
{
  const int QueryIdx;
  std::list<QueryNode>::iterator thisQueryNodeOnList; //pointer on list
  float first_sim, last_sim;
  std::list<TargetNode> MostSimilar_TargetNodes;

  void Add_TargetNode(int it, float sim, int MaxTnodes_num)
  {
    TargetNode tn(it,sim);
    std::list<TargetNode>::iterator target_iter;
    for(target_iter = MostSimilar_TargetNodes.begin(); target_iter != MostSimilar_TargetNodes.end(); ++target_iter)
      if ( tn > *target_iter )
        break;

    MostSimilar_TargetNodes.insert( target_iter, tn );
    if (MaxTnodes_num>0 && MostSimilar_TargetNodes.size()>MaxTnodes_num)
      MostSimilar_TargetNodes.pop_back();
      last_sim = (--MostSimilar_TargetNodes.end())->sim_with_query;
    first_sim = MostSimilar_TargetNodes.begin()->sim_with_query;
  };

  QueryNode(int Idx):QueryIdx(Idx){ first_sim = -1; last_sim = -1; };
};

struct DescStatsClass
{
  float norm, max, min, mean, sigma;
  std::bitset<FullDescDim> AID;
  std::bitset<VecDescDim> AIDbyVec [FullDescDim/VecDescDim];

  DescStatsClass()
  {
    norm = 0.0f, mean = 0.0f, sigma = 0.0f;
    max = -std::numeric_limits<float>::infinity();
    min = std::numeric_limits<float>::infinity();
  };
};

int CountItFast(const DescStatsClass & q, const DescStatsClass & t, int thres)
{
  int xor_opp = 0;
  for (int v = 0; (v < FullDescDim/VecDescDim && (xor_opp < thres)); v++)
    xor_opp +=  (q.AIDbyVec[v] ^ t.AIDbyVec[v]).count();
  return(xor_opp);
}

float FastSimi(int iq, float* DescsQuery, DescStatsClass* QueryStats, int it, float* DescsTarget, DescStatsClass* TargetStats, float simi_thres)
{
  float m_sq = QueryStats[iq].max * TargetStats[it].max;
  float norms_prod = QueryStats[iq].norm * TargetStats[it].norm;
  float dynamic_thres = (simi_thres * norms_prod) - FullDescDim * m_sq;
  float SP = 0.0;
  int qpos = iq*FullDescDim, tpos = it*FullDescDim;
  for (int i = 0; (i < FullDescDim); i++)
  {
    SP +=  DescsQuery[qpos + i] * DescsTarget[tpos + i];
  }
  return(SP/norms_prod);
};

DescStatsClass LoadInfo(int iq, float* DescsQuery)
{
  DescStatsClass ds;
  float val;
  int qpos = iq*FullDescDim;
  for (int i = 0; (i < FullDescDim); i++)
  {
    val = DescsQuery[qpos + i];
    ds.norm += val * val;
  }

  for (int i = 0; (i < FullDescDim); i++)
    if (DescsQuery[qpos + i]>=0)
    {
      ds.AID.set(i);
    }

  for (int v = 0; (v < FullDescDim/VecDescDim); v++)
  {
    int vpos = VecDescDim*v;
    for (int i = 0; (i < VecDescDim); i++)
      {
        if (DescsQuery[qpos + vpos + i]>=0)
          ds.AIDbyVec[v].set(i);
      }
  }

  ds.norm = std::sqrt(ds.norm);
  return(ds);
};

// this gives P(X>=k) when X is a binomial distribution of parameters N and p
float* BinomialSurvivals(int N, float p)
{
  float * survivals = new float[N+1];
  // binomial distribution object:
  boost::math::binomial bino(N,p);
  // survival values
  for (int x=0; x<=N; x++)
    survivals[x] = cdf(complement(bino, x));
  return(survivals);
}

struct KeypointClass{
  float x,y;
};

struct MatchersClass
{
  std::list<QueryNode> QueryNodes;
  const int k; // k as in knn, if <=0 then we store all matches having a sim above sim_thres
  const float sim_thres;
  const int Nvec;
  std::vector<KeypointClass> QueryKPs, TargetKPs;

  int* FilteredIdxMatches;
  int N_FilteredMatches;

  int BinDesc_GetAlarmThreshold(int Nt)
  {
    float *survivals = BinomialSurvivals(VecDescDim, 0.5);
    int alarmThres = 0.0;
    for (int i = 0; i <= VecDescDim; i++)
      if (Nt * survivals[i] <= 1)
      {
        alarmThres = (int)(i * Nvec);
        break;
      }
    return (alarmThres);
  }

  MatchersClass(int knn_num, float sim_thres) : k(knn_num), sim_thres(sim_thres), Nvec((int)(FullDescDim / VecDescDim)){};

  void KnnMatcher(float *DescsQuery, int Nquery, float *DescsTarget, int Ntarget, int FastCode)
  {
    DescStatsClass *QueryStats = new DescStatsClass[Nquery], *TargetStats = new DescStatsClass[Ntarget];
#pragma omp parallel for default(shared)
    for (int iq = 0; iq < Nquery; iq++)
      QueryStats[iq] = LoadInfo(iq, DescsQuery);
#pragma omp parallel for default(shared)
    for (int it = 0; it < Ntarget; it++)
      TargetStats[it] = LoadInfo(it, DescsTarget);

    switch (FastCode)
    {
      case 0:
      { // BigAID
        // std::cout<<"---> Brute Force Angle Comparisons"<<std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          for (int it = 0; it < Ntarget; it++)
          {
            float updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = FastSimi(iq, DescsQuery, QueryStats, it, DescsTarget, TargetStats, updated_sim_thres);
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
  #pragma omp critical
          {
            QueryNodes.push_back(qn);
            std::list<QueryNode>::iterator itqn = --QueryNodes.end();
            itqn->thisQueryNodeOnList = itqn;
          }
        }
        break;
      } // end of BigAID
      case 1:
      { // model new AID
        // std::cout << "---> Full sign comparisons with bitset!!!" << std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          float updated_sim_thres;
          for (int it = 0; it < Ntarget; it++)
          {
            // This is like counting bits after an XNOR opperation on both binary descriptors
            updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = (float) ( FullDescDim - CountItFast(QueryStats[iq], TargetStats[it], FullDescDim - updated_sim_thres) );
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
          {
  #pragma omp critical
            {
              QueryNodes.push_back(qn);
              std::list<QueryNode>::iterator itqn = --QueryNodes.end();
              itqn->thisQueryNodeOnList = itqn;
            }
          }
        }
        break;
      } // end of AID
      case 2:
      { // model AID
        // std::cout << "---> Full sign comparisons with bitset!!!" << std::endl;
  #pragma omp parallel for default(shared)
        for (int iq = 0; iq < Nquery; iq++)
        {
          QueryNode qn(iq);
          for (int it = 0; it < Ntarget; it++)
          {
            // This is like counting bits after an XNOR opperation on both binary descriptors
            int concor = FullDescDim - (QueryStats[iq].AID ^ TargetStats[it].AID).count();
            float updated_sim_thres = (this->k > 0 && qn.last_sim > sim_thres) ? qn.last_sim : sim_thres;
            float simi = (float)concor;
            if (simi > updated_sim_thres)
              qn.Add_TargetNode(it, simi, this->k);
          }
          if (qn.first_sim > sim_thres)
          {
  #pragma omp critical
            {
              QueryNodes.push_back(qn);
              std::list<QueryNode>::iterator itqn = --QueryNodes.end();
              itqn->thisQueryNodeOnList = itqn;
            }
          }
        }
        break;
      } // end of AID
    } // end of the switch
  }

private:
  MatchersClass() : k(0), sim_thres(0.0), Nvec(0){};
};

float max_euclidean_dist(const Match & m1, const Match & m2)
{
  float left_dist = std::sqrt( std::pow(m1.x1-m2.x1,2.0) + std::pow(m1.y1-m2.y1,2.0) );
  float right_dist = std::sqrt( std::pow(m1.x2-m2.x2,2.0) + std::pow(m1.y2-m2.y2,2.0) );
  if (left_dist>right_dist)
    return left_dist;
  else
    return right_dist;
}

std::vector<Match> UniqueFilter(const std::vector<Match>& matches)
{
  std::vector<Match> uniqueM;
  bool *duplicatedM = new bool[matches.size()];
  float best_sim;
  int bestidx;
  for (int i =0; i<matches.size(); i++)
    duplicatedM[i] = false;
  for (int i =0; i<matches.size(); i++)
  {
    if (duplicatedM[i])
      continue;
    best_sim = matches[i].similarity;
    bestidx = i;
    for(int j=i+1; j<matches.size();j++)
    {
      // std::cout<< max_euclidean_dist(matches[i],matches[j])<<std::endl;
      if ( !duplicatedM[j] && max_euclidean_dist(matches[i],matches[j])<SameKPThres )
      {
        duplicatedM[j] = true;
        if (best_sim<matches[j].similarity)
        {
          bestidx = j;
          best_sim = matches[j].similarity;
        }
      }
    }
    uniqueM.push_back(matches[bestidx]);
  }
  return uniqueM;
}

void ORSA_Filter(std::vector<Match>& matches, bool* MatchMask, float* T, int w1,int h1,int w2,int h2, bool Fundamental, const double & precision, bool verb)
{
  libNumerics::matrix<double> H(3,3);
  std::vector<int> vec_inliers;
  double nfa;
  const float nfa_max = -2;
  const int ITER_ORSA=10000;
  if (Fundamental)
    orsa::orsa_fundamental(matches, w1,h1,w2,h2, precision, ITER_ORSA,H, vec_inliers,nfa,verb);
  else
     orsa::ORSA_homography(matches, w1,h1,w2,h2, precision, ITER_ORSA,H, vec_inliers,nfa,verb);

  for (int cc = 0; cc < matches.size(); cc++ )
    MatchMask[cc] = false;

  if ( nfa < nfa_max )
  {
    for (int vi = 0; vi < vec_inliers.size(); vi++ )
      MatchMask[vec_inliers[vi]] = true;
    H /= H(2,2);

    int t = 0;
    for(int i = 0; i < H.nrow(); ++i)
        for (int j = 0; j < H.ncol(); ++j)
            T[t++] = H(i,j);
    if (verb)
    {
        printf("The two images match! %d matchings are identified. log(nfa)=%.2f.\n", (int) vec_inliers.size(), nfa);
        if (Fundamental)
          std::cout << "*************** Fundamental **************"<< std::endl;
        else
          std::cout << "*************** Homography ***************"<< std::endl;
        std::cout << H <<std::endl;
        std::cout << "******************************************"<< std::endl;
    }
  }
  else
  {
    if (verb)
        printf("The two images do not match. The matching is not significant:  log(nfa)=%.2f.\n", nfa);
  }
}

// Define C functions for the C++ class - as ctypes can only talk to C...
extern "C"
{

  int NumberOfFilteredMatches(MatchersClass* M)
  {
    return M->N_FilteredMatches;
  }

  void ArrayOfFilteredMatches(MatchersClass* M, int* arr)
  {
    for (int i=0; i<3*M->N_FilteredMatches;i++)
      arr[i] = M->FilteredIdxMatches[i];
  }

  void GeometricFilterFromNodes(MatchersClass* M, float* T, int w1,int h1,int w2,int h2, int type, float precision, bool verb)
  {
    std::vector<Match> matches;
    for(std::list<QueryNode>::const_iterator iq = M->QueryNodes.begin(); iq != M->QueryNodes.end(); ++iq)
      for(std::list<TargetNode>::const_iterator it = iq->MostSimilar_TargetNodes.begin(); it != iq->MostSimilar_TargetNodes.end(); ++it)
    {
      Match match1;
      match1.x1 = M->QueryKPs[iq->QueryIdx].x;
      match1.y1 = M->QueryKPs[iq->QueryIdx].y;
      match1.x2 = M->TargetKPs[it->TargetIdx].x;
      match1.y2 = M->TargetKPs[it->TargetIdx].y;
      match1.similarity = it->sim_with_query;
      match1.Qidx = iq->QueryIdx;
      match1.Tidx = it->TargetIdx;
      matches.push_back(match1);
    }

    matches = UniqueFilter(matches);
    bool* MatchMask = new bool[matches.size()];
    switch (type)
    {
      case 0: //Homography
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, false, (double)precision, verb);
        break;
      }
      case 1: // Fundamental
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, true, (double)precision, verb);
        break;
      }
    }

    M->N_FilteredMatches = 0;
    for (int cc = 0; cc < matches.size(); cc++ )
      if (MatchMask[cc])
        M->N_FilteredMatches++;
    M->FilteredIdxMatches = new int[3*M->N_FilteredMatches];
    int fcc = 0;
    for (int cc = 0; cc < matches.size(); cc++ )
      if (MatchMask[cc])
        {
          M->FilteredIdxMatches[3*fcc] = matches[cc].Qidx;
          M->FilteredIdxMatches[3*fcc+1] = matches[cc].Tidx;
          M->FilteredIdxMatches[3*fcc+2] = matches[cc].similarity;
          fcc++;
        }
  }

  void GeometricFilter(float* scr_pts, float* dts_pts, bool* MatchMask, float* T, int N, int w1,int h1,int w2,int h2, int type, float precision, bool verb)
  {
    std::vector<Match> matches;
    for (int cc = 0; cc < N; cc++ )
    {
        Match match1;
        match1.x1 = scr_pts[cc*2];
        match1.y1 = scr_pts[cc*2+1];
        match1.x2 = dts_pts[cc*2];
        match1.y2 = dts_pts[cc*2+1];

        matches.push_back(match1);
    }

    switch (type)
    {
      case 0: //Homography
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, false, (double)precision, verb);
        break;
      }
      case 1: // Fundamental
      {
        ORSA_Filter(matches, MatchMask, T, w1, h1, w2, h2, true, (double)precision, verb);
        break;
      }
    }
  }

  MatchersClass* newMatcher(int k, int full_desc_dim, float sim_thres)
  {
    if (full_desc_dim!=FullDescDim)
      std::cout<<"Desc dims don't match ("<<full_desc_dim<<"!="<<FullDescDim<<")"<<std::endl;
    return ( new MatchersClass(k, (float) sim_thres) );
  }

  void KnnMatcher(MatchersClass* M, float* Query_pts, float* DescsQuery, int Nquery, float* Target_pts, float* DescsTarget, int Ntarget, int FastCode)
  {
    M->QueryNodes.clear();
    M->QueryKPs.clear();
    M->TargetKPs.clear();
    KeypointClass kp;
    for (int cc = 0; cc < Nquery; cc++ ){
      kp.x = Query_pts[cc*2]; kp.y = Query_pts[cc*2+1];
      M->QueryKPs.push_back( kp );
    }
    for (int cc = 0; cc < Ntarget; cc++ ){
      kp.x = Target_pts[cc*2]; kp.y = Target_pts[cc*2+1];
      M->TargetKPs.push_back( kp );
    }

    M->KnnMatcher(DescsQuery, Nquery, DescsTarget, Ntarget, FastCode);
  }

  int GetQueryNodeLength(QueryNode* qn)
  {
    return(qn->MostSimilar_TargetNodes.size());
  }

  QueryNode* LastQueryNode(MatchersClass* M)
  {
    if (M->QueryNodes.begin()!=M->QueryNodes.end())
      return(&*(--M->QueryNodes.end()));
    else
      return(0);
  }

  QueryNode* FirstQueryNode(MatchersClass* M)
  {
    if (M->QueryNodes.begin()!=M->QueryNodes.end())
      return(&*M->QueryNodes.begin());
    else
      return(0);
  }

  QueryNode* NextQueryNode(MatchersClass* M, QueryNode* qn)
  {
    if (qn!=0 && ++qn->thisQueryNodeOnList!=M->QueryNodes.end())
      return(&*(++qn->thisQueryNodeOnList));
    else
      return(0);
  }

  QueryNode* PrevQueryNode(MatchersClass* M, QueryNode* qn)
  {
    if (qn!=0 && qn->thisQueryNodeOnList!=M->QueryNodes.begin())
      return(&*(--qn->thisQueryNodeOnList));
    else
      return(0);
  }

  void GetData_from_QueryNode(QueryNode* qn, int* QueryIdx, int *TargetIdxes, float* simis)
  {
    QueryIdx[0] = qn->QueryIdx;
    int i = 0;
    for(std::list<TargetNode>::const_iterator it = qn->MostSimilar_TargetNodes.begin(); it != qn->MostSimilar_TargetNodes.end(); ++it)
    {
      TargetIdxes[i] = it->TargetIdx;
      simis[i] = it->sim_with_query;
      i++;
    }
  }

  void FastMatCombi(int N, float* bP, int* i1_list, int *i2_list, float *patches1, float *patches2, int MemStepImg, int* last_i1_list, int *last_i2_list)
  {
    int MemStepBlock = 2*MemStepImg;
    #pragma omp parallel for firstprivate(MemStepImg, MemStepBlock, N)
    for (int k = 0; k<N; k++)
    {
      int i1 = i1_list[k];
      int i2 = i2_list[k];

      if (last_i1_list[k]!=i1)
        for (int i = 0; i<MemStepImg;i++)
          bP[k*MemStepBlock + 2*i] = patches1[i1*MemStepImg + i];

      if (last_i2_list[k]!=i2)
        for (int i = 0; i<MemStepImg;i++)
          bP[k*MemStepBlock + 2*i+1] = patches2[i2*MemStepImg + i];
    }
  }
}


//
//   Copyright 2019 University of Sheffield
//
//   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.

#include "elasticregistration.hpp"

#include <boost/format.hpp>

#include "basewriter.hpp"
#include "exceptions.hpp"
#include "file_utils.hpp"
#include "image.hpp"
#include "mapmanager.hpp"
#include "mask.hpp"
#include "math_utils.hpp"
#include "tmatrix.hpp"

ElasticRegistration::ElasticRegistration(std::shared_ptr<Image> fixed, std::shared_ptr<Image> moved,
                                         std::shared_ptr<Mask> mask, const intcoord& target_spacing,
                                         const ConfigurationBase& config)
  : _comm(fixed->comm()), _fixed(std::move(fixed)), _moved(std::move(moved)), _mask(std::move(mask)),
    _target_spacing(target_spacing), _configuration(config),
    _mapmanager(std::make_unique<MapManager>(target_spacing, *_mask)),
    _registered(GridVariable::copy(*_moved))
{
}

void ElasticRegistration::set_initial_guess(const MapBase& initial_guess __attribute__((unused)))
{
  // Query manager for map closest in shape to initial_guess
  // Set Mapmanager index to that shape
  // Interpolate initial_guess to current map
  // Carry on from there...
  throw InternalError("Not yet implemented");
}

void ElasticRegistration::autoregister(integer max_iterations)
{
  for (_current_map = _mapmanager->get_current_map(); _current_map != nullptr;
       _current_map = _mapmanager->get_next_map())
  {
    registration_inner_loop(max_iterations);
  }
  _current_map = _mapmanager->get_current_map(); // Replace nullptr with final valid map
}

void ElasticRegistration::registration_inner_loop(integer max_iterations)
{
  integer iteration = 0;
  if (_configuration.grab<bool>("save_intermediate_frames"))
  {
    save_debug_frame(iteration);
  }
  for (iteration = 1; iteration <= max_iterations; iteration++)
  {
    this->_previous_mis.push_back(this->_fixed->mutual_information(*this->_moved));

    // Do solver step, recalculating lambda if it is the first iteration of the loop
    Vec_unique map_delta = solver_step(iteration, iteration == 1);

    if (this->is_converged(*map_delta))
    {
      break;
    }
  }
  if (iteration == max_iterations)
  {
    PetscPrintf(_comm, "Warning: Maximum iteration limit reached (%d)\n", max_iterations);
  }
}

Vec_unique ElasticRegistration::solver_step(floating iternum, bool recalculate_lambda)
{
  auto tmat2_and_tmatfm = build_tmat2_and_tmatfm(*this->_fixed, *_registered, *this->_current_map);
  Mat_unique tmat2 = std::move(tmat2_and_tmatfm.first);
  Vec_unique tmatfm = std::move(tmat2_and_tmatfm.second);

  floating lapl2_diag_sum = diagonal_sum(_current_map->laplacian2());
  floating tmat2_diag_sum = diagonal_sum(*tmat2);
  floating lapl_premult = tmat2_diag_sum / lapl2_diag_sum;
  PetscPrintf(_comm, "lapl_mult = %f, laplsum= %f, tmatsum = %f\n", lapl_premult, lapl2_diag_sum,
              tmat2_diag_sum);

  // Recalculate lambda if requested
  if (recalculate_lambda)
  {
    _lambda = approximate_optimum_lambda(*tmat2, _current_map->laplacian2(), lapl_premult, _lambda,
                                         _initial_search_width, _lambda_search_maxiter, _lambda_min);
    PetscPrintf(_comm, "Calculated lambda = %f\n", _lambda);
  }

  // Add lambda*L^tL to T^t T
  PetscErrorCode perr = MatAXPY(*tmat2, _lambda * lapl_premult, _current_map->laplacian2(),
                                DIFFERENT_NONZERO_PATTERN);
  CHKERRXX(perr);

  // Subtract lambda*L^tL*a from T^t(f-m)
  Vec_unique lambda_l2_a = create_unique_vec();
  perr = VecDuplicate(*tmatfm, lambda_l2_a.get());
  CHKERRXX(perr);
  perr = MatMult(_current_map->laplacian2(), _current_map->displacement_vector(), *lambda_l2_a);
  CHKERRXX(perr);
  //  perr = VecAXPY(*tmatfm, -_lambda * lapl_premult, *lambda_l2_a);
  // CHKERRXX(perr);

  // Co-opt now defunct lambda_l2_a as solution vector
  Vec_unique delta = std::move(lambda_l2_a);

  // Now do actual solve
  KSP_unique m_ksp = create_unique_ksp();
  perr = KSPCreate(_comm, m_ksp.get());
  CHKERRXX(perr);
  perr = KSPSetOperators(*m_ksp, *tmat2, *tmat2);
  CHKERRXX(perr);
  perr = KSPSetUp(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSetFromOptions(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSetUp(*m_ksp);
  CHKERRXX(perr);
  perr = KSPSolve(*m_ksp, *tmatfm, *delta);
  CHKERRXX(perr);

  // update map
  _current_map->update(*delta);

  // warp image
  _registered = _moved->warp(*_current_map);
  _registered->normalize();

  floating mutinf = _registered->mutual_information(*_fixed);
  _previous_mis.push_back(mutinf);
  PetscPrintf(_comm, "Mutual information: %f\n", mutinf);

  if (_configuration.grab<bool>("save_intermediate_frames"))
  {
    save_debug_frame(iternum);
  }

  return delta;
}

bool ElasticRegistration::is_converged(const Vec& map_delta, floating threshold)
{
  // Roll our own because we need to avoid intensity values
  std::array<floating, 2> avg_data;
  auto& avg_sum = avg_data[0];
  auto& avg_num = avg_data[1];

  floating localmax = PETSC_MIN_REAL;
  integer local_lo, local_hi;
  PetscErrorCode perr = VecGetOwnershipRange(map_delta, &local_lo, &local_hi);
  CHKERRXX(perr);
  avg_num = local_hi - local_lo;

  const floating* delta_data;
  perr = VecGetArrayRead(map_delta, &delta_data);
  CHKERRXX(perr);
  for (integer iidx = local_lo; iidx < local_hi; iidx++)
  {
    if (iidx % _current_map->ndof() == 0) // skip intensity values
    {
      continue;
    }
    floating absval = std::abs(delta_data[iidx]);
    localmax = absval > localmax ? absval : localmax;
    avg_sum += absval;
  }
  perr = VecRestoreArrayRead(map_delta, &delta_data);
  CHKERRXX(perr);

  MPI_Allreduce(MPI_IN_PLACE, &localmax, 1, MPIU_SCALAR, MPI_MAX, _comm);
  MPI_Allreduce(MPI_IN_PLACE, avg_data.data(), 2, MPIU_SCALAR, MPI_SUM, _comm);

  floating average = avg_sum / avg_num;

  PetscPrintf(_comm, "Displacement delta: %f / %f (avg / max)\n", average, localmax);

  bool disp_conv = average < threshold;

  // Now calculate gradient of MI over last n iterations
  // First check if we need to discard values
  while (this->_previous_mis.size() > this->_mi_window)
  {
    this->_previous_mis.pop_front();
  }

  // Now calculate simple linear regression
  intvector xpoints(this->_previous_mis.size());
  std::iota(xpoints.begin(), xpoints.end(), 1);
  floating num_points = static_cast<double>(xpoints.size());

  floating s_x = 0.5 * num_points * (num_points + 1);
  floating s_xx = (1.0 / 6.0) * (num_points * (num_points + 1) * (2 * num_points + 1));
  floating s_y = std::accumulate(this->_previous_mis.cbegin(), this->_previous_mis.cend(), 0.);
  floating s_xy = accumulate(xpoints.cbegin(), xpoints.cend(), this->_previous_mis.cbegin(), 0.,
                             [](floating x, integer y, floating z) -> floating { return x + y * z; });

  floating mi_grad = (num_points*s_xy - s_x*s_y)/(num_points*s_xx - s_x*s_x);

  PetscPrintf(_comm, "Mutual Information gradient: %f\n", mi_grad);

  bool mi_conv = false;
  if(this->_previous_mis.size() >= this->_mi_window)
  {
    mi_conv = mi_grad < 0;
  }

  return disp_conv || mi_conv;
}

void ElasticRegistration::save_debug_frame(integer iteration_num)
{
  integer outer_count = _mapmanager->step_number();
  std::ostringstream outname;
  std::string file_str(_configuration.grab<std::string>("intermediate_template"));
  bf::path registered_path(_configuration.grab<std::string>("registered"));

  boost::format pad2("%02d");
  replace_token(file_str, ConfigurationBase::k_outer_token, (pad2 % outer_count).str());

  boost::format pad3("%03d");
  replace_token(file_str, ConfigurationBase::k_inner_token, (pad3 % iteration_num).str());

  replace_token(file_str, ConfigurationBase::k_stem_token, registered_path.filename().stem().string());

  replace_token(file_str, ConfigurationBase::k_extension_token, registered_path.extension().string());

  bf::path output_path(registered_path.parent_path());

  bf::path intermediates_path(_configuration.grab<std::string>("intermediate_directory"));
  if (intermediates_path.is_absolute())
  {
    if (!bf::exists(intermediates_path))
    {
      std::ostringstream errss;
      errss << "Intermediate frame output path " << intermediates_path << " does not exist.";
      throw std::runtime_error(errss.str());
    }
    output_path = intermediates_path;
  }
  else
  {
    // Don't create directories we don't say we will, error instead
    // (Pre-flight checks mean this should never throw)
    if (!output_path.empty() && !bf::exists(output_path))
    {
      std::ostringstream errss;
      errss << "Output path " << output_path << " does not exist.";
      throw std::runtime_error(errss.str());
    }

    // Append intermediates dir
    output_path /= _configuration.grab<std::string>("intermediate_directory");

    // Create intermediates dir as needed
    bf::create_directories(output_path);
  }

  // Finally append filename and any group extension
  output_path /= file_str;
  std::string output_path_str(output_path.string());
  output_path_str.append(":");
  output_path_str.append(_configuration.grab<std::string>("registered_h5_path"));

  BaseWriter_unique wtr = BaseWriter::get_writer_for_filename(output_path_str, _comm);
  wtr->write_image(*_registered);
}

floating ElasticRegistration::approximate_optimum_lambda(const Mat& mat_a, const Mat& mat_b,
                                                         floating lambda_mult, floating initial_guess,
                                                         floating search_width, uinteger max_iter,
                                                         floating lambda_min)
{
  floating x_lo = initial_guess - search_width;
  x_lo = x_lo > lambda_min ? x_lo : lambda_min;
  floating x_mid = initial_guess;
  x_mid = x_mid > x_lo ? x_mid : x_lo + search_width/2;
  floating x_hi = initial_guess + search_width;
  x_hi = x_hi > x_mid ? x_hi : x_mid + search_width;

  floating y_lo(0), y_mid(0), y_hi(0);
  for (uinteger iter = 0; iter < max_iter; iter++)
  {
    Mat_unique mat_c = create_unique_mat();
    PetscErrorCode perr = MatDuplicate(mat_a, MAT_COPY_VALUES, mat_c.get());

    // calculate at lower value
    perr = MatAXPY(*mat_c, lambda_mult * x_lo, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_lo = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    // Calculate at initial guess
    perr = MatAXPY(*mat_c, lambda_mult * x_mid, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_mid = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    // calculate at higher value
    perr = MatAXPY(*mat_c, lambda_mult * x_hi, mat_b, DIFFERENT_NONZERO_PATTERN);
    CHKERRXX(perr);
    y_hi = get_condnum_by_poweriter(*mat_c, 0.01, 100);

    PetscPrintf(_comm, " lo: %g(%g) mid: %g(%g) hi: %g(%g)\n", x_lo, y_lo, x_mid, y_mid, x_hi, y_hi);

    // check initial lower than either side
    if (y_lo >= y_mid)
    {
      // two options, either close to a minimum or need to search to higher x
      if (y_hi < y_mid)
      {
        // All points are negative gradient: keep searching to higher values of x:
        x_mid *= 2;
        x_hi *= 2;
        continue;
      }
      // Otherwise we are close to a local minimum and are done
      break;
    }

    // If we already reached lambda_min we need to be done at that
    if (x_lo == lambda_min)
    {
      break;
    }

    // Otherwise it is a positive gradient so keep searching left
    x_lo = x_lo >= 2.0 * lambda_min ? x_lo / 2 : 1.0 * lambda_min;
    x_mid = x_mid >= 2.0 * x_lo ? x_mid / 2 : x_lo + 1.0;
    x_hi = x_hi >= 2.0 * x_mid ? x_hi / 2 : x_mid + 1.0;
  }
  // fit quadratic and return minimum
  floating a, b, c;
  quadratic_from_points(x_lo, x_mid, x_hi, y_lo, y_mid, y_hi, a, b, c);

  if (a < 0)
  {
    PetscPrintf(_comm, "Warning: convex function in smoothing parameter estimation.\n");
    return lambda_min;
  }

  floating x, y;
  quadratic_vertex(a, b, c, x, y);

  if (x < lambda_min)
  {
    return lambda_min;
  }

  return x;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015, 2017, 2018.
// Modifications copyright (c) 2015-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include <boost/geometry/strategies/cartesian/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/envelope.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/strategies/side.hpp>

#include <boost/geometry/algorithms/detail/equals/point_point.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies:
    left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_triangle
{
    template <typename Policy>
    struct eps_policy
    {
        eps_policy() {}
        template <typename Type>
        eps_policy(Type const& a, Type const& b, Type const& c, Type const& d)
            : policy(a, b, c, d)
        {}
        Policy policy;
    };

    struct eps_empty
    {
        eps_empty() {}
        template <typename Type>
        eps_empty(Type const&, Type const&, Type const&, Type const&) {}
    };

public :
    typedef strategy::envelope::cartesian<CalculationType> envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return envelope_strategy_type();
    }

    typedef strategy::disjoint::segment_box disjoint_strategy_type;

    static inline disjoint_strategy_type get_disjoint_strategy()
    {
        return disjoint_strategy_type();
    }

    typedef strategy::within::cartesian_point_point equals_point_point_strategy_type;
    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return equals_point_point_strategy_type();
    }

    // Template member function, because it is not always trivial
    // or convenient to explicitly mention the typenames in the
    // strategy-struct itself.

    // Types can be all three different. Therefore it is
    // not implemented (anymore) as "segment"

    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P,
        typename EpsPolicy
    >
    static inline
    PromotedType side_value(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & eps_policy)
    {
        CoordinateType const x = get<0>(p);
        CoordinateType const y = get<1>(p);

        CoordinateType const sx1 = get<0>(p1);
        CoordinateType const sy1 = get<1>(p1);
        CoordinateType const sx2 = get<0>(p2);
        CoordinateType const sy2 = get<1>(p2);

        PromotedType const dx = sx2 - sx1;
        PromotedType const dy = sy2 - sy1;
        PromotedType const dpx = x - sx1;
        PromotedType const dpy = y - sy1;

        eps_policy = EpsPolicy(dx, dy, dpx, dpy);

        return geometry::detail::determinant<PromotedType>
                (
                    dx, dy,
                    dpx, dpy
                );

    }

    template
    <
        typename CoordinateType,
        typename PromotedType,
        typename P1,
        typename P2,
        typename P
    >
    static inline
    PromotedType side_value(P1 const& p1, P2 const& p2, P const& p)
    {
        eps_empty dummy;
        return side_value<CoordinateType, PromotedType>(p1, p2, p, dummy);
    }

    template
    <
        typename CoordinateType,
        typename PromotedType,
        bool AreAllIntegralCoordinates
    >
    struct compute_side_value
    {
        template <typename P1, typename P2, typename P, typename EpsPolicy>
        static inline PromotedType apply(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & epsp)
        {
            return side_value<CoordinateType, PromotedType>(p1, p2, p, epsp);
        }
    };

    template <typename CoordinateType, typename PromotedType>
    struct compute_side_value<CoordinateType, PromotedType, false>
    {
        template <typename P1, typename P2, typename P, typename EpsPolicy>
        static inline PromotedType apply(P1 const& p1, P2 const& p2, P const& p, EpsPolicy & epsp)
        {
            // For robustness purposes, first check if any two points are
            // the same; in this case simply return that the points are
            // collinear
            if (equals_point_point(p1, p2)
                || equals_point_point(p1, p)
                || equals_point_point(p2, p))
            {
                return PromotedType(0);
            }

            // The side_by_triangle strategy computes the signed area of
            // the point triplet (p1, p2, p); as such it is (in theory)
            // invariant under cyclic permutations of its three arguments.
            //
            // In the context of numerical errors that arise in
            // floating-point computations, and in order to make the strategy
            // consistent with respect to cyclic permutations of its three
            // arguments, we cyclically permute them so that the first
            // argument is always the lexicographically smallest point.

            typedef compare::cartesian<compare::less> less;

            if (less::apply(p, p1))
            {
                if (less::apply(p, p2))
                {
                    // p is the lexicographically smallest
                    return side_value<CoordinateType, PromotedType>(p, p1, p2, epsp);
                }
                else
                {
                    // p2 is the lexicographically smallest
                    return side_value<CoordinateType, PromotedType>(p2, p, p1, epsp);
                }
            }

            if (less::apply(p1, p2))
            {
                // p1 is the lexicographically smallest
                return side_value<CoordinateType, PromotedType>(p1, p2, p, epsp);
            }
            else
            {
                // p2 is the lexicographically smallest
                return side_value<CoordinateType, PromotedType>(p2, p, p1, epsp);
            }
        }
    };

    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename coordinate_type<P1>::type coordinate_type1;
        typedef typename coordinate_type<P2>::type coordinate_type2;
        typedef typename coordinate_type<P>::type coordinate_type3;

        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                coordinate_type1, coordinate_type2
                            >::type,
                        coordinate_type3
                    >::type,
                CalculationType
            >::type coordinate_type;

        // Promote float->double, small int->int
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        bool const are_all_integral_coordinates =
            boost::is_integral<coordinate_type1>::value
            && boost::is_integral<coordinate_type2>::value
            && boost::is_integral<coordinate_type3>::value;

        eps_policy< math::detail::equals_factor_policy<promoted_type> > epsp;
        promoted_type s = compute_side_value
            <
                coordinate_type, promoted_type, are_all_integral_coordinates
            >::apply(p1, p2, p, epsp);

        promoted_type const zero = promoted_type();
        return math::detail::equals_by_policy(s, zero, epsp.policy) ? 0
            : s > zero ? 1
            : -1;
    }

private:
    template <typename P1, typename P2>
    static inline bool equals_point_point(P1 const& p1, P2 const& p2)
    {
        typedef equals_point_point_strategy_type strategy_t;
        return geometry::detail::equals::equals_point_point(p1, p2, strategy_t());
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef side_by_triangle<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP


#pragma once
//===----------------------------------------------------------------------===//
#include "part.hpp"

#include <dtl/bitmap/iterator.hpp>
#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>
#include <memory>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Applies a fixed size partitioning to the given bitmap.
/// Updates are forwarded to the internally used bitmap type. This requires the
/// bitmap type to be updatable.
template<
    /// The (compressed) bitmap type.
    typename B,
    /// The partition size in bits.
    std::size_t P>
class part_upforward
    : public part<B,P> {
public:
  /// C'tor (similar to all other implementations)
  explicit part_upforward(const boost::dynamic_bitset<$u32>& bitmap)
      : part<B,P>(bitmap) {}

  part_upforward(const part_upforward& other) = delete;
  part_upforward(part_upforward&& other) noexcept = default;
  part_upforward& operator=(const part_upforward& other) = delete;
  part_upforward& operator=(part_upforward&& other) noexcept = default;

  /// Return the name of the implementation.
  static std::string
  name() noexcept {
    return std::string("part_upforward<")
        + B::name() + std::string(",")
        + std::to_string(dtl::log_2(P))
        + std::string(">");
  }

  /// Set the i-th bit to the given value.
  void __forceinline__
  set(std::size_t i, u1 val) noexcept {
    // Forward the call.
    const auto part_idx = i / P;
    this->parts_[part_idx]->set(i % P, val);
  }

  /// Apply the pending updates and clear the diff.
  template<typename M>
  void
  merge() {
    for (std::size_t part_idx = 0; part_idx < this->parts_.size(); ++part_idx) {
      this->parts_[part_idx]->template merge<M>();
    }
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl