/*
Copyright (c) 2019 TOYOTA MOTOR CORPORATION
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted (subject to the limitations in the disclaimer
below) provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its contributors may be used
  to endorse or promote products derived from this software without specific
  prior written permission.

NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Pose.h>
#include <gazebo_msgs/GetModelState.h>

#include <Eigen/Dense>

#include <iostream>
#include <string>
#include <cmath>

std::string target_model_name;
double radius = 1.0;
double radius2 = 1.0;
const double max_area_size = 50.0;  // max area size is 50.0[meter] square
double step_size = 0.1;
int step_count = 0;
bool first_loop = true;
double area_offset = 0.0;

Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> reached_region;

int main(int argc, char **argv)
{
    // initialize ROS node
    ros::init(argc, argv, "reached_region_calculator");
    ros::NodeHandle n("~");

    if (n.getParam("target_model_name", target_model_name)) {
        ROS_INFO("target_model_name is defined as: %s", target_model_name.c_str());
    } else {
        ROS_ERROR("Failed to get param 'target_model_name' use default 'hsrb'");
        target_model_name = "hsrb";
    }

    if (n.getParam("radius", radius)) {
        ROS_INFO("radius is defined as: %f", radius);
    } else {
        ROS_ERROR("Failed to get param 'radius' use default %f", radius);
    }
    radius2 = radius * radius;

    if (n.getParam("step_size", step_size)) {
        ROS_INFO("step size is defined as: %f", step_size);
    } else {
        ROS_ERROR("Failed to get param 'step_size' use default %f", step_size);
    }

    // initialise reached region map
    step_count = (int)round(max_area_size * 2.0 / step_size);
    reached_region.resize(step_count, step_count);
    for (int y = 0; y < step_count; y++) {
        for (int x = 0; x < step_count; x++) {
            reached_region(x, y) = false;
        }
    }

    ros::Publisher pub = n.advertise<std_msgs::Float32>("area_size", 1000);
    ros::Rate rate(10);

    ros::service::waitForService("/gazebo/get_model_state");
    ros::ServiceClient getModelState = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    gazebo_msgs::GetModelState model_state;
    model_state.request.model_name = target_model_name;

    while (ros::ok()) {
        getModelState.call(model_state);
        double x = model_state.response.pose.position.x;
        double y = model_state.response.pose.position.y;
        // fill the reached region map with circle
        for (double dy = -radius; dy <= radius; dy += step_size) {
            for (double dx = -radius; dx <= radius; dx += step_size) {
                if (dx * dx + dy * dy <= radius2) {
                    int ix = (int)round((x + dx + max_area_size) / step_size);
                    if (ix < 0) ix = 0;
                    if (ix >= step_count) ix = step_count - 1;
                    int iy = (int)round((y + dy + max_area_size) / step_size);
                    if (iy < 0) iy = 0;
                    if (iy >= step_count) iy = step_count - 1;
                    reached_region(ix, iy) = true;
                }
            }
        }
        int count = 0;
        for (int y = 0; y < step_count; y++) {
            for (int x = 0; x < step_count; x++) {
                if (reached_region(x, y) == true) {
                    count++;
                }
            }
        }
        double area_size = ((double)count) * step_size * step_size - area_offset;
        if (first_loop) {
            area_offset = area_size;
            first_loop = false;
        } else {
            std_msgs::Float32 msg;
            msg.data = area_size;
            pub.publish(msg);
        }
        ros::spinOnce();
        rate.sleep();
    }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <utility>
#include <vector>

#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Tensor/Slice.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/ElementMap.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/DirectionMap.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Domain/Structure/Neighbors.hpp"
#include "Evolution/DgSubcell/SliceData.hpp"
#include "Evolution/Systems/Burgers/FiniteDifference/Reconstructor.hpp"
#include "Evolution/Systems/Burgers/Tags.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"

namespace TestHelpers {
/*!
 * \brief Defines functions useful for testing Burgers subcell
 */
namespace Burgers::fd {
template <typename F>
FixedHashMap<maximum_number_of_neighbors(1),
             std::pair<Direction<1>, ElementId<1>>, std::vector<double>,
             boost::hash<std::pair<Direction<1>, ElementId<1>>>>
compute_neighbor_data(
    const Mesh<1>& subcell_mesh,
    const tnsr::I<DataVector, 1, Frame::ElementLogical>& volume_logical_coords,
    const DirectionMap<1, Neighbors<1>>& neighbors,
    const size_t ghost_zone_size, const F& compute_variables_of_neighbor_data) {
  FixedHashMap<maximum_number_of_neighbors(1),
               std::pair<Direction<1>, ElementId<1>>, std::vector<double>,
               boost::hash<std::pair<Direction<1>, ElementId<1>>>>
      neighbor_data{};
  for (const auto& [direction, neighbors_in_direction] : neighbors) {
    REQUIRE(neighbors_in_direction.size() ==
            1);  // currently only support one neighbor in each direction
    const ElementId<1>& neighbor_id = *neighbors_in_direction.begin();

    auto neighbor_logical_coords = volume_logical_coords;
    neighbor_logical_coords.get(direction.dimension()) +=
        direction.sign() * 2.0;
    const auto neighbor_vars_for_reconstruction =
        compute_variables_of_neighbor_data(neighbor_logical_coords);

    DirectionMap<1, bool> directions_to_slice{};
    directions_to_slice[direction.opposite()] = true;
    const auto sliced_data = evolution::dg::subcell::detail::slice_data_impl(
        gsl::make_span(neighbor_vars_for_reconstruction.data(),
                       neighbor_vars_for_reconstruction.size()),
        subcell_mesh.extents(), ghost_zone_size, directions_to_slice);
    REQUIRE(sliced_data.size() == 1);
    REQUIRE(sliced_data.contains(direction.opposite()));
    neighbor_data[std::pair{direction, neighbor_id}] =
        sliced_data.at(direction.opposite());
  }
  return neighbor_data;
}

template <typename Reconstructor>
void test_reconstructor(const size_t num_pts,
                        const Reconstructor& derived_reconstructor) {
  // 1. create U that will be reconstructed, being linear to x
  // 2. send through reconstruction
  // 3. check if U was reconstructed correctly

  const ::Burgers::fd::Reconstructor& reconstructor = derived_reconstructor;
  static_assert(tmpl::list_contains_v<
                typename ::Burgers::fd::Reconstructor::creatable_classes,
                Reconstructor>);

  // create an element and its neighbor elements
  DirectionMap<1, Neighbors<1>> neighbors{};
  for (size_t i = 0; i < 2; ++i) {
    neighbors[gsl::at(Direction<1>::all_directions(), i)] =
        Neighbors<1>{{ElementId<1>{i + 1, {}}}, {}};
  }
  const Element<1> element{ElementId<1>{0, {}}, neighbors};

  // a simple, linear form of the scalar field U for testing purpose
  //   * U(x)   = x + 1       (for 1D)
  const auto compute_solution = [](const auto& coords) {
    Variables<tmpl::list<::Burgers::Tags::U>> vars{get<0>(coords).size(), 0.0};
    get(get<::Burgers::Tags::U>(vars)) += coords.get(0) + 1.0;
    return vars;
  };

  // compute neighbor data
  const Mesh<1> subcell_mesh{num_pts, Spectral::Basis::FiniteDifference,
                             Spectral::Quadrature::CellCentered};
  auto logical_coords = logical_coordinates(subcell_mesh);
  const FixedHashMap<maximum_number_of_neighbors(1),
                     std::pair<Direction<1>, ElementId<1>>, std::vector<double>,
                     boost::hash<std::pair<Direction<1>, ElementId<1>>>>
      neighbor_data = compute_neighbor_data(
          subcell_mesh, logical_coords, element.neighbors(),
          reconstructor.ghost_zone_size(), compute_solution);

  // create Variables on lower and upper faces to perform reconstruction
  using dg_package_data_argument_tags = tmpl::list<
      ::Burgers::Tags::U,
      ::Tags::Flux<::Burgers::Tags::U, tmpl::size_t<1>, Frame::Inertial>>;
  const size_t reconstructed_num_pts = subcell_mesh.number_of_grid_points() + 1;
  std::array<Variables<dg_package_data_argument_tags>, 1> vars_on_lower_face =
      make_array<1>(
          Variables<dg_package_data_argument_tags>(reconstructed_num_pts));
  std::array<Variables<dg_package_data_argument_tags>, 1> vars_on_upper_face =
      make_array<1>(
          Variables<dg_package_data_argument_tags>(reconstructed_num_pts));

  Variables<tmpl::list<::Burgers::Tags::U>> volume_vars{
      subcell_mesh.number_of_grid_points()};
  volume_vars.assign_subset(compute_solution(logical_coords));

  // call the reconstruction
  dynamic_cast<const Reconstructor&>(reconstructor)
      .reconstruct(make_not_null(&vars_on_lower_face),
                   make_not_null(&vars_on_upper_face), volume_vars, element,
                   neighbor_data, subcell_mesh);

  for (size_t dim = 0; dim < 1; ++dim) {
    // construct face-centered coordinates
    const auto basis = make_array<1>(Spectral::Basis::FiniteDifference);
    auto quadrature = make_array<1>(Spectral::Quadrature::CellCentered);
    auto extents = make_array<1>(num_pts);
    gsl::at(extents, dim) = num_pts + 1;
    gsl::at(quadrature, dim) = Spectral::Quadrature::FaceCentered;
    const Mesh<1> face_centered_mesh{extents, basis, quadrature};
    auto logical_coords_face_centered = logical_coordinates(face_centered_mesh);

    // check reconstructed values for reconstruct() function
    Variables<dg_package_data_argument_tags> expected_face_values{
        face_centered_mesh.number_of_grid_points()};
    expected_face_values.assign_subset(
        compute_solution(logical_coords_face_centered));

    CHECK_ITERABLE_APPROX(
        get<::Burgers::Tags::U>(gsl::at(vars_on_lower_face, dim)),
        get<::Burgers::Tags::U>(expected_face_values));
    CHECK_ITERABLE_APPROX(
        get<::Burgers::Tags::U>(gsl::at(vars_on_upper_face, dim)),
        get<::Burgers::Tags::U>(expected_face_values));

    // check reconstructed values for reconstruct_fd_neighbor() function
    Variables<dg_package_data_argument_tags> vars_on_mortar_face{0};
    Variables<dg_package_data_argument_tags> expected_mortar_face_values{0};

    for (size_t upper_or_lower = 0; upper_or_lower < 2; ++upper_or_lower) {
      // iterate over upper and lower direction
      Direction<1> direction =
          gsl::at(Direction<1>::all_directions(), 2 * dim + upper_or_lower);

      // 1D mortar has only one face point
      vars_on_mortar_face.initialize(1);
      expected_mortar_face_values.initialize(1);

      // call the reconstruction
      dynamic_cast<const Reconstructor&>(reconstructor)
          .reconstruct_fd_neighbor(make_not_null(&vars_on_mortar_face),
                                   volume_vars, element, neighbor_data,
                                   subcell_mesh, direction);

      // slice face-centered variables to get the values on the mortar
      data_on_slice(
          make_not_null(&get<::Burgers::Tags::U>(expected_mortar_face_values)),
          get<::Burgers::Tags::U>(expected_face_values),
          face_centered_mesh.extents(), direction.dimension(),
          direction.side() == Side::Lower ? 0
                                          : extents[direction.dimension()] - 1);

      CHECK_ITERABLE_APPROX(
          get<::Burgers::Tags::U>(vars_on_mortar_face),
          get<::Burgers::Tags::U>(expected_mortar_face_values));
    }
  }
}

}  // namespace Burgers::fd
}  // namespace TestHelpers


#include <alc/ast/expressions.hpp>
#include <alc/ast/manager.hpp>
#include <alc/constraint/set.hpp>

#include <boost/ut.hpp>

using namespace boost::ut;

struct consistency_test_case
{
    bool wantConsistent;
    std::vector<ast::constraint*> constraints;
};

struct satisfiability_test_case
{
    bool wantSatisfiability;
    std::vector<ast::constraint*> constraints;
    ast::constraint* target;
};

int main()
{
    ast::manager store;

    std::vector<consistency_test_case> checkConsistencyCases{
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::integer(store, 42),
                    ast::integer(store, 42)
                ),
            },
        },
        {
            false,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::neq,
                    ast::integer(store, 0),
                    ast::integer(store, 0)
                ),
            },
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::var(store, "foo")
                ),
            },
        },
        {
            false,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::neq,
                    ast::var(store, "foo"),
                    ast::var(store, "foo")
                ),
            },
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::add(store,
                        ast::var(store, "foo"), ast::var(store, "bar")
                    ),
                    ast::var(store, "foo")
                ),
            },
        },
        {
            false,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::add(store,
                        ast::var(store, "foo"), ast::integer(store, 0)
                    ),
                    ast::add(store,
                        ast::var(store, "foo"), ast::integer(store, 1)
                    )
                ),
            },
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::neq,
                    ast::multiply(store,
                        ast::integer(store, 2), ast::var(store, "foo")
                    ),
                    ast::multiply(store,
                        ast::integer(store, 3), ast::var(store, "foo")
                    )
                ),
            },
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::multiply(store,
                        ast::add(store,
                            ast::var(store, "foo"), ast::integer(store, 1)
                        ),
                        ast::add(store,
                            ast::var(store, "bar"), ast::integer(store, 1)
                        )
                    ),
                    ast::multiply(store,
                        ast::var(store, "foo"), ast::var(store, "bar")
                    )
                ),
            },
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::var(store, "bar")
                ),
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::integer(store, 42),
                    ast::integer(store, 42)
                ),
            },
        },
        {
            false,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::add(store,
                        ast::var(store, "foo"), ast::var(store, "bar")
                    ),
                    ast::integer(store, 0)
                ),
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::add(store,
                        ast::var(store, "foo"), ast::var(store, "bar")
                    ),
                    ast::integer(store, 1)
                ),
            },
        },
    };

    std::vector<satisfiability_test_case> satisfiabilityCases{
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::integer(store, 1)
                ),
            },
            store.make_expression<ast::constraint>(
                ast::constraint::relation::eq,
                ast::add(store,
                    ast::var(store, "foo"), ast::integer(store, 1)
                ),
                ast::integer(store, 2)
            ),
        },
        {
            false,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::integer(store, 1)
                ),
            },
            store.make_expression<ast::constraint>(
                ast::constraint::relation::eq,
                ast::add(store,
                    ast::var(store, "foo"), ast::integer(store, 1)
                ),
                ast::integer(store, 0)
            ),
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::integer(store, 1)
                ),
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::integer(store, 2)
                ),
            },
            store.make_expression<ast::constraint>(
                ast::constraint::relation::eq,
                ast::add(store,
                    ast::var(store, "foo"), ast::integer(store, 1)
                ),
                ast::integer(store, 0)
            ),
        },
        {
            true,
            {
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "foo"),
                    ast::integer(store, 1)
                ),
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::var(store, "bar"),
                    ast::integer(store, 2)
                ),
            },
            store.make_expression<ast::constraint>(
                ast::constraint::relation::eq,
                ast::add(store,
                    ast::var(store, "foo"), ast::var(store, "bar")
                ),
                ast::integer(store, 3)
            ),
        },
    };

    "default constructed"_test = [&] () {
        should("is consistent") = [&] () {
            constraint::set constraints;
            expect(constraints.check_consistency());
        };

        should("satisfies satisfiable constraint") = [&] () {
            constraint::set constraints;

            expect(constraints.check_satisfiability_of(
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::integer(store, 1),
                    ast::integer(store, 1)
            )) == true);
        };

        should("unsatisfies unsatisfiable constraint") = [&] () {
            constraint::set constraints;

            expect(constraints.check_satisfiability_of(
                store.make_expression<ast::constraint>(
                    ast::constraint::relation::eq,
                    ast::integer(store, 0),
                    ast::integer(store, 1)
            )) == false);
        };
    };

    "check_consistency"_test = [&] () {
        // TODO: range-v3
        for (auto& testCase : checkConsistencyCases)
        {
            std::string testName;
            for (size_t i = 0; i < testCase.constraints.size(); i++)
            {
                if (i != 0)
                    testName += " && ";

                testName += testCase.constraints[i]->to_string();
            }

            if (testCase.wantConsistent)
            {
                testName += " is consistent";
            }
            else
            {
                testName += " is inconsistent";
            }

            should(testName) = [&] () {
                constraint::set constraints;

                for (auto& constraint : testCase.constraints)
                {
                    constraints.add(constraint);
                }

                expect(constraints.check_consistency() == testCase.wantConsistent);
            };
        }
    };

    "satisfiability"_test = [&] () {
        // TODO: range-v3
        for (auto& testCase : satisfiabilityCases)
        {
            std::string testName = testCase.target->to_string();

            if (testCase.wantSatisfiability)
            {
                testName += " is satisfiable with ";
            }
            else
            {
                testName += " is unsatisfiable with ";
            }

            for (size_t i = 0; i < testCase.constraints.size(); i++)
            {
                if (i != 0)
                    testName += " && ";

                testName += testCase.constraints[i]->to_string();
            }

            should(testName) = [&] () {
                constraint::set constraints;

                for (auto& constraint : testCase.constraints)
                {
                    constraints.add(constraint);
                }

                expect(constraints.check_satisfiability_of(testCase.target) == testCase.wantSatisfiability);
            };
        }
    };

    return 0;
}


#include "../agg.hpp"
#include <boost/accumulators/statistics/mean.hpp>

ARRAY_AGGREGATE_FNC(amean, tag::mean);
SQL_AGGREGATE_FNC(v_mean, tag::mean);


#include "photobundle_pyramid.h"
#include "debug.h"

#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/LU>

PhotometricBundleAdjustmentPyr::
PhotometricBundleAdjustmentPyr(int num_levels, const Calibration& calib,
                               const ImageSize& im_size, const Options& options)
  : _rows(im_size.rows), _cols(im_size.cols)
{
  assert( num_levels > 0 );

  Calibration calib_pyr(calib);
  ImageSize im_size_pyr(im_size);

  _pyr.resize(num_levels);
  for(size_t i = 1; i < _pyr.size(); ++i) {
    _pyr[i].reset(new PhotometricBundleAdjustment(calib_pyr, im_size_pyr, options));
    calib_pyr = calib_pyr.pyrDown();
    im_size_pyr = im_size_pyr.pyrDown();
  }

  _im_pyr.resize(num_levels);
  for(size_t i = 0; i < _im_pyr.size(); ++i) {
    _im_pyr[i].reset(new cv::Mat);
  }

  _z_pyr.resize(num_levels);
  for(size_t i = 0; i < _im_pyr.size(); ++i) {
    _z_pyr[i].reset(new cv::Mat);
  }
}

void PhotometricBundleAdjustmentPyr::
addFrame(const uint8_t* image, const float* depth, const Mat44& T, Result* result)
{
  Warn("This is not finished yet\n");

  {
    const cv::Mat I(_rows, _cols, CV_8U, (void*) image);
    *_im_pyr[0] = I;
    for(size_t i = 1; i < _im_pyr.size(); ++i) {
      cv::pyrDown(*_im_pyr[i-1], *_im_pyr[i]);
    }
  }

  {
    // for depth, we'll do nearest neighbor resize
    const cv::Mat Z(_rows, _cols, CV_32F, (void*) depth);
    *_z_pyr[0] = Z;
    for(size_t i = 1; i < _z_pyr.size(); ++i) {
      cv::resize(*_z_pyr[i-1], *_z_pyr[i], _im_pyr[i]->size());
    }
  }

  Result tmp;
  Mat44 T_init(T);
  for(int i = (int) _im_pyr.size(); i >= 0; --i) {
    Info("Pyramid level %d\n", i);
    _pyr[i]->addFrame(_im_pyr[i]->ptr<uint8_t>(), _z_pyr[i]->ptr<float>(), T_init, &tmp);
    T_init = tmp.poses.back().inverse();
  }

  if(result)
    *result = tmp;
}


/* Boost test/integer.cpp
 * test int extension
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * 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/numeric/interval.hpp>
#include <boost/numeric/interval/ext/integer.hpp>
#include "bugs.hpp"

typedef boost::numeric::interval<float> I;

int main() {
  I x, y;
  x = 4 - (2 * y + 1) / 3;
# ifdef __BORLANDC__
  ::detail::ignore_warnings();
# endif
  return 0;
}


// -*- c++ -*-
// Program to extract word cooccurrence counts from a memory-mapped
// word-aligned bitext stores the counts lexicon in the format for
// mm2dTable<uint32_t> (ug_mm_2d_table.h)
//
// (c) 2010-2012 Ulrich Germann

// to do: multi-threading

#include <queue>
#include <iomanip>
#include <vector>
#include <iterator>
#include <sstream>
#include <algorithm>

#include <boost/program_options.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/thread.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

#include "moses/TranslationModel/UG/generic/program_options/ug_get_options.h"
#include "ug_mm_2d_table.h"
#include "ug_mm_ttrack.h"
#include "ug_corpus_token.h"

using namespace std;
using namespace sapt;
using namespace ugdiss;
using namespace boost::math;

typedef mm2dTable<id_type,id_type,uint32_t,uint32_t> LEX_t;
typedef SimpleWordId Token;

// DECLARATIONS
void interpret_args(int ac, char* av[]);

string swrd,twrd,L1,L2,bname;
TokenIndex V1,V2;
LEX_t LEX;

void
lookup_source(ostream& out, id_type r)
{
  vector<LEX_t::Cell> foo(LEX[r].start,LEX[r].stop);
  sort(foo.begin(),foo.end(),LEX_t::Cell::SortDescendingByValue());
  out << V1[r] << " " << LEX.m1(r) << endl;
  BOOST_FOREACH(LEX_t::Cell const& c, foo)
    {
      out << setw(10) << float(c.val)/LEX.m1(r)       << " "
	  << setw(10) << float(c.val)/LEX.m2(c.id) << " "
	  << V2[c.id] << " " << c.val    << "/" << LEX.m2(c.id) << endl;
    }
}

void
lookup_target(ostream& out, id_type c)
{
  vector<LEX_t::Cell> foo;
  LEX_t::Cell cell;
  for (size_t r = 0; r < LEX.numRows; ++r)
    {
      size_t j = LEX[r][c];
      if (j)
	{
	  cell.id  = r;
	  cell.val = j;
	  foo.push_back(cell);
	}
    }
  sort(foo.begin(),foo.end(),LEX_t::Cell::SortDescendingByValue());
  out << V2[c] << " " << LEX.m2(c) << endl;
  BOOST_FOREACH(LEX_t::Cell const& r, foo)
    {
      out << setw(10) << float(r.val)/LEX.m2(c)       << " "
	  << setw(10) << float(r.val)/LEX.m1(r.id) << " "
	  << V1[r.id] << " " << r.val    << "/" << LEX.m1(r.id) << endl;
    }
}

void
dump(ostream& out)
{
  for (size_t r = 0; r < LEX.numRows; ++r)
    lookup_source(out,r);
  out << endl;
}

int
main(int argc, char* argv[])
{
  interpret_args(argc,argv);
  char c = *bname.rbegin();
  if (c != '/' && c != '.') bname += '.';
  V1.open(bname+L1+".tdx");
  V2.open(bname+L2+".tdx");
  LEX.open(bname+L1+"-"+L2+".lex");

  cout.precision(2);
  id_type swid = V1[swrd];
  id_type twid = V2[twrd];
  if (swid != 1 && twid != 1)
    {
      cout << swrd << " " << twrd << " "
	   << LEX.m1(swid)    << " / "
	   << LEX[swid][twid] << " / "
	   << LEX.m2(twid)    << endl;
    }
  else if (swid != 1)
    lookup_source(cout,swid);
  else if (twid != 1)
    lookup_target(cout,twid);
  else
    dump(cout);
}

void
interpret_args(int ac, char* av[])
{
  namespace po=boost::program_options;
  po::variables_map vm;
  po::options_description o("Options");
  po::options_description h("Hidden Options");
  po::positional_options_description a;

  o.add_options()
    ("help,h",    "print this message")
    ("source,s",po::value<string>(&swrd),"source word")
    ("target,t",po::value<string>(&twrd),"target word")
    ;

  h.add_options()
    ("bname", po::value<string>(&bname), "base name")
    ("L1",    po::value<string>(&L1),"L1 tag")
    ("L2",    po::value<string>(&L2),"L2 tag")
    ;
  a.add("bname",1);
  a.add("L1",1);
  a.add("L2",1);
  get_options(ac,av,h.add(o),a,vm,"cfg");

}


/**
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Tests for the ScalarKernels Operations
 */

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

#include <stdexcept>
#include <string>

#include "ScalarKernels.h"
#include "MeshConfig.h"
#include "MeshSourceStructGenConfig.h"
#include "CupCfdAoSMesh.h"
#include "EuclideanVector.h"

#include "PartitionerNaiveConfig.h"
#include "PartitionerConfig.h"

#include <cstdlib>

#include <iostream>

using namespace cupcfd::fvm;

namespace utf = boost::unit_test;
namespace euc = cupcfd::geometry::euclidean;
namespace meshgeo = cupcfd::geometry::mesh;

// Setup
BOOST_AUTO_TEST_CASE(setup)
{
    int argc = boost::unit_test::framework::master_test_suite().argc;
    char ** argv = boost::unit_test::framework::master_test_suite().argv;

    MPI_Init(&argc, &argv);
}

// === FluxScalarDolfynFaceLoop ===
// Test 1: Test it runs without error on a small mesh
BOOST_AUTO_TEST_CASE(FluxScalarDolfynFaceLoop_test1)
{
	cupcfd::error::eCodes status;
    cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// Create a small test mesh
    cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;

	// Setup the source config
    meshgeo::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0);

	// Setup the config to use for building
    meshgeo::MeshConfig<int, double, int> meshConfig(partConfig, meshSourceConfig);

    meshgeo::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	int nCells = mesh->properties.lTCells;
	// int nOwnedCells = mesh->properties.lOCells;
	// int nGhostCells = mesh->properties.lGhCells;
	int nBnds = mesh->properties.lBoundaries;
	int nFaces = mesh->properties.lFaces;
	// int nRegions = mesh->properties.lRegions;
	// int repetitions = 1000;

	double * PhiCell = (double *) malloc(sizeof(double) * nCells);
	double * VisEff = (double *) malloc(sizeof(double) * nCells);
	double * Au = (double *) malloc(sizeof(double) * nCells);
	double * Su = (double *) malloc(sizeof(double) * nCells);
	double * Den = (double *) malloc(sizeof(double) * nCells);
	double * MassFlux = (double *) malloc(sizeof(double) * nFaces);
	double * TE = (double *) malloc(sizeof(double) * nCells);
	double * PhiBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * CpBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * visEffBoundary = (double *) malloc(sizeof(double) * nBnds);
	double * RFace = (double *) malloc(sizeof(double) * nFaces * 2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> * dPhidx =
		(cupcfd::geometry::euclidean::EuclideanVector<double,3> *)
		malloc(sizeof(cupcfd::geometry::euclidean::EuclideanVector<double,3>) * nCells);

	bool SolveTurb = true;
	bool SolveEnthalpy = false;
	// double sigma = 1.0;
	// double sigma2 = 1.2;
	double vislam = 1.3;
	int ivar = 1;
	int VarT = 2;
	double Sigma_T = 1.4;
	double Prandtl = 1.2E-3;
	int VarTE = 3;
	double Sigma_k = 1.5;
	int VarED = 4;
	double Sigma_e = 1.6;
	double Sigma_s = 1.7;
	double Schmidt = 1.4E-3;
	double GammaBlend = 1.1;
	double Small = 1E-18;
	double Large = 1E+18;
	double TMCmu = 1.8;

	// Won't bother populating the data arrays for now

	status = cupcfd::fvm::FluxScalarDolfynFaceLoop(*mesh,
										PhiCell, nCells,
										PhiBoundary, nBnds,
										VisEff, nCells,
										Au, nCells,
										Su, nCells,
										Den, nCells,
										MassFlux, nFaces,
										TE, nCells,
										CpBoundary, nBnds,
										visEffBoundary, nBnds,
										RFace, nFaces * 2,
										dPhidx, nCells,
										SolveTurb, SolveEnthalpy,
										// sigma, sigma2,
										vislam,
										ivar, VarT, Sigma_T, Prandtl,
										VarTE, Sigma_k, VarED, Sigma_e,
										Sigma_s, Schmidt, GammaBlend, Small, Large, TMCmu);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	free(PhiCell);
	free(VisEff);
	free(Au);
	free(Su);
	free(Den);
	free(MassFlux);
	free(TE);
	free(CpBoundary);
	free(visEffBoundary);
	free(RFace);
	free(dPhidx);

	delete(mesh);
}

BOOST_AUTO_TEST_CASE(cleanup)
{
    MPI_Finalize();
}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_AVX_DEINTERLEAVE_SECOND_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_AVX_DEINTERLEAVE_SECOND_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/toolbox/swar/functions/deinterleave_second.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      typedef typename meta::scalar_of<A0>::type                  sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >       svtype;

      svtype a00 = _mm256_extractf128_ps(a0, 0);
      svtype a01 = _mm256_extractf128_ps(a0, 1);
      svtype a10 = _mm256_extractf128_ps(a1, 0);
      svtype a11 = _mm256_extractf128_ps(a1, 1);
      __m256 that = _mm256_castps128_ps256(boost::simd::deinterleave_second(a00, a01));
      return _mm256_insertf128_ps(that,  boost::simd::deinterleave_second(a10, a11), 1);
     }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      typedef typename meta::scalar_of<A0>::type                  sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >       svtype;
      typedef simd::native<sctype, boost::simd::tag::avx_ >        vtype;

      svtype a00 = _mm256_extractf128_pd(a0, 0);
      svtype a01 = _mm256_extractf128_pd(a0, 1);
      svtype a10 = _mm256_extractf128_pd(a1, 0);
      svtype a11 = _mm256_extractf128_pd(a1, 1);
      __m256d that = _mm256_castpd128_pd256(boost::simd::deinterleave_second(a00, a01));
      return  _mm256_insertf128_pd(that,  boost::simd::deinterleave_second(a10, a11), 1);
     }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      typedef typename meta::scalar_of<A0>::type                  sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >       svtype;
      typedef simd::native<sctype, boost::simd::tag::avx_ >        vtype;

      svtype a00 = _mm256_extractf128_si256(a0, 0);
      svtype a01 = _mm256_extractf128_si256(a0, 1);
      svtype a10 = _mm256_extractf128_si256(a1, 0);
      svtype a11 = _mm256_extractf128_si256(a1, 1);
      __m256i that = _mm256_castsi128_si256(boost::simd::deinterleave_second(a00, a01));
      return  _mm256_insertf128_si256(that,  boost::simd::deinterleave_second(a10, a11), 1);

     }
  };

} } }

#endif
#endif


#include <exe/labatut.h>
#include <exe/inputParser.h>

#include <base/cgal_typedefs.h>
#include <IO/fileIO.h>
#include <IO/ttIO.h>
#include <IO/ethIO.h>
#ifdef COLMAP
#include <IO/colmapIO.h>
#endif
#include <util/helper.h>
#include <util/geometricOperations.h>

#include <processing/meshProcessing.h>
#include <processing/graphCut.h>
#include <processing/pointSetProcessing.h>
#include <processing/normalAndSensorProcessing.h>
#include <processing/evaluation.h>
#include <processing/rayTracingFacet.h>

#ifdef Open3D
#include "open3d/Open3D.h"
#include "open3d/geometry/TetraMesh.h"
#endif

#include <boost/filesystem.hpp>
using namespace boost::filesystem;

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
///////////////////////// CONTROL FUNCTIONS /////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
int runLabatut(dirHolder& dir, dataHolder& data, runningOptions& options, exportOptions& exportO){

    // TOOD: add an assert for checking that there are no duplicate points in the input for the 3DT, because the ray tracing crashes
    // if there are. Maybe it would however be best to change my data.points+data.infos vector to a CGAL::Point_set_3
    // with property maps for each info.

    ///////////////////////////////
    ///////// FILE NAMING /////////
    ///////////////////////////////
    options.scoring="_"+options.scoring;
    string rw_string = double2string(options.area_reg_weight+options.angle_reg_weight+options.cc_reg_weight+options.sv_reg_weight);
    string ol_string;
    if(options.percentage_of_outliers > 0.0)
        ol_string = double2string(options.percentage_of_outliers*100);
    if(dir.read_file.empty())
        dir.read_file = dir.write_file;
    if(dir.write_file.empty())
        dir.write_file = dir.read_file+ol_string+options.scoring+rw_string;
    else
        dir.write_file+=options.scoring+rw_string;

    ///////////////////////////////
    ///////// IMPORT DATA /////////
    ///////////////////////////////
    // reconstruction input
    if(options.data_source == "ply"){
        importPLYPoints(dir, data);
    }
    else if(options.data_source == "npz"){
        importNPZ(dir, data);
    }
    #ifdef COLMAP
    else if(options.data_source == "colmap"){
        readColmapFiles(dir, data);
    }
    #endif
    #ifdef OpenMVS
    else if(options.data_source == "omvs"){
        if(importOMVSScene(dir, data))
            return 1;
    }
    #endif
    else{
        cout << "ERROR: not a valid reconstruction input" << endl;
        return 1;
    }

    ///////////////////////////////////
    ///////// PREPROCESS DATA /////////
    ///////////////////////////////////

//    if(options.scale > 0.0)
//        scalePointSet(dir, data, options.scale);

    if(options.scale > 0.0)
        standardizePointSet(dir, data, options.scale);

//    if(options.scoring == "_rt")
//        orderSensors(data);
//    else
//        cout << "\nSensors not ordered for ray-tracing" << endl;

    if(exportO.cameras){
        dir.suffix = "_cameras";
        exportCameraCenter(dir, data);
    }
    // export the scanned points
    if(exportO.scan){
        dir.suffix = "_scan";
        exportPLY(dir, data.points, data.infos, exportO);
    }

    //////////////////////////////////////
    /////// DELAUNAY TRIANGULATION ///////
    //////////////////////////////////////
    if(options.Dt_epsilon > 0.0)
        makeAdaptiveDelaunayWithInfo(data, options.Dt_epsilon);
    else
        makeDelaunayWithInfo(data);

    if(options.labatut_sigma == -1.0 && (options.scoring == "_rt" || options.scoring == "_clrt"))
        options.labatut_sigma = pcaKNN(data.Dt);
    else{
        cout << "\nd parameter of Labatu set to " << options.labatut_sigma << endl;
    }

    //////////////////////////////////////
    ///////////// RAY TRACING ////////////
    //////////////////////////////////////
    auto rc = processing::RayCaster(data, options);
    rc.run(1);

    ////////////////////////////
    /////// OPTIMIZATION ///////
    ////////////////////////////
    if(options.optimization == 1){
        options.gco_iterations = -1;
        if(options.score_type == "mine" || options.scoring == "_cl" || options.scoring == "_cs" || options.scoring == "_clcs" || options.scoring == "_lrtcs")
            graphCutTet(data, options);
        else
            graphCutFacet(data,options);
    }
    else{
        cout << "\nLabel cells without optimization by taking max score" << endl;
        cout << "\t-infinite cells will be labelled outside" << endl;
        for(auto cit = data.Dt.all_cells_begin(); cit != data.Dt.all_cells_end(); cit++){
            if(data.Dt.is_infinite(cit)){
                cit->info().gc_label = 1;
                continue;
            }
            // this means untraversed cells and 50/50 cells will be labelled as inside
            cit->info().gc_label = cit->info().outside_score > cit->info().inside_score ? 1 : 0;
        }
    }

    ////////////////////////////
    ////////// EXPORT //////////
    ////////////////////////////

    //// export interface
    if(exportO.interface)
        exportInterface(dir, data, options, exportO);

    #ifdef OpenMVS
    if(options.clean_mesh)
        omvsCleanMesh(dir,data);
    #endif

//    if(exportO.coloredFacets)
//        exportColoredFacets(dir, data.Dt, options.optimization);
    if(exportO.cellScore){
        exportCellCenter(dir, data.Dt);
        exportCellScore(dir, data.Dt);
    }
    if(exportO.convexHull){
        exportConvexHull(dir, data.Dt);
    }
    ////////// create surface mesh ////////////
    // set export options
    meshProcessingOptions mpOptions;
    mpOptions.try_to_close = options.try_to_close;
    mpOptions.try_to_make_manifold = options.try_to_make_manifold;
    mpOptions.number_of_components_to_keep = options.number_of_components_to_keep;
    mpOptions.factor_for_removing_large_faces = options.factor_for_removing_large_faces;
    if(exportO.mesh){
        dir.suffix = "_mesh";
        createSurfaceMesh(data, mpOptions);
        exportPLY(dir, data.smesh);
    }

    ////////// sample surface mesh //////////
    if(exportO.sampling){
        data.points.clear();
        data.infos.clear();
        if(data.smesh.is_empty()){
            createSurfaceMesh(data, mpOptions);
        }
        sampleMesh(data, exportO);
        dir.suffix = "_sampled";
        // turn off color and sensor_vec export, because mesh sampling obviously does not have that
        exportO.sensor_vec = false;
        exportO.sensor_position = false;
        exportO.color = false;
        exportO.normals = false;
        exportPLY(dir, data.points, data.infos, exportO);
    }

    if(options.evaluate_mesh){
        if(data.smesh.is_empty()){
            createSurfaceMesh(data, mpOptions);
        }
        if(options.ground_truth){
            double iou;
            if(calcIOU(data, exportO.sampling_method_option, iou))
                return 1;
            printMeshEvaluation(dir,data,iou);
        }
        else
            printMeshEvaluation(dir,data,-1.0);

    }

    return 0;
}

int main(int argc, char const *argv[]){

    cliParser ip("labatut");
    if(ip.parse(argc, argv))
        return 1;
    if(ip.getInput())
        return 1;
    if(ip.getOutput())
        return 1;
    if(ip.getLabatut())
        return 1;

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\n-----LABATUT SURFACE RECONSTRUCTION-----" << endl;
    cout << "\nWorking dir set to:\n\t-" << ip.dh.path << endl;

    dataHolder data;
    if(runLabatut(ip.dh, data, ip.ro, ip.eo))
        return 1;

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\n-----LABATUT SURFACE RECONSTRUCTION FINISHED in "<< duration.count() << "s -----\n" << endl;

    return 0;

}


#include <iostream>

#include <frovedis.hpp>
#include <frovedis/ml/w2v/word2vec.hpp>
#include <boost/program_options.hpp>

using namespace frovedis;
using namespace boost::program_options;

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

  std::string encode_path, vocab_path, vocab_count_path, weight_path;
  int hidden_size, window, negative, iter, min_count, min_sync_words, full_sync_times, message_size;
  int num_threads;
  float sample, alpha, model_sync_period;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<std::string>(&encode_path)->required(), "path for input encoded text.")
    ("vocab,v", value<std::string>(&vocab_path)->required(), "path for input binary file of vocabulary.")
    ("vocab-count,c", value<std::string>(&vocab_count_path)->required(), "path for input binary file of vocabulary count.")
    ("output,o", value<std::string>(&weight_path)->required(), "path for output file of weight matrix")

    ("size", value<int>(&hidden_size)->default_value(100, "100"),
     "size of word vectors.")
    ("window", value<int>(&window)->default_value(5, "5"),
     "max skip length between words")
    ("sample", value<float>(&sample)->default_value(1e-3f, "1e-3, useful range is (0, 1e-5)"),
     "threshold for occurrence of words")
    ("negative", value<int>(&negative)->default_value(5, "5"),
     "Number of negative examples")
    ("iter", value<int>(&iter)->default_value(5, "5"),
     "Number of training iterations")
    // ("min-count", value<int>(&min_count)->default_value(5, "5"),
    //  "This will discard words that appear less than the times")
    ("alpha", value<float>(&alpha)->default_value(0.1, "0.1"),
     "starting learning rate")
    // ("batch-size", value<int>(&batch_size)->default_value(11, "11 (i.e., 2 * window + 1)"),
    //  "The batch size used for mini-batch training")
    ("sync-period", value<float>(&model_sync_period)->default_value(0.1, "0.1"),
     "Synchronize model every specified seconds")
    ("min-sync-words", value<int>(&min_sync_words)->default_value(1024, "1024"),
     "Minimal number of words to be synced at each model sync")
    ("full-sync-times", value<int>(&full_sync_times)->default_value(0, "0"),
     "Enforced full model sync-up times during training")
    ("message-size", value<int>(&message_size)->default_value(1024, "1024"),
     "MPI message chunk size in MB")

    ("threads", value<int>(&num_threads)->default_value(0, "0"),
     "Number of threads; '0' use as many as the number of cores.")
    ;

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().run(), argmap);
  if(argmap.count("help")){
    std::cerr << opt << std::endl;
    exit(1);
  }

  notify(argmap);

  // Alert
  int are_supposed_parameters =	   \
    hidden_size <= 512 &&	   \
    hidden_size % 2 == 0 &&	   \
    negative == 5 &&		   \
    window <= 8;
  if (!are_supposed_parameters) {
    LOG(INFO) << "===============  CAUTION  ===============" << std::endl
	      << "Optimized computation is not supported for the specified arguments so this program fallbacks to slower version." << std::endl
	      << "Recommended arguments to enable optimization are " << std::endl
	      << "    size <= 512 && " << std::endl
	      << "    size % 2 == 0 && " << std::endl
	      << "    negative == 5 && " <<	std::endl
	      << "    window <= 8" << std::endl
	      << "=========================================" << std::endl;

  }

  w2v::train_config config = {
    hidden_size, window, sample, negative, iter,
    alpha, model_sync_period, min_sync_words,
    full_sync_times, message_size, num_threads
  };

  w2v::train_each(
    encode_path, vocab_count_path, weight_path, config
  );

  return 0;
}


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

#ifndef CRYPTO3_RANDOM_CHACHA_HPP
#define CRYPTO3_RANDOM_CHACHA_HPP

#include <string>

#include <boost/config.hpp>
#include <boost/noncopyable.hpp>
#include <boost/random/detail/auto_link.hpp>
#include <boost/system/config.hpp>    // force autolink to find Boost.System

#include <nil/crypto3/hash/algorithm/hash.hpp>
#include <nil/crypto3/hash/sha2.hpp>

#include <nil/crypto3/mac/algorithm/compute.hpp>
#include <nil/crypto3/mac/hmac.hpp>

#include <nil/crypto3/stream/algorithm/encrypt.hpp>
#include <nil/crypto3/stream/chacha.hpp>

namespace nil {
    namespace crypto3 {
        namespace random {

            /*!
             * @brief
             * @tparam StreamCipher
             * @tparam MessageAuthenticationCode
             *
             * ChaCha_RNG is a very fast but completely ad-hoc RNG created by
             * creating a 256-bit random value and using it as a key for ChaCha20.
             *
             * The RNG maintains two 256-bit keys, one for HMAC_SHA256 (HK) and the
             * other for ChaCha20 (CK). To compute a new key in response to
             * reseeding request or add_entropy calls, ChaCha_RNG computes
             *   CK' = HMAC_SHA256(HK, input_material)
             * Then a new HK' is computed by running ChaCha20 with the new key to
             * output 32 bytes:
             *   HK' = ChaCha20(CK')
             *
             * Now output can be produced by continuing to produce output with ChaCha20
             * under CK'
             *
             * The first HK (before seeding occurs) is taken as the all zero value.
             *
             * @warning This RNG construction is probably fine but is non-standard.
             * The primary reason to use it is in cases where the other RNGs are
             * not fast enough.
             */
            template<typename StreamCipher = stream::chacha<64, 128, 20>,
                     typename MessageAuthenticationCode = mac::hmac<hashes::sha2<256>>>
            struct chacha : private boost::noncopyable {
                typedef StreamCipher stream_cipher_type;
                typedef MessageAuthenticationCode mac_type;
                typedef typename mac_type::key_type key_type;

                typedef std::vector<std::uint8_t> result_type;

                BOOST_STATIC_CONSTANT(std::size_t, reseed_interval = 256);

                BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

                /** Returns the smallest value that the \random_device can produce. */
                static BOOST_CONSTEXPR std::size_t min BOOST_PREVENT_MACRO_SUBSTITUTION() {
                    return 0;
                }
                /** Returns the largest value that the \random_device can produce. */
                static BOOST_CONSTEXPR std::size_t max BOOST_PREVENT_MACRO_SUBSTITUTION() {
                    return ~0u;
                }

                /** Constructs a @c random_device, optionally using the default device. */
                BOOST_RANDOM_DECL chacha() : cnt(0) {
                    mac_key.fill(0);
                }
                /**
                 * Constructs a @c random_device, optionally using the given token as an
                 * access specification (for example, a URL) to some implementation-defined
                 * service for monitoring a stochastic process.
                 */
                template<typename SeedSinglePassRange>
                BOOST_RANDOM_DECL explicit chacha(const SeedSinglePassRange &token) : cnt(0) {
                    mac_key.fill(0);
                }

                BOOST_RANDOM_DECL ~chacha() {
                    mac_key.fill(0);
                    mac_key.clear();
                }

                /** default seeds the underlying generator. */
                void seed() {
                    cnt = 0;
                }

                /** Seeds the underlying generator with first and last. */
                template<typename InputIterator>
                void seed(InputIterator &first, InputIterator last) {
                    update(first, last);

                    if (CHAR_BIT * std::distance(first, last) *
                            sizeof(typename std::iterator_traits<InputIterator>::value_type) >=
                        reseed_interval) {
                        cnt = 0;
                    }
                }

                /**
                 * Returns: An entropy estimate for the random numbers returned by
                 * operator(), in the range min() to log2( max()+1). A deterministic
                 * random number generator (e.g. a pseudo-random number engine)
                 * has entropy 0.
                 *
                 * Throws: Nothing.
                 */
                BOOST_RANDOM_DECL double entropy() const {
                }
                /** Returns a random value in the range [min, max]. */
                BOOST_RANDOM_DECL result_type operator()() {
                    return accumulators::extract::stream<StreamCipher>(acc);
                }

                /** Fills a range with random values. */
                template<class Iter>
                void generate(Iter begin, Iter end) {
                    detail::pack(accumulators::extract::stream<StreamCipher>(acc), begin, end);
                }

            protected:
                template<typename InputIterator>
                void update(InputIterator first, InputIterator last) {
                    mac_key = encrypt<StreamCipher>({0}, mac::compute<mac_type>(first, last, mac_key), acc);
                }

                std::size_t cnt;
                key_type mac_key;
                accumulator_set<StreamCipher> acc;
            };
        }    // namespace random
    }        // namespace crypto3
}    // namespace nil

#endif /* BOOST_RANDOM_RANDOM_DEVICE_HPP */


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

#include <dlib/python.h>
#include <boost/shared_ptr.hpp>
#include <dlib/matrix.h>
#include <dlib/svm.h>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "testing_results.h"
#include <boost/python/args.hpp>

using namespace dlib;
using namespace std;
using namespace boost::python;

typedef matrix<double,0,1> sample_type;
typedef std::vector<std::pair<unsigned long,double> > sparse_vect;

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

namespace dlib
{
    template <typename T>
    bool operator== (
        const ranking_pair<T>& ,
        const ranking_pair<T>&
    )
    {
        pyassert(false, "It is illegal to compare ranking pair objects for equality.");
        return false;
    }
}

template <typename T>
void resize(T& v, unsigned long n) { v.resize(n); }

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

template <typename trainer_type>
typename trainer_type::trained_function_type train1 (
    const trainer_type& trainer,
    const ranking_pair<typename trainer_type::sample_type>& sample
)
{
    typedef ranking_pair<typename trainer_type::sample_type> st;
    pyassert(is_ranking_problem(std::vector<st>(1, sample)), "Invalid inputs");
    return trainer.train(sample);
}

template <typename trainer_type>
typename trainer_type::trained_function_type train2 (
    const trainer_type& trainer,
    const std::vector<ranking_pair<typename trainer_type::sample_type> >& samples
)
{
    pyassert(is_ranking_problem(samples), "Invalid inputs");
    return trainer.train(samples);
}

template <typename trainer_type>
void set_epsilon ( trainer_type& trainer, double eps)
{
    pyassert(eps > 0, "epsilon must be > 0");
    trainer.set_epsilon(eps);
}

template <typename trainer_type>
double get_epsilon ( const trainer_type& trainer) { return trainer.get_epsilon(); }

template <typename trainer_type>
void set_c ( trainer_type& trainer, double C)
{
    pyassert(C > 0, "C must be > 0");
    trainer.set_c(C);
}

template <typename trainer_type>
double get_c (const trainer_type& trainer)
{
    return trainer.get_c();
}

template <typename trainer>
void add_ranker (
    const char* name
)
{
    class_<trainer>(name)
        .add_property("epsilon", get_epsilon<trainer>, set_epsilon<trainer>)
        .add_property("c", get_c<trainer>, set_c<trainer>)
        .add_property("max_iterations", &trainer::get_max_iterations, &trainer::set_max_iterations)
        .add_property("force_last_weight_to_1", &trainer::forces_last_weight_to_1, &trainer::force_last_weight_to_1)
        .add_property("learns_nonnegative_weights", &trainer::learns_nonnegative_weights, &trainer::set_learns_nonnegative_weights)
        .def("train", train1<trainer>)
        .def("train", train2<trainer>)
        .def("be_verbose", &trainer::be_verbose)
        .def("be_quiet", &trainer::be_quiet);
}

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

template <
    typename trainer_type,
    typename T
    >
const ranking_test _cross_ranking_validate_trainer (
    const trainer_type& trainer,
    const std::vector<ranking_pair<T> >& samples,
    const unsigned long folds
)
{
    pyassert(is_ranking_problem(samples), "Training data does not make a valid training set.");
    pyassert(1 < folds && folds <= samples.size(), "Invalid number of folds given.");
    return cross_validate_ranking_trainer(trainer, samples, folds);
}

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

void bind_svm_rank_trainer()
{
    using boost::python::arg;
    class_<ranking_pair<sample_type> >("ranking_pair")
        .add_property("relevant", &ranking_pair<sample_type>::relevant)
        .add_property("nonrelevant", &ranking_pair<sample_type>::nonrelevant)
        .def_pickle(serialize_pickle<ranking_pair<sample_type> >());

    class_<ranking_pair<sparse_vect> >("sparse_ranking_pair")
        .add_property("relevant", &ranking_pair<sparse_vect>::relevant)
        .add_property("nonrelevant", &ranking_pair<sparse_vect>::nonrelevant)
        .def_pickle(serialize_pickle<ranking_pair<sparse_vect> >());

    typedef std::vector<ranking_pair<sample_type> > ranking_pairs;
    class_<ranking_pairs>("ranking_pairs")
        .def(vector_indexing_suite<ranking_pairs>())
        .def("clear", &ranking_pairs::clear)
        .def("resize", resize<ranking_pairs>)
        .def_pickle(serialize_pickle<ranking_pairs>());

    typedef std::vector<ranking_pair<sparse_vect> > sparse_ranking_pairs;
    class_<sparse_ranking_pairs>("sparse_ranking_pairs")
        .def(vector_indexing_suite<sparse_ranking_pairs>())
        .def("clear", &sparse_ranking_pairs::clear)
        .def("resize", resize<sparse_ranking_pairs>)
        .def_pickle(serialize_pickle<sparse_ranking_pairs>());

    add_ranker<svm_rank_trainer<linear_kernel<sample_type> > >("svm_rank_trainer");
    add_ranker<svm_rank_trainer<sparse_linear_kernel<sparse_vect> > >("svm_rank_trainer_sparse");

    def("cross_validate_ranking_trainer", &_cross_ranking_validate_trainer<
                svm_rank_trainer<linear_kernel<sample_type> >,sample_type>,
                (arg("trainer"), arg("samples"), arg("folds")) );
    def("cross_validate_ranking_trainer", &_cross_ranking_validate_trainer<
                svm_rank_trainer<sparse_linear_kernel<sparse_vect> > ,sparse_vect>,
                (arg("trainer"), arg("samples"), arg("folds")) );
}


// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.

#include <iostream>
#include <sstream>

#include <boost/python.hpp>

#include "dynamic-graph/python/signal.hh"

#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/value.h>

#include "dynamic-graph/python/dynamic-graph-py.hh"
#include "dynamic-graph/python/signal-wrapper.hh"

using dynamicgraph::SignalBase;

namespace bp = boost::python;

namespace dynamicgraph {
namespace python {

typedef int time_type;

typedef Eigen::AngleAxis<double> VectorUTheta;
typedef Eigen::Quaternion<double> Quaternion;

typedef Eigen::VectorXd Vector;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 7, 1> Vector7;

typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, 3, 3> MatrixRotation;
typedef Eigen::Matrix<double, 4, 4> Matrix4;
typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
typedef Eigen::Matrix<double, 6, 6> MatrixTwist;

template <typename Time>
void exposeSignalBase(const char* name) {
  typedef SignalBase<Time> S_t;
  bp::class_<S_t, boost::noncopyable>(name, bp::no_init)
      .add_property("time", bp::make_function(&S_t::getTime, bp::return_value_policy<bp::copy_const_reference>()),
                    &S_t::setTime)
      .add_property("name", bp::make_function(&S_t::getName, bp::return_value_policy<bp::copy_const_reference>()))

      .def("getName", &S_t::getName, bp::return_value_policy<bp::copy_const_reference>())
      .def("getClassName",
           +[](const S_t& s) -> std::string {
             std::string ret;
             s.getClassName(ret);
             return ret;
           })

      .def("plug", &S_t::plug, "Plug the signal to another signal")
      .def("unplug", &S_t::unplug, "Unplug the signal")
      .def("isPlugged", &S_t::isPlugged, "Whether the signal is plugged")
      .def("getPlugged", &S_t::getPluged, bp::return_value_policy<bp::reference_existing_object>(),
           "To which signal the signal is plugged")

      .def("recompute", &S_t::recompute, "Recompute the signal at given time")

      .def("__str__",
           +[](const S_t& s) -> std::string {
             std::ostringstream oss;
             s.display(oss);
             return oss.str();
           })
      .def("displayDependencies",
           +[](const S_t& s, int time) -> std::string {
             std::ostringstream oss;
             s.displayDependencies(oss, time);
             return oss.str();
           },
           "Print the signal dependencies in a string");
}

template <>
auto exposeSignal<MatrixHomogeneous, time_type>(const std::string& name) {
  typedef Signal<MatrixHomogeneous, time_type> S_t;
  bp::class_<S_t, bp::bases<SignalBase<time_type> >, boost::noncopyable> obj(name.c_str(), bp::init<std::string>());
  obj.add_property("value", +[](const S_t& signal) -> Matrix4 { return signal.accessCopy().matrix(); },
                   +[](S_t& signal, const Matrix4& v) {
                     // TODO it isn't hard to support pinocchio::SE3 type here.
                     // However, this adds a dependency to pinocchio.
                     signal.setConstant(MatrixHomogeneous(v));
                   },
                   "the signal value.");
  return obj;
}

void exposeSignals() {
  exposeSignalBase<time_type>("SignalBase");

  exposeSignalsOfType<bool, time_type>("Bool");
  exposeSignalsOfType<int, time_type>("Int");
  exposeSignalsOfType<double, time_type>("Double");

  exposeSignalsOfType<Vector, time_type>("Vector");
  exposeSignalsOfType<Vector3, time_type>("Vector3");
  exposeSignalsOfType<Vector7, time_type>("Vector7");

  exposeSignalsOfType<Matrix, time_type>("Matrix");
  exposeSignalsOfType<MatrixRotation, time_type>("MatrixRotation");
  exposeSignalsOfType<MatrixHomogeneous, time_type>("MatrixHomogeneous");
  exposeSignalsOfType<MatrixTwist, time_type>("MatrixTwist");

  exposeSignalsOfType<Quaternion, time_type>("Quaternion");
  exposeSignalsOfType<VectorUTheta, time_type>("VectorUTheta");
}

namespace signalBase {

template <class T>
SignalWrapper<T, int>* createSignalWrapperTpl(const char* name, bp::object o, std::string& error) {
  typedef SignalWrapper<T, int> SignalWrapper_t;
  if (!SignalWrapper_t::checkCallable(o, error)) {
    return NULL;
  }

  SignalWrapper_t* obj = new SignalWrapper_t(name, o);
  return obj;
}

PythonSignalContainer* getPythonSignalContainer() {
  Entity* obj = entity::create("PythonSignalContainer", "python_signals");
  return dynamic_cast<PythonSignalContainer*>(obj);
}

#define SIGNAL_WRAPPER_TYPE(IF, Enum, Type)                               \
  IF(command::Value::typeName(command::Value::Enum).compare(type) == 0) { \
    obj = createSignalWrapperTpl<Type>(name, object, error);              \
  }

/**
   \brief Create an instance of SignalWrapper
*/
SignalBase<int>* createSignalWrapper(const char* name, const char* type, bp::object object) {
  PythonSignalContainer* psc = getPythonSignalContainer();
  if (psc == NULL) return NULL;

  SignalBase<int>* obj = NULL;
  std::string error;
  SIGNAL_WRAPPER_TYPE(if, BOOL, bool)
  // SIGNAL_WRAPPER_TYPE(else if, UNSIGNED ,bool)
  SIGNAL_WRAPPER_TYPE(else if, INT, int)
  SIGNAL_WRAPPER_TYPE(else if, FLOAT, float)
  SIGNAL_WRAPPER_TYPE(else if, DOUBLE, double)
  // SIGNAL_WRAPPER_TYPE(else if, STRING   ,bool)
  SIGNAL_WRAPPER_TYPE(else if, VECTOR, Vector)
  // SIGNAL_WRAPPER_TYPE(else if, MATRIX   ,bool)
  // SIGNAL_WRAPPER_TYPE(else if, MATRIX4D ,bool)
  else {
    error = "Type not understood";
  }

  if (obj == NULL) throw std::runtime_error(error);
  // Register signal into the python signal container
  psc->signalRegistration(*obj);

  // Return the pointer
  return obj;
}

}  // namespace signalBase
}  // namespace python
}  // namespace dynamicgraph


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

// run via:
// export LD_LIBRARY_PATH=/opt/ibm/ILOG/CPLEX_Studio1210/opl/bin/x86-64_linux
// test : env | grep LD_
// any code that uses the wrapper has to set the following copts: "-DIL_STD",
// "-fPIC", "-DILOUSEMT", "-D_REENTRANT" bazel test //test:cplex_wrapper_test

#ifndef CPLEX_WRAPPER_HEADER
#define CPLEX_WRAPPER_HEADER

#include "miqp_planner_data.hpp"
#include "miqp_planner_settings.h"
#include "model_input_data_source.hpp"

#include <sstream>

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

#include "bark/commons/util/util.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"

#include "ilopl/iloopl.h"

namespace miqp {
namespace planner {
namespace cplex {

class NotImplementedException : public std::logic_error {
 public:
  NotImplementedException()
      : std::logic_error{"Function not yet implemented."} {}
};

struct SolutionProperties {
  int status;
  double gap;
  double objective;
  double time;
  int NrConstraints;
  int NrBinaryVariables;
  int NrFloatVariables;
  int NonZeroCoefficients;
  int NrIterations;
  int NrSolutionPool;
};

enum OptimizationStatus {
  SUCCESS = 0,
  FAILED_NO_SOLUT = 1,
  FAILED_SEG_FAULT = 2,
  FAILED_TIMEOUT = 3
};

class CplexWrapper {
 public:
  enum ParameterSource { DATFILE = 0, CPPINPUTS = 1, MIXED = 2 };
  typedef MiqpPlannerWarmstartType WarmstartType;
  typedef MiqpPlannerParallelMode ParallelMode;

  /**
   * @brief CplexWrapper - a class interfacing our MIQP cplex model with the car
   *agent.
   *
   * @note the @param percision: The floating point precision that is a) used to
   *fix all parameters to in ModelInputDataSource and b) as an output accurancy
   *when serializing to the problem to a DAT file. A too low precision procudes
   *wrong results. A too high precision causes numerical problems and
   *differences in the dat file and cpp struct solutions of the same problem. A
   *precision of 12 is the sweet spot in the py_debugging_toolchain_test unit
   *test.
   * @note the precision in the ModelInputDataSource ds_ shall be -2 the
   *precision that the results match! -2 is the sweet spot in the unit test.
   **/

  CplexWrapper(std::string modpath, std::string modfile,
               ParameterSource parameterSource, const int precision)
      : modfile_(modpath + modfile),
        datfile_(""),
        env_(IloEnv()),
        precision_(precision),
        ds_(env_, precision - 2),  // -2
        parameterSource_(parameterSource),
        rawResults_(std::make_shared<RawResults>()),
        print_debug_outputs_(false),
        doWarmstart_(NO_WARMSTART),
        warmstartValues_(std::make_shared<RawResults>()),
        cplexHandlerOut_(),
        handler_(env_, cplexHandlerOut_),
        modelSource_(env_, modfile_.c_str()),
        settings_(env_, handler_),
        def_(modelSource_, settings_),
        solutionProperties_(),
        debugOutputFilePath_(""),
        debugOutputFilePrefix_(""),
        debugOutputParameterFilePath_(""),
        useSpecialOrderedSets_(false),
        tmpWarmstartFile_("/tmp/warmstart_debug_res.mst"),
        useBranchingPriorities_(false),
        increasedBranchingPriorityValue_(1),
        branchingPriorityExtent_(1),
        bufferCplexOutputsToStream_(false) {
    settings_.setWithNames(IloTrue);
    settings_.setDisplayPrecision(precision);
  }

  CplexWrapper(std::string modfile, ParameterSource parameterSource,
               const int precision)
      : CplexWrapper("cplexmodel/", modfile, parameterSource, precision) {
  }  // due to the bazel structure this is
     // the correct relative location....

  CplexWrapper(std::string modfile, const int precision)
      : CplexWrapper(modfile, ParameterSource::DATFILE, precision) {}

  CplexWrapper() : CplexWrapper("", ParameterSource::CPPINPUTS, 12) {}

  CplexWrapper(const CplexWrapper& cp2)
      : modfile_(cp2.modfile_),
        datfile_(cp2.datfile_),
        env_(IloEnv()),  // HACK
        precision_(cp2.precision_),
        ds_(env_, cp2.precision_),  // HACK
        parameterSource_(cp2.parameterSource_),
        rawResults_(std::make_shared<RawResults>()),
        print_debug_outputs_(cp2.print_debug_outputs_),
        doWarmstart_(cp2.doWarmstart_),
        warmstartValues_(std::make_shared<RawResults>()),
        cplexHandlerOut_(cp2.cplexHandlerOut_.str().c_str()),
        handler_(env_, cplexHandlerOut_),
        modelSource_(env_, modfile_.c_str()),
        settings_(env_, handler_),
        def_(modelSource_, settings_),
        solutionProperties_(cp2.solutionProperties_),
        debugOutputFilePath_(cp2.debugOutputFilePath_),
        debugOutputFilePrefix_(cp2.debugOutputFilePrefix_),
        debugOutputParameterFilePath_(cp2.debugOutputParameterFilePath_),
        tmpWarmstartFile_(cp2.tmpWarmstartFile_),
        useBranchingPriorities_(cp2.useBranchingPriorities_),
        increasedBranchingPriorityValue_(cp2.increasedBranchingPriorityValue_),
        branchingPriorityExtent_(cp2.branchingPriorityExtent_),
        bufferCplexOutputsToStream_(cp2.bufferCplexOutputsToStream_) {
    settings_.setWithNames(IloTrue);
    settings_.setDisplayPrecision(precision_);
  }

  CplexWrapper& operator=(const CplexWrapper& rhs) {
    this->debugOutputParameterFilePath_ = rhs.debugOutputParameterFilePath_;
    return *this;
  }

  ~CplexWrapper() { env_.end(); }

 private:
  std::string modfile_;
  std::string datfile_;
  IloEnv env_;
  int precision_;
  ModelInputDataSource ds_;
  ParameterSource parameterSource_;
  std::shared_ptr<RawResults> rawResults_;
  bool print_debug_outputs_;
  WarmstartType doWarmstart_;
  std::shared_ptr<RawResults> warmstartValues_;
  std::stringstream cplexHandlerOut_;
  IloOplErrorHandler handler_;
  IloOplModelSource modelSource_;
  IloOplSettings settings_;
  IloOplModelDefinition def_;
  SolutionProperties solutionProperties_;
  std::string debugOutputFilePath_;
  std::string debugOutputFilePrefix_;
  std::string debugOutputParameterFilePath_;
  bool useSpecialOrderedSets_;
  std::string tmpWarmstartFile_;
  bool useBranchingPriorities_;
  int increasedBranchingPriorityValue_;
  int branchingPriorityExtent_;  // from 1 to N-1
  bool bufferCplexOutputsToStream_;

 public:
  void addRecedingHorizonWarmstart(
      std::shared_ptr<RawResults> warmstart,
      WarmstartType wt = RECEDING_HORIZON_WARMSTART);

  void setLastSolutionWarmstart(WarmstartType wt = LAST_SOLUTION_WARMSTART);

  void deleteLastSolutionWarmstartFile();

  void setParameterDatFileRelative(const char* datfile);

  void setParameterDatFileAbsolute(const char* datfile);

  void resetParameters(std::shared_ptr<ModelParameters> parameters);

  OptimizationStatus callCplex(const double timestemp = 0.0);

  std::shared_ptr<RawResults> getRawResults() const { return rawResults_; }

  SolutionProperties getSolutionProperties() const {
    return solutionProperties_;
  }

  void setDebugOutputPrint(bool print_debug_outputs) {
    print_debug_outputs_ = print_debug_outputs;
  }

  void setDebugOutputFilePath(std::string in);

  void setDebugOutputFilePrefix(std::string in) { debugOutputFilePrefix_ = in; }

  std::string getDebugOutputParameterFilePath() const {
    return debugOutputParameterFilePath_;
  }

  void setSpecialOrderedSets(bool in) { useSpecialOrderedSets_ = in; }

  void setUseBranchingPriorities(bool in) { useBranchingPriorities_ = in; }

  std::string getTmpWarmstartFile() { return tmpWarmstartFile_; }

  void setBranchingPriorityValueExtent(int value, int extent);

  void setBufferCplexOutputsToStream(bool in) {
    bufferCplexOutputsToStream_ = in;
  }

  void overrideSolverSettingsDataSource(
      std::shared_ptr<ModelParameters> parameters);

 private:
  void initializeWarmstart(IloOplModel& opl, IloCplex& cplex);

  template <class TensorType, class IloVarMapType>
  void initializeWarmstartElement(IloVarMapType& varMap, TensorType& vals,
                                  IloNumVarArray& startVar,
                                  IloNumArray& startVal, size_t dimDepth,
                                  std::vector<size_t> dimIdx);

  template <class TensorType>
  void getDecisionVariable(const IloOplElement& element, TensorType& outdata);

  // getNdimDecisionVariable: Put content from a IloMap into an EigenTensor
  // NOTE: no size checking! sizes have to be a priori correct!
  template <class TensorType, class IloMapType>
  void getNdimDecisionVariable(IloMapType m, TensorType& outdata,
                               std::vector<int> dimIdx);

  void setParametersFromDatfile(IloOplModel& opl);

  void setParametersFromModelInputDataSource(IloOplModel& opl);

  void setParallelMode(IloCplex& cplex);

  void collectRawResults(IloOplModel& opl);

  void collectSolutionStatus(const IloCplex& cplex, const double& solutiontime);

  void collectCplexStatistics(const IloCplex& cplex);

  void addSOSConstraints(IloOplModel& opl);

  template <class TensorType>
  void setBranchingPriorityFromTensor(IloCplex& cplex, IloIntVarMap& varMap,
                                      TensorType& vals, size_t dimDepth,
                                      std::vector<size_t> dimIdx);

  void setBranchingPriorities(IloOplModel& opl, IloCplex& cplex);
};

}  // namespace cplex
}  // namespace planner
}  // namespace miqp

#endif  // CPLEX_WRAPPER_HEADER


// Copyright 2015 National ICT Australia Limited (NICTA)
//
// 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 <SgtCore/Common.h>
#include <SgtCore/json.h>
#include <SgtCore/Network.h>
#include <SgtCore/NetworkParser.h>
#include <SgtCore/Parser.h>

#include <cpprest/http_listener.h>
#include <cpprest/uri.h>
#include <cpprest/asyncrt_utils.h>

#include <boost/filesystem.hpp>

#include <map>
#include <regex>

using namespace arma;
using namespace Sgt;
using namespace std;

using namespace web;
using namespace http;
using namespace utility;
using namespace http::experimental::listener;

using Json = Sgt::json;

namespace
{
    Col<double> argDeg(const Col<Complex>& x)
    {
        Col<double> result(x.size(), fill::none);
        transform(x.begin(), x.end(), result.begin(), [](const Complex& y)->double{return arg(y);});
        return result * 180 / pi;
    }

    void addProperties()
    {

        Bus::sProperties().addGetProperty<Bus, Col<double>>(
                [](const Bus& bus){return abs(bus.V());}, "VMag_kV");

        Bus::sProperties().addGetProperty<Bus, Col<double>>(
                [](const Bus& bus){return abs(bus.V())/bus.VBase();}, "VMagPu");

        Bus::sProperties().addGetProperty<Bus, double>(
                [&](const Bus& bus){return rms(bus.V());}, "VRms_kV");

        Bus::sProperties().addGetProperty<Bus, double>(
                [&](const Bus& bus){return rms(bus.V())/bus.VBase();}, "VRmsPu");

        Bus::sProperties().addGetProperty<Bus, Col<double>>(
                [&](const Bus& bus)->Col<double>{return argDeg(bus.V());}, "VAngDeg");

        Bus::sProperties().addGetProperty<Bus, double>(
                [&](const Bus& bus){return real(bus.SZipTot() - bus.SGenTot());}, "PTot");

        GenericZip::sProperties().addGetProperty<GenericZip, double>(
                [&](const GenericZip& zip){return accu(real(zip.SConst()));}, "PTot");

        GenericGen::sProperties().addGetProperty<GenericGen, double>(
                [&](const GenericGen& gen){return sum(real(gen.S()));}, "PTot");
    }

    template<typename T> Json componentPropsJson(const T& comp)
    {
        Json result;
        for (const auto& propPair : comp.properties())
        {
            result[propPair.first] = propPair.second->json(comp);
        }
        return result;
    }

    template<typename T> Json componentJson(const T& comp)
    {
        return componentPropsJson(comp);
    }

    template<> Json componentJson<BranchAbc>(const BranchAbc& comp)
    {
        Json result = componentPropsJson(comp);
        result["bus0"] = comp.bus0() != nullptr ? Json(comp.bus0()->id()) : Json();
        result["bus1"] = comp.bus1() != nullptr ? Json(comp.bus1()->id()) : Json();
        return result;
    }

    template<> Json componentJson<Bus>(const Bus& comp)
    {
        Json result = componentPropsJson(comp);

        auto branches0 = comp.branches0();
        auto branches1 = comp.branches1();
        auto gens = comp.gens();
        auto zips = comp.zips();

        auto getId = [](const Component* x){return x->id();};

        vector<string> branch0Ids;
        transform(branches0.begin(), branches0.end(), back_inserter(branch0Ids), getId);
        result["branches0"] = branch0Ids;

        vector<string> branch1Ids;
        transform(branches1.begin(), branches1.end(), back_inserter(branch1Ids), getId);
        result["branches1"] = branch1Ids;

        vector<string> genIds;
        transform(gens.begin(), gens.end(), back_inserter(genIds), getId);
        result["gens"] = genIds;

        vector<string> zipIds;
        transform(zips.begin(), zips.end(), back_inserter(zipIds), getId);
        result["zips"] = zipIds;

        return result;
    }

    template<> Json componentJson<GenAbc>(const GenAbc& comp)
    {
        Json result = componentPropsJson(comp);
        result["bus"] = comp.bus() != nullptr ? Json(comp.bus()->id()) : Json();
        return result;
    }

    template<> Json componentJson<ZipAbc>(const ZipAbc& comp)
    {
        Json result = componentPropsJson(comp);
        result["bus"] = comp.bus() != nullptr ? Json(comp.bus()->id()) : Json();
        return result;
    }

    template<typename T> Json componentsJson(const T& compVec)
    {
        Json result;
        for (auto comp : compVec)
        {
            result.push_back(componentJson(*comp));
        }
        return result;
    }
}

class SgtServer
{
public:
    SgtServer(const string& url, const string& dataDir);

    pplx::task<void> open() {return listener_.open();}
    pplx::task<void> close() {return listener_.close();}

private:
    void handleGet(http_request message);
    void handlePut(http_request message);
    // void handlePost(http_request message);
    // void handleDelete(http_request message);

    Json getInputFiles(const std::string& suffix);

private:
    http_listener listener_;
    unique_ptr<Network> netw_;
    boost::filesystem::path dataDir_;
};

static unique_ptr<SgtServer> gServer;

void on_initialize(const string& address, const string& dataDir)
{
    uri_builder uri(address);

    auto addr = uri.to_uri().to_string();
    gServer = unique_ptr<SgtServer>(new SgtServer(addr, dataDir));
    gServer->open().wait();

    cout << "Listening for requests at: " << addr << endl;
}

void on_shutdown()
{
    gServer->close().wait();
}

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

    string port = "34568";

    string address = "http://localhost:";
    string dataDir(argv[1]);

    address.append(port);

    on_initialize(address, dataDir);
    cout << "Press ENTER to exit." << endl;

    string line;
    getline(cin, line);

    on_shutdown();
    return 0;
}

SgtServer::SgtServer(const string& url, const string& dataDir) :
    listener_(url),
    dataDir_(dataDir.c_str())
{
    addProperties();

    listener_.support(methods::GET, bind(&SgtServer::handleGet, this, placeholders::_1));
    listener_.support(methods::PUT, bind(&SgtServer::handlePut, this, placeholders::_1));
}

void SgtServer::handleGet(http_request message)
{
    cout << "GET received." << endl;
    auto pathsVec = http::uri::split_path(http::uri::decode(message.relative_uri().path()));
    auto query = web::uri::split_query(message.relative_uri().query());

    status_code status = status_codes::OK;
    Json reply;
    try
    {
        std::string view = query.at("view");
        if (view == "network")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                reply = {
                    {"branches", componentsJson(netw_->branches())},
                    {"buses", componentsJson(netw_->buses())},
                    {"gens", componentsJson(netw_->gens())},
                    {"zips", componentsJson(netw_->zips())}};
            }
        }
        else if (view == "bus_voltages")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                for (auto b : netw_->buses())
                {
                    reply.push_back(Json(rms(b->V()) / b->VBase()));
                }
            }
        }
        else if (view == "zip_powers")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                auto& p = GenericZip::sProperties()["PTot"];
                for (auto z : netw_->zips())
                {
                    reply.push_back(p.json(*z));
                }
            }
        }
        else if (view == "gen_powers")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                auto& p = GenericGen::sProperties()["PTot"];
                for (auto g : netw_->gens())
                {
                    reply.push_back(p.json(*g));
                }
            }
        }
        else if (view == "branch")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                std::string id = query.at("id");
                reply = componentJson(*netw_->branches()[id]);
            }
        }
        else if (view == "bus")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                std::string id = query.at("id");
                reply = componentJson(*netw_->buses()[id]);
            }
        }
        else if (view == "gen")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                std::string id = query.at("id");
                reply = componentJson(*netw_->gens()[id]);
            }
        }
        else if (view == "zip")
        {
            if (netw_ == nullptr)
            {
                status = status_codes::NotFound;
            }
            else
            {
                std::string id = query.at("id");
                reply = componentJson(*netw_->zips()[id]);
            }
        }
        else if (view == "input_files")
        {
            std::string suffix = query.at("suffix");
            reply = getInputFiles(suffix);
        }
    }
    catch (out_of_range e)
    {
        status = status_codes::BadRequest;
    }
    message.reply(status, reply.dump(2));
}

void SgtServer::handlePut(http_request message)
{
    cout << "PUT received." << endl;
    auto query = web::uri::split_query(message.relative_uri().query());

    status_code status = status_codes::OK;
    Json reply;
    try
    {
        std::string action = query.at("action");
        if (action == "load_yaml")
        {
            std::string fName = query.at("file");
            netw_.reset(new Network);
            NetworkParser p;
            p.parse(dataDir_.string() + "/" + fName, *netw_);
        }
        else if (action == "load_matpower")
        {
            std::string fName = query.at("file");
            string yamlStr = string("--- [{matpower : {input_file : ")
                + dataDir_.string() + "/" + fName + ", default_kV_base : 11}}]";
            YAML::Node n = YAML::Load(yamlStr);
            netw_.reset(new Network);
            NetworkParser p;
            p.parse(n, *netw_);
        }
    }
    catch (out_of_range e)
    {
        status = status_codes::BadRequest;
    }
    message.reply(status, reply.dump(2));
}

Json SgtServer::getInputFiles(const std::string& suffix)
{
    using namespace boost::filesystem;
    directory_iterator it(dataDir_);
    vector<string> files;
    vector<string> mFiles;
    transform(it, directory_iterator(), back_inserter(files),
            [](decltype(*it)& entry){return entry.path().filename().string();});
    copy_if(files.begin(), files.end(), back_inserter(mFiles),
            [&suffix](const string& s){return regex_search(s, regex(".*\\." + suffix + "$"));});
    return Json(mFiles);
}


//
// Created by eriks on 7/24/17.
//

#include "velocity_transition_observer.hpp"
#include "interpolation.hpp"
#include "fileIO.hpp"
#include <boost/algorithm/clamp.hpp>
#include "initial_conditions/initial_conditions.hpp"
#include <boost/range/join.hpp>
#include <boost/range/adaptor/transformed.hpp>

VelocityTransitionData::VelocityTransitionData(std::size_t dimension, std::size_t bin_count, double range,
                                       std::vector<bool> in, std::vector<bool> out, bool increments) :
    mDimension(dimension), mRange(range), mBinCount(bin_count), mIncrementMode(increments)
{
    if(in.size() != dimension)
    {
        THROW_EXCEPTION(std::logic_error, "Invalid number of entries in 'in'");
    }

    if(out.size() != dimension)
    {
        THROW_EXCEPTION(std::logic_error, "Invalid number of entries in 'out'");
    }

    using namespace boost::adaptors;
    using namespace boost::range;

    auto extends_range = join(in, out) | transformed([this](bool use){ return use ? mBinCount : 1; });

    std::vector<std::size_t> extents(begin(extends_range), end(extends_range));
    mData = histogram_t(extents);

    // pre-calculate the bin centers.
    for(unsigned j = 0; j < mBinCount; ++j) {
        double bin_center = double(j) / (mBinCount - 1) * 2 - 1;
        mBinCenters.push_back(bin_center * mRange);
    }
}

int to_index(double value, std::size_t bin_count)
{
    double v = (boost::algorithm::clamp(value, -1.0, 1.0) + 1) / 2;
    return static_cast<int>(std::round(v * (bin_count - 1)));
}

void VelocityTransitionData::record(const gen_vect& old_velocity, const gen_vect& velocity)
{
    boost::numeric::ublas::c_vector<int, 6> index(old_velocity.size() + velocity.size());
    for(unsigned i = 0; i < old_velocity.size(); ++i)
    {
        index[i] = to_index(old_velocity[i] / mRange, mData.getExtents()[i]);
    }

    if(!mIncrementMode) {
        for (unsigned i = 0; i < velocity.size(); ++i) {
            index[i + old_velocity.size()] = to_index(velocity[i] / mRange,
                                                      mData.getExtents()[i + old_velocity.size()]);
        }
    }
    else
    {
        for (unsigned i = 0; i < velocity.size(); ++i) {
            index[i + old_velocity.size()] = to_index((velocity[i] - old_velocity[i]) / mRange,
                                                      mData.getExtents()[i + old_velocity.size()]);
        }
    }

    mData(index) += 1;
}

VelocityTransitionObserver::VelocityTransitionObserver(std::size_t dimension, double timeInterval, std::size_t bin_count,
                                                       double start_time, double end_time, std::vector<bool> in,
                                                       std::vector<bool> out, bool increment_mode, std::string file_name)
        : ThreadSharedObserver( std::move(file_name) ),
        mBinCount(bin_count), mDimension(dimension), mTimeInterval(timeInterval),
          mStartRecordingTime(start_time), mEndRecordingTime(end_time),
          mLastStepVelocity(dimension), mBinCounts(dimension, bin_count, 1.5, in, out, increment_mode),
          mVelocityRange(1.5)
{
    if(timeInterval <= 0)
    {
        THROW_EXCEPTION(std::logic_error, "Negative time interval passed to VelocityTransitionObserver");
    }
}

void VelocityTransitionObserver::startTrajectory( const InitialCondition& start, std::size_t )
{
    mStartTransitionTime = mEndRecordingTime;
    mLastStepTime = 0;
    mLastStepVelocity = start.getState().getVelocity();
}

bool VelocityTransitionObserver::watch( const State& state, double t )
{
    // allow for recording to start at arbitrary times. In that case, we need to
    // generate the initial velocity by doing an interpolation.
    // need to include mLastStepTime == mStartRecordingTime in case recording starts
    // at time 0.
    if(t >= mStartRecordingTime && mLastStepTime <= mStartRecordingTime) {
        double record_step = mStartRecordingTime - mLastStepTime;
        double time_step = t - mLastStepTime;
        double r = record_step / time_step;

        // in the very first timestep, in case mStartRecordingTime == 0, we can get time_step == 0
        // which causes NaNs.
        if(time_step < 1e-20) {
            r = 0.0;
        }

        auto interpol = interpolate_linear_1d(mLastStepVelocity, state.getVelocity(), r);
        mStartTransitionTime = mStartRecordingTime;
        mOldVelocity = interpol;
    }

    // otherwise, check for passing time boundary
    // we add a very small epsilon 1e-10 to the right side of the boundary, because
    // otherwise it is possible for transitions to be skipped. We expect the times
    // involved here to be significantly bigger than 1e-10.
    while( t >= mStartTransitionTime + mTimeInterval &&
            mStartTransitionTime + mTimeInterval <= mEndRecordingTime + 1e-10)
    {
        // linearly interpolated velocity between last and current state
        // this is important, if e have multiple observation times that
        // are closely spaced in an area where the adaptive time step
        // gets big.
        double record_step = mStartTransitionTime + mTimeInterval - mLastStepTime;
        double time_step = t - mLastStepTime;
        double r = record_step / time_step;

        auto interpol = interpolate_linear_1d(mLastStepVelocity, state.getVelocity(), r);
        mBinCounts.record(mOldVelocity, interpol);
        mStartTransitionTime += mTimeInterval;
        mOldVelocity = interpol;
    }

    mLastStepTime = t;
    mLastStepVelocity = state.getVelocity();

    return t < mEndRecordingTime;
}

void VelocityTransitionObserver::save(std::ostream& target)
{
    /*! Angular Velocity histogram save file format.
        Header: velh001\\n
        Data type   | Count | Meaning
        ---------   | ----- | -------
        Int [B]     | 1     | Number of bins per histogram
        Int [D]     | 1     | Number of dimensions
        Double      | 1     | Time interval
        Double      | B     | bin center velocities
        Grid[UInt32]| B ^ (D-1)| grids with transition data
    */

    // file header
    target << "velt002\n";
    writeInteger(target, mBinCount);
    writeInteger(target, mDimension);
    writeFloat(target, mTimeInterval);
    writeFloats(target, mBinCounts.bin_centers());

    mBinCounts.data().dump(target);
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// 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_EXTENSIONS_NSPHERE_ALGORITHMS_COVERED_BY_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_COVERED_BY_HPP

#include <boost/geometry/algorithms/covered_by.hpp>

#include <boost/geometry/extensions/nsphere/strategies/cartesian/nsphere_in_box.hpp>
#include <boost/geometry/extensions/nsphere/strategies/cartesian/point_in_nsphere.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename NSphere, typename Box>
struct covered_by<NSphere, Box, nsphere_tag, box_tag>
{
    template <typename Strategy>
    static inline bool apply(NSphere const& nsphere, Box const& box, Strategy const& strategy)
    {
        assert_dimension_equal<NSphere, Box>();
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(nsphere, box);
    }
};

template <typename Point, typename NSphere>
struct covered_by<Point, NSphere, point_tag, nsphere_tag>
{
    template <typename Strategy>
    static inline bool apply(Point const& point, NSphere const& nsphere, Strategy const& strategy)
    {
        assert_dimension_equal<Point, NSphere>();
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(point, nsphere);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_COVERED_BY_HPP