#include <Functions/FunctionFactory.h>
#include <Functions/PolygonUtils.h>
#include <Functions/FunctionHelpers.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnFixedString.h>
#include <Columns/ColumnString.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnsNumber.h>
#include <Common/ObjectPool.h>
#include <Common/ProfileEvents.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeString.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypesNumber.h>
#include <IO/WriteHelpers.h>
#include <Interpreters/ExpressionActions.h>
#include <Interpreters/Context.h>
#include <Interpreters/castColumn.h>

#include <string>
#include <memory>

namespace ProfileEvents
{
    extern const Event PolygonsAddedToPool;
    extern const Event PolygonsInPoolAllocatedBytes;
}

namespace DB
{
namespace ErrorCodes
{
    extern const int TOO_FEW_ARGUMENTS_FOR_FUNCTION;
    extern const int BAD_ARGUMENTS;
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int ILLEGAL_COLUMN;
}

namespace
{

using CoordinateType = Float64;
using Point = boost::geometry::model::d2::point_xy<CoordinateType>;
using Polygon = boost::geometry::model::polygon<Point, false>;
using Box = boost::geometry::model::box<Point>;

template <typename PointInConstPolygonImpl>
class FunctionPointInPolygon : public IFunction
{
public:
    static inline const char * name = "pointInPolygon";

    explicit FunctionPointInPolygon(bool validate_) : validate(validate_) {}

    static FunctionPtr create(const Context & context)
    {
        return std::make_shared<FunctionPointInPolygon<PointInConstPolygonImpl>>(
            context.getSettingsRef().validate_polygons);
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return true;
    }

    size_t getNumberOfArguments() const override
    {
        return 0;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes & arguments) const override
    {
        if (arguments.size() < 2)
        {
            throw Exception("Too few arguments", ErrorCodes::TOO_FEW_ARGUMENTS_FOR_FUNCTION);
        }

        /** We allow function invocation in one of the following forms:
          *
          * pointInPolygon((x, y), [(x1, y1), (x2, y2), ...])
          * - simple polygon
          * pointInPolygon((x, y), [(x1, y1), (x2, y2), ...], [(x21, y21), (x22, y22), ...], ...)
          * - polygon with a number of holes, each hole as a subsequent argument.
          * pointInPolygon((x, y), [[(x1, y1), (x2, y2), ...], [(x21, y21), (x22, y22), ...], ...])
          * - polygon with a number of holes, all as multidimensional array
          */

        auto validate_tuple = [this](size_t i, const DataTypeTuple * tuple)
        {
            if (tuple == nullptr)
                throw Exception(getMessagePrefix(i) + " must contain a tuple", ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

            const DataTypes & elements = tuple->getElements();

            if (elements.size() != 2)
                throw Exception(getMessagePrefix(i) + " must have exactly two elements", ErrorCodes::BAD_ARGUMENTS);

            for (auto j : ext::range(0, elements.size()))
            {
                if (!isNativeNumber(elements[j]))
                {
                    throw Exception(getMessagePrefix(i) + " must contain numeric tuple at position " + toString(j + 1),
                                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);
                }
            }
        };

        validate_tuple(0, checkAndGetDataType<DataTypeTuple>(arguments[0].get()));

        if (arguments.size() == 2)
        {
            const auto * array = checkAndGetDataType<DataTypeArray>(arguments[1].get());
            if (array == nullptr)
                throw Exception(getMessagePrefix(1) + " must contain an array of tuples or an array of arrays of tuples.",
                                ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

            const auto * nested_array = checkAndGetDataType<DataTypeArray>(array->getNestedType().get());
            if (nested_array != nullptr)
            {
                array = nested_array;
            }

            validate_tuple(1, checkAndGetDataType<DataTypeTuple>(array->getNestedType().get()));
        }
        else
        {
            for (size_t i = 1; i < arguments.size(); i++)
            {
                const auto * array = checkAndGetDataType<DataTypeArray>(arguments[i].get());
                if (array == nullptr)
                    throw Exception(getMessagePrefix(i) + " must contain an array of tuples",
                                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);

                validate_tuple(i, checkAndGetDataType<DataTypeTuple>(array->getNestedType().get()));
            }
        }

        return std::make_shared<DataTypeUInt8>();
    }

    ColumnPtr executeImpl(ColumnsWithTypeAndName & arguments, const DataTypePtr & result_type, size_t input_rows_count) const override
    {
        const IColumn * point_col = arguments[0].column.get();
        const auto * const_tuple_col = checkAndGetColumn<ColumnConst>(point_col);
        if (const_tuple_col)
            point_col = &const_tuple_col->getDataColumn();

        const auto * tuple_col = checkAndGetColumn<ColumnTuple>(point_col);
        if (!tuple_col)
            throw Exception("First argument for function " + getName() + " must be constant array of tuples.",
                            ErrorCodes::ILLEGAL_COLUMN);

        const auto & tuple_columns = tuple_col->getColumns();

        const ColumnWithTypeAndName poly = arguments[1];
        const IColumn * poly_col = poly.column.get();
        const ColumnConst * const_poly_col = checkAndGetColumn<ColumnConst>(poly_col);

        bool point_is_const = const_tuple_col != nullptr;
        bool poly_is_const = const_poly_col != nullptr;

        /// Two different algorithms are used for constant and non constant polygons.
        /// Constant polygons are preprocessed to speed up matching.
        /// For non-constant polygons, we cannot spend time for preprocessing
        ///  and have to quickly match on the fly without creating temporary data structures.

        if (poly_is_const)
        {
            Polygon polygon;
            parseConstPolygon(arguments, polygon);

            /// Polygons are preprocessed and saved in cache.
            /// Preprocessing can be computationally heavy but dramatically speeds up matching.

            using Pool = ObjectPoolMap<PointInConstPolygonImpl, UInt128>;
            /// C++11 has thread-safe function-local statics.
            static Pool known_polygons;

            auto factory = [&polygon]()
            {
                auto ptr = std::make_unique<PointInConstPolygonImpl>(polygon);

                ProfileEvents::increment(ProfileEvents::PolygonsAddedToPool);
                ProfileEvents::increment(ProfileEvents::PolygonsInPoolAllocatedBytes, ptr->getAllocatedBytes());

                return ptr.release();
            };

            auto impl = known_polygons.get(sipHash128(polygon), factory);

            if (point_is_const)
            {
                bool is_in = impl->contains(tuple_columns[0]->getFloat64(0), tuple_columns[1]->getFloat64(0));
                return result_type->createColumnConst(input_rows_count, is_in);
            }
            else
            {
                return pointInPolygon(*tuple_columns[0], *tuple_columns[1], *impl);
            }
        }
        else
        {
            if (arguments.size() != 2)
                throw Exception("Multi-argument version of function " + getName() + " works only with const polygon",
                    ErrorCodes::BAD_ARGUMENTS);

            auto res_column = ColumnVector<UInt8>::create(input_rows_count);
            auto & data = res_column->getData();

            /// A polygon, possibly with holes, is represented by 2d array:
            /// [[(outer_x_1, outer_y_1, ...)], [(hole1_x_1, hole1_y_1), ...], ...]
            ///
            /// Or, a polygon without holes can be represented by 1d array:
            /// [(outer_x_1, outer_y_1, ...)]

            if (isTwoDimensionalArray(*arguments[1].type))
            {
                /// We cast everything to Float64 in advance (in batch fashion)
                ///  to avoid casting with virtual calls in a loop.
                /// Note that if the type is already Float64, the operation in noop.

                ColumnPtr polygon_column_float64 = castColumn(
                    arguments[1],
                    std::make_shared<DataTypeArray>(
                        std::make_shared<DataTypeArray>(
                            std::make_shared<DataTypeTuple>(DataTypes{
                                std::make_shared<DataTypeFloat64>(),
                                std::make_shared<DataTypeFloat64>()}))));

                for (size_t i = 0; i < input_rows_count; ++i)
                {
                    size_t point_index = point_is_const ? 0 : i;
                    data[i] = isInsidePolygonWithHoles(
                        tuple_columns[0]->getFloat64(point_index),
                        tuple_columns[1]->getFloat64(point_index),
                        *polygon_column_float64,
                        i);
                }
            }
            else
            {
                ColumnPtr polygon_column_float64 = castColumn(
                    arguments[1],
                    std::make_shared<DataTypeArray>(
                        std::make_shared<DataTypeTuple>(DataTypes{
                            std::make_shared<DataTypeFloat64>(),
                            std::make_shared<DataTypeFloat64>()})));

                for (size_t i = 0; i < input_rows_count; ++i)
                {
                    size_t point_index = point_is_const ? 0 : i;
                    data[i] = isInsidePolygonWithoutHoles(
                        tuple_columns[0]->getFloat64(point_index),
                        tuple_columns[1]->getFloat64(point_index),
                        *polygon_column_float64,
                        i);
                }
            }

            return res_column;
        }
    }

private:
    bool validate;

    std::string getMessagePrefix(size_t i) const
    {
        return "Argument " + toString(i + 1) + " for function " + getName();
    }

    bool isTwoDimensionalArray(const IDataType & type) const
    {
        return WhichDataType(type).isArray()
            && WhichDataType(static_cast<const DataTypeArray &>(type).getNestedType()).isArray();
    }

    /// Implementation methods to check point-in-polygon on the fly (for non-const polygons).

    bool isInsideRing(
        Float64 point_x,
        Float64 point_y,
        const Float64 * ring_x_data,
        const Float64 * ring_y_data,
        size_t ring_begin,
        size_t ring_end) const
    {
        size_t size = ring_end - ring_begin;

        if (size < 2)
            return false;

        /** This is the algorithm by W. Randolph Franklin
          * https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
          *
          * Basically it works like this:
          * From the point, cast a horizontal ray to the right
          *  and count the number of intersections with polygon edges
          *  (every edge is considered semi-closed, e.g. includes the first vertex and does not include the last)
          *
          * Advantages:
          * - works regardless to the orientation;
          * - for polygon without holes:
          *   works regardless to whether the polygon is closed by last vertex equals to first vertex or not;
          *   (no need to preprocess polygon in any way)
          * - easy to apply for polygons with holes and for multi-polygons;
          * - it even works for polygons with self-intersections in a reasonable way;
          * - simplicity and performance;
          * - can be additionally speed up with loop unrolling and/or binary search for possible intersecting edges.
          *
          * Drawbacks:
          * - it's unspecified whether a point of the edge is inside or outside of a polygon
          *   (looks like it's inside for "left" edges and outside for "right" edges)
          *
          * Why not to apply the same algorithm available in boost::geometry?
          * It will require to move data from columns to temporary containers.
          * Despite the fact that the boost library is template based and allows arbitrary containers and points,
          *  it's diffucult to use without data movement because
          *  we use structure-of-arrays for coordinates instead of arrays-of-structures.
          */

        size_t vertex1_idx = ring_begin;
        size_t vertex2_idx = ring_end - 1;
        bool res = false;

        while (vertex1_idx < ring_end)
        {
            /// First condition checks that the point is inside horizontal row between edge top and bottom y-coordinate.
            /// Second condition checks for intersection with the edge.

            if (((ring_y_data[vertex1_idx] > point_y) != (ring_y_data[vertex2_idx] > point_y))
                && (point_x < (ring_x_data[vertex2_idx] - ring_x_data[vertex1_idx])
                    * (point_y - ring_y_data[vertex1_idx]) / (ring_y_data[vertex2_idx] - ring_y_data[vertex1_idx])
                    + ring_x_data[vertex1_idx]))
            {
                res = !res;
            }

            vertex2_idx = vertex1_idx;
            ++vertex1_idx;
        }

        return res;
    }

    bool isInsidePolygonWithoutHoles(
        Float64 point_x,
        Float64 point_y,
        const IColumn & polygon_column,
        size_t i) const
    {
        const auto & array_col = static_cast<const ColumnArray &>(polygon_column);

        size_t begin = array_col.getOffsets()[i - 1];
        size_t end = array_col.getOffsets()[i];
        size_t size = end - begin;

        if (size < 2)
            return false;

        const auto & tuple_columns = static_cast<const ColumnTuple &>(array_col.getData()).getColumns();
        const auto * x_data = static_cast<const ColumnFloat64 &>(*tuple_columns[0]).getData().data();
        const auto * y_data = static_cast<const ColumnFloat64 &>(*tuple_columns[1]).getData().data();

        return isInsideRing(point_x, point_y, x_data, y_data, begin, end);
    }

    bool isInsidePolygonWithHoles(
        Float64 point_x,
        Float64 point_y,
        const IColumn & polygon_column,
        size_t i) const
    {
        const auto & array_col = static_cast<const ColumnArray &>(polygon_column);
        size_t rings_begin = array_col.getOffsets()[i - 1];
        size_t rings_end = array_col.getOffsets()[i];

        const auto & nested_array_col = static_cast<const ColumnArray &>(array_col.getData());
        const auto & tuple_columns = static_cast<const ColumnTuple &>(nested_array_col.getData()).getColumns();
        const auto * x_data = static_cast<const ColumnFloat64 &>(*tuple_columns[0]).getData().data();
        const auto * y_data = static_cast<const ColumnFloat64 &>(*tuple_columns[1]).getData().data();

        for (size_t j = rings_begin; j < rings_end; ++j)
        {
            size_t begin = nested_array_col.getOffsets()[j - 1];
            size_t end = nested_array_col.getOffsets()[j];

            if (j == rings_begin)
            {
                if (!isInsideRing(point_x, point_y, x_data, y_data, begin, end))
                    return false;
            }
            else
            {
                if (isInsideRing(point_x, point_y, x_data, y_data, begin, end))
                    return false;
            }
        }

        return true;
    }

    /// Implementation methods to create boost::geometry::polygon for subsequent preprocessing.
    /// They are used to optimize matching for constant polygons. Preprocessing may take significant amount of time.

    template <typename T>
    void parseRing(
        const Float64 * x_data,
        const Float64 * y_data,
        size_t begin,
        size_t end,
        T & out_container) const
    {
        out_container.reserve(end - begin);
        for (size_t i = begin; i < end; ++i)
            out_container.emplace_back(x_data[i], y_data[i]);
    }

    void parseConstPolygonWithoutHolesFromSingleColumn(const IColumn & column, size_t i, Polygon & out_polygon) const
    {
        const auto & array_col = static_cast<const ColumnArray &>(column);
        size_t begin = array_col.getOffsets()[i - 1];
        size_t end = array_col.getOffsets()[i];

        const auto & tuple_columns = static_cast<const ColumnTuple &>(array_col.getData()).getColumns();
        const auto * x_data = static_cast<const ColumnFloat64 &>(*tuple_columns[0]).getData().data();
        const auto * y_data = static_cast<const ColumnFloat64 &>(*tuple_columns[1]).getData().data();

        parseRing(x_data, y_data, begin, end, out_polygon.outer());
    }

    void parseConstPolygonWithHolesFromSingleColumn(const IColumn & column, size_t i, Polygon & out_polygon) const
    {
        const auto & array_col = static_cast<const ColumnArray &>(column);
        size_t rings_begin = array_col.getOffsets()[i - 1];
        size_t rings_end = array_col.getOffsets()[i];

        const auto & nested_array_col = static_cast<const ColumnArray &>(array_col.getData());
        const auto & tuple_columns = static_cast<const ColumnTuple &>(nested_array_col.getData()).getColumns();
        const auto * x_data = static_cast<const ColumnFloat64 &>(*tuple_columns[0]).getData().data();
        const auto * y_data = static_cast<const ColumnFloat64 &>(*tuple_columns[1]).getData().data();

        for (size_t j = rings_begin; j < rings_end; ++j)
        {
            size_t begin = nested_array_col.getOffsets()[j - 1];
            size_t end = nested_array_col.getOffsets()[j];

            if (out_polygon.outer().empty())
            {
                parseRing(x_data, y_data, begin, end, out_polygon.outer());
            }
            else
            {
                out_polygon.inners().emplace_back();
                parseRing(x_data, y_data, begin, end, out_polygon.inners().back());
            }
        }
    }

    void parseConstPolygonWithHolesFromMultipleColumns(ColumnsWithTypeAndName & arguments, Polygon & out_polygon) const
    {
        for (size_t i = 1; i < arguments.size(); ++i)
        {
            const auto * const_col = checkAndGetColumn<ColumnConst>(arguments[i].column.get());
            if (!const_col)
                throw Exception("Multi-argument version of function " + getName() + " works only with const polygon",
                    ErrorCodes::BAD_ARGUMENTS);

            const auto * array_col = checkAndGetColumn<ColumnArray>(&const_col->getDataColumn());
            const auto * tuple_col = array_col ? checkAndGetColumn<ColumnTuple>(&array_col->getData()) : nullptr;

            if (!tuple_col)
                throw Exception(getMessagePrefix(i) + " must be constant array of tuples", ErrorCodes::ILLEGAL_COLUMN);

            const auto & tuple_columns = tuple_col->getColumns();
            const auto & column_x = tuple_columns[0];
            const auto & column_y = tuple_columns[1];

            if (!out_polygon.outer().empty())
                out_polygon.inners().emplace_back();

            auto & container = out_polygon.outer().empty() ? out_polygon.outer() : out_polygon.inners().back();

            auto size = column_x->size();

            if (size == 0)
                throw Exception(getMessagePrefix(i) + " shouldn't be empty.", ErrorCodes::ILLEGAL_COLUMN);

            for (auto j : ext::range(0, size))
            {
                CoordinateType x_coord = column_x->getFloat64(j);
                CoordinateType y_coord = column_y->getFloat64(j);
                container.push_back(Point(x_coord, y_coord));
            }
        }
    }

    void parseConstPolygonFromSingleColumn(ColumnsWithTypeAndName & arguments, Polygon & out_polygon) const
    {
        if (isTwoDimensionalArray(*arguments[1].type))
        {
            ColumnPtr polygon_column_float64 = castColumn(
                arguments[1],
                std::make_shared<DataTypeArray>(
                    std::make_shared<DataTypeArray>(
                        std::make_shared<DataTypeTuple>(DataTypes{
                            std::make_shared<DataTypeFloat64>(),
                            std::make_shared<DataTypeFloat64>()}))));

            const ColumnConst & column_const = typeid_cast<const ColumnConst &>(*polygon_column_float64);
            const IColumn & column_const_data = column_const.getDataColumn();

            parseConstPolygonWithHolesFromSingleColumn(column_const_data, 0, out_polygon);
        }
        else
        {
            ColumnPtr polygon_column_float64 = castColumn(
                arguments[1],
                std::make_shared<DataTypeArray>(
                    std::make_shared<DataTypeTuple>(DataTypes{
                        std::make_shared<DataTypeFloat64>(),
                        std::make_shared<DataTypeFloat64>()})));

            const ColumnConst & column_const = typeid_cast<const ColumnConst &>(*polygon_column_float64);
            const IColumn & column_const_data = column_const.getDataColumn();

            parseConstPolygonWithoutHolesFromSingleColumn(column_const_data, 0, out_polygon);
        }
    }

    void parseConstPolygon(ColumnsWithTypeAndName & arguments, Polygon & out_polygon) const
    {
        if (arguments.size() == 2)
            parseConstPolygonFromSingleColumn(arguments, out_polygon);
        else
            parseConstPolygonWithHolesFromMultipleColumns(arguments, out_polygon);

        /// Fix orientation and close rings. It's required for subsequent processing.
        boost::geometry::correct(out_polygon);

#if !defined(__clang_analyzer__) /// It does not like boost.
        if (validate)
        {
            std::string failure_message;
            auto is_valid = boost::geometry::is_valid(out_polygon, failure_message);
            if (!is_valid)
                throw Exception("Polygon is not valid: " + failure_message, ErrorCodes::BAD_ARGUMENTS);
        }
#endif
    }
};

}

void registerFunctionPointInPolygon(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPointInPolygon<PointInPolygonWithGrid<Float64>>>();
}

}


#include "gshhg.hpp"

#include <shapefil.h>

#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <fstream>
#include <iostream>

namespace gshhg {

GSHHG::GSHHG(const std::string& dirname,
             const std::optional<std::string>& resolution,
             const std::optional<std::vector<int>>& levels,
             std::optional<Box> bbox)
    : bbox_(std::move(bbox)) {
  auto resolution_ident =
      parse_resolution_string(resolution.value_or("intermediate"));
  auto resolution_code = std::string(1, static_cast<char>(resolution_ident));
  auto points = std::vector<Cartesian>();

  // For all hierarchical levels
  for (auto level = 1; level < 7; ++level) {
    // Does the user want to filter the levels to be loaded?
    if (levels) {
      const auto it = std::find(levels->begin(), levels->end(), level);
      if (it == levels->end()) {
        continue;
      }
    }

    // No boundary between pond-in-island and island in crude resolution
    if (resolution_ident == Resolution::kCrude && level == 4) {
      continue;
    }

    // Build the path to the ESRI shape files
    std::filesystem::path path =
        dirname / std::filesystem::path(resolution_code) /
        std::filesystem::path("GSHHS_" + resolution_code + "_L" +
                              std::to_string(level) + ".shp");

    // Load the hierarchical dataset selected
    load_shp(path.string(), level,
             // Level 5 at full resolution must be patched.
             resolution_ident == Resolution::kFull && level == 5, points);
  }
  rtree_.reset(new RTree(points));
}

// Calculate the ECEF coordinates of the polygon points
static inline auto transform_polygon_points(const Polygon& polygon,
                                            std::vector<Cartesian>& points)
    -> void {
  for (const auto& point : polygon.outer()) {
    const auto ecef = geodetic_2_cartesian(
        geodetic_2_radian(GeodeticDegree(point.get<0>(), point.get<1>())));
    points.emplace_back(ecef);
  }
}

void GSHHG::load_shp(const std::string& filename, const uint8_t level,
                     const bool patch, std::vector<Cartesian>& points) {
  SHPHandle handle = SHPOpen(filename.c_str(), "rb");
  if (handle == nullptr) {
    throw std::system_error(ENOENT, std::system_category(), filename);
  }
  int shape_types, entities;
  double min_bound[4], max_bound[4];

  // Read file properties
  SHPGetInfo(handle, &entities, &shape_types, min_bound, max_bound);

  // Skim over the list of shapes
  for (int ix = 0; ix < entities; ++ix) {
    SHPObject* shape = SHPReadObject(handle, ix);
    if (shape == nullptr ||
        (shape->nParts > 0 && shape->panPartStart[0] != 0)) {
      SHPDestroyObject(shape);
      SHPClose(handle);
      throw std::runtime_error("unable to read shape " + std::to_string(ix));
    }

    if (shape->nSHPType == SHPT_POLYGON && shape->nVertices) {
      const auto* x = shape->padfX;
      const auto* y = shape->padfY;

      // Current polygon read
      auto polygon = Polygon();

      // Skim over vertices
      for (int jx = 0; jx < shape->nVertices; ++jx) {
        // Level 5 at full resolution must be patched(skip the two first points
        // and the last one).
        if (patch && ix == 0 && (jx < 2 || jx == shape->nVertices - 1)) {
          x++;
          y++;
          continue;
        }
        boost::geometry::append(polygon, Point(*x++, *y++));
      }

      // Level 5 at full resolution must be patched (close the polygon).
      if (patch && ix == 0) {
        boost::geometry::append(polygon, Point(180, -90));
        boost::geometry::append(polygon, Point(0, -90));
      }

      // Envelope of the current polygon
      auto envelope = Box();

      // Is it necessary to make a geographical selection?
      if (bbox_.has_value()) {
        auto intersection = std::deque<Polygon>();
        boost::geometry::intersection(polygon, bbox_.value(), intersection);
        // If the read polygon is located in the geographical selection
        if (!intersection.empty()) {
          for (auto&& item : intersection) {
            transform_polygon_points(item, points);
            boost::geometry::envelope(item, envelope);
            polygons_.emplace_back(
                PolygonIndex{std::move(item), std::move(envelope), level});
          }
        }
      } else {
        // We store the current polygon and its points
        transform_polygon_points(polygon, points);
        boost::geometry::envelope(polygon, envelope);
        polygons_.emplace_back(
            PolygonIndex{std::move(polygon), std::move(envelope), level});
      }
    }
    SHPDestroyObject(shape);
  }
  SHPClose(handle);
}

void GSHHG::to_svg(const std::string& filename, const int width,
                   const int height) const {
  std::ofstream svg;
  svg.exceptions(std::ios_base::failbit | std::ios_base::badbit);
  svg.open(filename.c_str());

  boost::geometry::svg_mapper<Point> mapper(svg, width, height);
  mapper.add(bbox_.value_or(Box({-180, -90}, {180, 90})));

  unsigned int index = 0;

  for (const auto& item : boost::adaptors::reverse(polygons_)) {
    auto rgb = (++index) % 0x1000000U;
    auto code = std::to_string((rgb >> 16U) & 0xFFU) + "," +
                std::to_string((rgb >> 8U) & 0xFFU) + "," +
                std::to_string(rgb & 0xFFU);
    mapper.add(item.polygon);
    mapper.map(item.polygon, "fill-opacity:0.5;fill:rgb(" + code +
                                 ");stroke:rgb(" + code + ");" +
                                 "stroke-width:0.2");
  }
}

}  // namespace gshhg


/*
 qe2alm.cpp

 Copyright (c) 2015 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/version.hpp>
#include <boost/lexical_cast.hpp>
#include "memory.h"
#include "constants.h"
#include "mathfunctions.h"
#include "qe2alm.h"

using namespace std;

int main()
{
    cout << " QE2ALM -- a converter of harmonic FCs" << endl;
    cout << " Input file (QE fc file generated by q2r.x) : ";
    cin >> file_fc2qe;
    cout << " Output file (xml file for ALAMODE) : ";
    cin >> file_xmlout;
    cout << " Impose acoustic sum rule ? [y/n] : ";
    cin >> flag_asr;

    ifs_fc2qe.open(file_fc2qe.c_str(), std::ios::in);
    if (!ifs_fc2qe) {
        cout << " ERROR: Cannot open file " << file_fc2qe << endl;
        exit(EXIT_FAILURE);
    }

    transform(flag_asr.begin(), flag_asr.end(), flag_asr.begin(), ::tolower);
    if (flag_asr[0] == 'y') {
        consider_asr = true;
    } else if (flag_asr[0] == 'n') {
        consider_asr = false;
    } else {
        cout << " ERROR: Please specify 'y' or 'n' for the acoustic sum rule." << endl;
        exit(EXIT_FAILURE);
    }

    // Read structural parameters

    ifs_fc2qe >> nkd >> natmin >> ibrav
              >> celldm[0] >> celldm[1] >> celldm[2]
              >> celldm[3] >> celldm[4] >> celldm[5];

    if (ibrav == 0) {
        for (i = 0; i < 3; ++i) {
            ifs_fc2qe >> lavec[0][i] >> lavec[1][i] >> lavec[2][i];
        }

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                lavec[i][j] *= celldm[0];
            }
        }
    } else {
        calc_lattice_vector(ibrav, celldm, lavec);
    }

    recips(lavec, rlavec);

    int dummy;
    string str_dummy;
    double tmp;

    allocate(kd_symbol, nkd);

    for (i = 0; i < nkd; ++i) {
        ifs_fc2qe >> dummy >> kd_symbol[i] >> str_dummy >> tmp;
        kd_symbol[i] = kd_symbol[i].substr(1);
    }

    allocate(kd, natmin);
    allocate(xcrd, natmin, 3);

    for (i = 0; i < natmin; ++i) {
        ifs_fc2qe >> dummy >> kd[i] >> xcrd[i][0] >> xcrd[i][1] >> xcrd[i][2];
    }

    std::string str_na;
    ifs_fc2qe >> str_na;

    if (str_na[0] == 'T') {
        include_na = true;
    } else if (str_na[0] == 'F') {
        include_na = false;
    } else {
        cout << " ERROR: This cannot happen " << endl;
        exit(EXIT_FAILURE);
    }

    if (include_na) {
        cout << " WARNING: The given force constant file contains information of" << endl;
        cout << "          dielectric constants and Born effective charges." << endl;
        cout << "          The program skips these parts and converts the" << endl;
        cout << "          short-range terms only." << endl;
    }

    if (include_na) {
        allocate(born, natmin, 3, 3);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                ifs_fc2qe >> dielec[i][j];
            }
        }
        for (i = 0; i < natmin; ++i) {
            ifs_fc2qe >> dummy;
            for (j = 0; j < 3; ++j) {
                for (k = 0; k < 3; ++k) {
                    ifs_fc2qe >> born[i][j][k];
                }
            }
        }
    }

    // Read force constant info

    ifs_fc2qe >> nq[0] >> nq[1] >> nq[2];
    nsize = nq[0] * nq[1] * nq[2];

    allocate(fc2, 3 * natmin, 3 * natmin, nsize);

    int icell = 0;
    int m1, m2, m3;
    int icrd, jcrd;
    int iat, jat;

    for (icrd = 0; icrd < 3; ++icrd) {
        for (jcrd = 0; jcrd < 3; ++jcrd) {
            for (iat = 0; iat < natmin; ++iat) {
                for (jat = 0; jat < natmin; ++jat) {
                    ifs_fc2qe >> dummy >> dummy >> dummy >> dummy;

                    icell = 0;
                    for (m3 = 0; m3 < nq[2]; ++m3) {
                        for (m2 = 0; m2 < nq[1]; ++m2) {
                            for (m1 = 0; m1 < nq[0]; ++m1) {
                                ifs_fc2qe >> dummy >> dummy >> dummy;

                                // Force constant matrix should be transposed here.
                                ifs_fc2qe >> fc2[3 * jat + jcrd][3 * iat + icrd][icell];

                                ++icell;
                            }
                        }
                    }
                }
            }
        }
    }

    if (consider_asr) {

        double sum_fc;

        for (icrd = 0; icrd < 3; ++icrd) {
            for (jcrd = 0; jcrd < 3; ++jcrd) {
                for (iat = 0; iat < natmin; ++iat) {
                    sum_fc = 0.0;
                    for (jat = 0; jat < natmin; ++jat) {
                        for (icell = 0; icell < nsize; ++icell) {
                            sum_fc += fc2[3 * iat + icrd][3 * jat + jcrd][icell];
                        }
                    }
                    fc2[3 * iat + icrd][3 * iat + jcrd][0] -= sum_fc;
                }
            }
        }
    }

    // Convert positions from alat to crystal basis
    for (i = 0; i < natmin; ++i) {
        rotvec(xcrd[i], xcrd[i], rlavec);
        for (j = 0; j < 3; ++j) {
            xcrd[i][j] *= celldm[0] / (2.0 * pi);
        }
    }

    // Extend the system to the supercell

    nat = natmin * nsize;

    allocate(xcrd_super, nat, 3);
    allocate(kd_super, nsize, natmin);
    allocate(map_p2s, nsize, natmin);

    icell = 0;
    int icount = 0;

    for (m3 = 0; m3 < nq[2]; ++m3) {
        for (m2 = 0; m2 < nq[1]; ++m2) {
            for (m1 = 0; m1 < nq[0]; ++m1) {
                for (i = 0; i < natmin; ++i) {
                    xcrd_super[icount][0] = (xcrd[i][0] + static_cast<double>(m1)) / static_cast<double>(nq[0]);
                    xcrd_super[icount][1] = (xcrd[i][1] + static_cast<double>(m2)) / static_cast<double>(nq[1]);
                    xcrd_super[icount][2] = (xcrd[i][2] + static_cast<double>(m3)) / static_cast<double>(nq[2]);
                    kd_super[icell][i] = kd[i];

                    map_p2s[icell][i] = icount;
                    ++icount;
                }
                ++icell;
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            lavec[i][j] *= static_cast<double>(nq[j]);
        }
    }

    allocate(mindist, natmin, nat);

    get_pairs_of_minimum_distance(natmin, nat, map_p2s, xcrd_super, mindist);

    // Write to XML file

    using boost::property_tree::ptree;

    ptree pt;
    string str_pos[3], str_tmp;

    str_tmp.clear();

    //    for (i = 0; i < 3; ++i) {
    //        str_tmp += " " + std::to_string(nq[i]);
    //    }

    pt.put("Data.FilenameOfQE", file_fc2qe);
    //    pt.put("Data.Qmesh", str_tmp);

    pt.put("Data.Structure.NumberOfAtoms", nat);
    pt.put("Data.Structure.NumberOfElements", nkd);

    for (i = 0; i < nkd; ++i) {
        ptree &child = pt.add("Data.Structure.AtomicElements.element", kd_symbol[i]);
        child.put("<xmlattr>.number", i + 1);
    }

    for (i = 0; i < 3; ++i) {
        str_pos[i].clear();
        for (j = 0; j < 3; ++j) {
            str_pos[i] += " " + double2string(lavec[j][i]);
        }
    }
    pt.put("Data.Structure.LatticeVector", "");
    pt.put("Data.Structure.LatticeVector.a1", str_pos[0]);
    pt.put("Data.Structure.LatticeVector.a2", str_pos[1]);
    pt.put("Data.Structure.LatticeVector.a3", str_pos[2]);

    pt.put("Data.Structure.Position", "");

    icount = 0;

    for (i = 0; i < nsize; ++i) {

        for (j = 0; j < natmin; ++j) {
            str_tmp.clear();
            for (k = 0; k < 3; ++k) str_tmp += " " + double2string(xcrd_super[map_p2s[i][j]][k]);
            ptree &child = pt.add("Data.Structure.Position.pos", str_tmp);
            child.put("<xmlattr>.index", icount + 1);
            child.put("<xmlattr>.element", kd_symbol[kd_super[i][j] - 1]);

            ++icount;
        }
    }

    pt.put("Data.Symmetry.NumberOfTranslations", nsize);
    for (i = 0; i < nsize; ++i) {
        for (j = 0; j < natmin; ++j) {
            ptree &child = pt.add("Data.Symmetry.Translations.map", map_p2s[i][j] + 1);
            child.put("<xmlattr>.tran", i + 1);
            child.put("<xmlattr>.atom", j + 1);
        }
    }

    pt.put("Data.ForceConstants", "");
    str_tmp.clear();

    int nmulti, kat;

    for (iat = 0; iat < natmin; ++iat) {
        for (icrd = 0; icrd < 3; ++icrd) {
            for (icell = 0; icell < nsize; ++icell) {
                for (jat = 0; jat < natmin; ++jat) {
                    kat = map_p2s[icell][jat];
                    nmulti = mindist[iat][kat].size();
                    for (jcrd = 0; jcrd < 3; ++jcrd) {
                        if (std::abs(fc2[3 * iat + icrd][3 * jat + jcrd][icell]) < eps15) continue;
                        for (i = 0; i < nmulti; ++i) {
                            ptree &child = pt.add("Data.ForceConstants.HARMONIC.FC2",
                                                  double2string(fc2[3 * iat + icrd][3 * jat + jcrd][icell] /
                                                                static_cast<double>(nmulti)));

                            child.put("<xmlattr>.pair1", boost::lexical_cast<std::string>(iat + 1)
                                                         + " " + boost::lexical_cast<std::string>(icrd + 1));
                            child.put("<xmlattr>.pair2", boost::lexical_cast<std::string>(kat + 1)
                                                         + " " + boost::lexical_cast<std::string>(jcrd + 1)
                                                         + " " + boost::lexical_cast<std::string>(
                                    mindist[iat][kat][i].cell + 1));
                        }
                    }
                }
            }
        }
    }

    using namespace boost::property_tree::xml_parser;
    const int indent = 2;

#if BOOST_VERSION >= 105600
    write_xml(file_xmlout, pt, std::locale(),
              xml_writer_make_settings<ptree::key_type>(' ', indent, widen<std::string>("utf-8")));
#else
    write_xml(file_xmlout, pt, std::locale(),
              xml_writer_make_settings(' ', indent, widen<char>("utf-8")));
#endif

    deallocate(kd);
    deallocate(kd_symbol);
    deallocate(xcrd);
    deallocate(fc2);
    deallocate(xcrd_super);
    deallocate(kd_super);
    deallocate(map_p2s);

    if (include_na) {
        deallocate(born);
    }
}

void calc_lattice_vector(const int ibrav, double celldm[6], double aa[3][3])
{
    double a, b, c;
    double alpha, beta, gamma;
    double tx, ty, tz;
    double a2, b2, c2;
    double cosalpha, a_prime, u, v;

    switch (ibrav) {
        case 1:
            a = celldm[0];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = a;

            break;

        case 2:
            a = celldm[0] / 2.0;
            aa[0][0] = -a;
            aa[0][1] = 0.0;
            aa[0][2] = a;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = a;
            aa[2][0] = -a;
            aa[2][1] = a;
            aa[2][2] = 0.0;

            break;

        case 3:
            a = celldm[0] / 2.0;
            aa[0][0] = a;
            aa[0][1] = a;
            aa[0][2] = a;
            aa[1][0] = -a;
            aa[1][1] = a;
            aa[1][2] = a;
            aa[2][0] = -a;
            aa[2][1] = -a;
            aa[2][2] = a;

            break;

        case 4:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = -0.5 * a;
            aa[1][1] = sqrt(3.0) / 2.0 * a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 5:
        case -5:
            a = celldm[0];
            cosalpha = celldm[3];
            tx = a * sqrt((1.0 - cosalpha) / 2.0);
            ty = a * sqrt((1.0 - cosalpha) / 6.0);
            tz = a * sqrt((1.0 + 2.0 * cosalpha) / 3.0);

            if (ibrav == 5) {
                aa[0][0] = tx;
                aa[0][1] = -ty;
                aa[0][2] = tz;
                aa[1][0] = 0.0;
                aa[1][1] = 2.0 * ty;
                aa[1][2] = tz;
                aa[2][0] = -tx;
                aa[2][1] = -ty;
                aa[2][2] = tz;
            } else {
                a_prime = a / sqrt(3.0);
                u = tz - 2.0 * sqrt(2.0) * ty;
                v = tz + sqrt(2.0) * ty;
                aa[0][0] = u;
                aa[0][1] = v;
                aa[0][2] = v;
                aa[1][0] = v;
                aa[1][1] = u;
                aa[1][2] = v;
                aa[2][0] = v;
                aa[2][1] = v;
                aa[2][2] = u;
            }

            break;

        case 6:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = a;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 7:
            a = celldm[0];
            c = celldm[0] * celldm[2];
            a2 = a * 0.5;
            c2 = c * 0.5;
            aa[0][0] = a2;
            aa[0][1] = -a2;
            aa[0][2] = c2;
            aa[1][0] = a2;
            aa[1][1] = a2;
            aa[1][2] = c2;
            aa[2][0] = -a2;
            aa[2][1] = -a2;
            aa[2][2] = c2;

            break;

        case 8:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = b;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case 9:
        case -9:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            a2 = a * 0.5;
            b2 = b * 0.5;

            if (ibrav == 9) {
                aa[0][0] = a2;
                aa[0][1] = b2;
                aa[0][2] = 0.0;
                aa[1][0] = -a2;
                aa[1][1] = b2;
                aa[1][2] = 0.0;
                aa[2][0] = 0.0;
                aa[2][1] = 0.0;
                aa[2][2] = c;
            } else {
                aa[0][0] = a2;
                aa[0][1] = -b2;
                aa[0][2] = 0.0;
                aa[1][0] = a2;
                aa[1][1] = b2;
                aa[1][2] = 0.0;
                aa[2][0] = 0.0;
                aa[2][1] = 0.0;
                aa[2][2] = c;
            }

            break;

        case 10:
            a2 = celldm[0] * 0.5;
            b2 = celldm[0] * celldm[1] * 0.5;
            c2 = celldm[0] * celldm[2] * 0.5;
            aa[0][0] = a2;
            aa[0][1] = 0.0;
            aa[0][2] = c2;
            aa[1][0] = a2;
            aa[1][1] = b2;
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = b2;
            aa[2][2] = c2;

            break;

        case 11:
            a2 = celldm[0] * 0.5;
            b2 = celldm[0] * celldm[1] * 0.5;
            c2 = celldm[0] * celldm[2] * 0.5;
            aa[0][0] = a2;
            aa[0][1] = b2;
            aa[0][2] = c2;
            aa[1][0] = -a2;
            aa[1][1] = b2;
            aa[1][2] = c2;
            aa[2][0] = -a2;
            aa[2][1] = -b2;
            aa[2][2] = c2;

            break;

        case 12:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            gamma = acos(celldm[3]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = 0.0;
            aa[2][1] = 0.0;
            aa[2][2] = c;

            break;

        case -12:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            beta = acos(celldm[4]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = 0.0;
            aa[1][1] = b;
            aa[1][2] = 0.0;
            aa[2][0] = c * cos(beta);
            aa[2][1] = 0.0;
            aa[2][2] = c * sin(beta);

            break;

        case 13:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            gamma = acos(celldm[3]);
            aa[0][0] = 0.5 * a;
            aa[0][1] = 0.0;
            aa[0][2] = -0.5 * c;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = 0.5 * a;
            aa[2][1] = 0.0;
            aa[2][2] = 0.5 * c;

            break;

        case 14:
            a = celldm[0];
            b = celldm[0] * celldm[1];
            c = celldm[0] * celldm[2];
            alpha = acos(celldm[3]);
            beta = acos(celldm[4]);
            gamma = acos(celldm[5]);
            aa[0][0] = a;
            aa[0][1] = 0.0;
            aa[0][2] = 0.0;
            aa[1][0] = b * cos(gamma);
            aa[1][1] = b * sin(gamma);
            aa[1][2] = 0.0;
            aa[2][0] = c * cos(beta);
            aa[2][1] = c * (cos(alpha) - cos(beta) * cos(gamma)) / sin(gamma);
            aa[2][2] = c * sqrt(1.0 + 2.0 * cos(alpha) * cos(beta) * cos(gamma)
                                - cos(alpha) * cos(alpha) - cos(beta) * cos(beta) - cos(gamma) * cos(gamma)) /
                       sin(gamma);

            break;

        default:

            cout << "ERROR: Invalid ibrav." << endl;
            exit(EXIT_FAILURE);
    }

    // Transpose lavec for later use

    double tmp[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            tmp[i][j] = aa[i][j];
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            aa[i][j] = tmp[j][i];
        }
    }
}

void recips(double aa[3][3], double bb[3][3])
{
    /*
    Calculate Reciprocal Lattice Vectors

    Here, BB is just the inverse matrix of AA (multiplied by factor 2 Pi)

    BB = 2 Pi AA^{-1},
    = t(b1, b2, b3)

    = (b11 b12 b13)
      (b21 b22 b23)
      (b31 b32 b33),

    b1 = t(b11, b12, b13) etc.
    */

    double det;
    det = aa[0][0] * aa[1][1] * aa[2][2]
          + aa[1][0] * aa[2][1] * aa[0][2]
          + aa[2][0] * aa[0][1] * aa[1][2]
          - aa[0][0] * aa[2][1] * aa[1][2]
          - aa[2][0] * aa[1][1] * aa[0][2]
          - aa[1][0] * aa[0][1] * aa[2][2];

    if (std::abs(det) < eps12) {
        cout << " ERROR: Lattice vector is singular" << endl;
        exit(EXIT_FAILURE);
    }

    double factor = 2.0 * pi / det;

    bb[0][0] = (aa[1][1] * aa[2][2] - aa[1][2] * aa[2][1]) * factor;
    bb[0][1] = (aa[0][2] * aa[2][1] - aa[0][1] * aa[2][2]) * factor;
    bb[0][2] = (aa[0][1] * aa[1][2] - aa[0][2] * aa[1][1]) * factor;

    bb[1][0] = (aa[1][2] * aa[2][0] - aa[1][0] * aa[2][2]) * factor;
    bb[1][1] = (aa[0][0] * aa[2][2] - aa[0][2] * aa[2][0]) * factor;
    bb[1][2] = (aa[0][2] * aa[1][0] - aa[0][0] * aa[1][2]) * factor;

    bb[2][0] = (aa[1][0] * aa[2][1] - aa[1][1] * aa[2][0]) * factor;
    bb[2][1] = (aa[0][1] * aa[2][0] - aa[0][0] * aa[2][1]) * factor;
    bb[2][2] = (aa[0][0] * aa[1][1] - aa[0][1] * aa[1][0]) * factor;
}

string double2string(const double d)
{
    std::string rt;
    std::stringstream ss;

    ss << std::scientific << std::setprecision(15) << d;
    ss >> rt;
    return rt;
}

void get_pairs_of_minimum_distance(const int natmin, const int nat, int **map_p2s,
                                   double **xf, std::vector<DistInfo> **mindist_pairs)
{
    int icell = 0;
    int i, j, k;
    int isize, jsize, ksize;
    int iat;
    double dist_tmp;
    double vec[3];
    std::vector<DistInfo> **distall;
    int nneib = 27;

    double ***xcrd;

    allocate(distall, natmin, nat);
    allocate(xcrd, nneib, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            xcrd[0][i][j] = xf[i][j];
        }
    }

    for (isize = -1; isize <= 1; ++isize) {
        for (jsize = -1; jsize <= 1; ++jsize) {
            for (ksize = -1; ksize <= 1; ++ksize) {

                if (isize == 0 && jsize == 0 && ksize == 0) continue;

                ++icell;
                for (i = 0; i < nat; ++i) {
                    xcrd[icell][i][0] = xf[i][0] + static_cast<double>(isize);
                    xcrd[icell][i][1] = xf[i][1] + static_cast<double>(jsize);
                    xcrd[icell][i][2] = xf[i][2] + static_cast<double>(ksize);
                }
            }
        }
    }

    for (icell = 0; icell < nneib; ++icell) {
        for (i = 0; i < nat; ++i) {
            rotvec(xcrd[icell][i], xcrd[icell][i], lavec);
        }
    }

    for (i = 0; i < natmin; ++i) {
        iat = map_p2s[0][i];
        for (j = 0; j < nat; ++j) {
            for (icell = 0; icell < nneib; ++icell) {

                dist_tmp = distance(xcrd[0][iat], xcrd[icell][j]);

                for (k = 0; k < 3; ++k) vec[k] = xcrd[icell][j][k] - xcrd[0][iat][k];

                distall[i][j].push_back(DistInfo(icell, dist_tmp, vec));
            }
            std::sort(distall[i][j].begin(), distall[i][j].end());
        }
        /*
                for (j = 0; j < nat; ++j) {
                    for (k = 0; k < distall[i][j].size(); ++k) {
                        std::cout << std::setw(5) << i + 1;
                        std::cout << std::setw(5) << j + 1;
                        std::cout << std::setw(5) << k + 1;
                        std::cout << std::setw(15) << distall[i][j][k].dist << endl;
                    }
                }
        */
    }

    // Construct pairs of minimum distance.

    double dist_min;
    for (i = 0; i < natmin; ++i) {
        for (j = 0; j < nat; ++j) {
            mindist_pairs[i][j].clear();

            dist_min = distall[i][j][0].dist;
            for (std::vector<DistInfo>::const_iterator it = distall[i][j].begin(); it != distall[i][j].end(); ++it) {
                if (std::abs((*it).dist - dist_min) < 1.e-3) {
                    mindist_pairs[i][j].push_back(DistInfo(*it));
                }
            }
        }
    }

    deallocate(distall);
    deallocate(xcrd);
}

double distance(double *x1, double *x2)
{
    double dist;
    dist = std::pow(x1[0] - x2[0], 2) + std::pow(x1[1] - x2[1], 2) + std::pow(x1[2] - x2[2], 2);
    dist = std::sqrt(dist);

    return dist;
}


/****************************************************************************
# # minas_hardware_interface.cpp:  MINAS A5B EtherCAT Motor Controller      #
# Copyright (C) 2017, Tokyo Opensource Robotics Kyokai Association          #
#                                                                           #
# This program is free software; you can redistribute it and/or modify      #
# it under the terms of the GNU General Public License as published by      #
# the Free Software Foundation; either version 2 of the License, or         #
# (at your option) any later version.                                       #
#                                                                           #
# This program is distributed in the hope that it will be useful,           #
# but WITHOUT ANY WARRANTY; without even the implied warranty of            #
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             #
# GNU General Public License for more details.                              #
#                                                                           #
# You should have received a copy of the GNU General Public License         #
# along with this program; if not, write to the Free Software               #
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA #
#                                                                           #
****************************************************************************/

#include <minas_control/minas_hardware_interface.h>
#include <getopt.h>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

namespace minas_control
{

#define PULSE_PER_REVOLUTE ( (1048576 / (2 * M_PI) ) * 101 ) // 20 bit / 101 reduction
  //#define PULSE_PER_REVOLUTE ( ( 131072 / (2 * M_PI) ) * 101 )// 17 bit / 101 reduction

  EtherCATJointControlInterface::EtherCATJointControlInterface(ethercat::EtherCatManager* manager, int slave_no, hardware_interface::JointStateInterface& jnt_stat, hardware_interface::PositionJointInterface& jnt_cmd, int torque_for_emergency_stop, int over_load_level, int over_speed_level, double motor_working_range, int max_motor_speed, int max_torque, int home_encoder_offset) : JointControlInterface(slave_no, jnt_stat, jnt_cmd)
  {
    ROS_INFO("Initialize EtherCATJoint (%d)", slave_no);
    // EtherCAT
    int operation_mode = 0x08; // (csp) cyclic synchronous position mode

    client = new MinasClient(*manager, slave_no);

    ROS_INFO("Initialize EtherCATJoint (reset)");
    client->reset();

    // set paramete from PANATERM test program
    ROS_INFO("Initialize EtherCATJoint (TorqueForEmergencyStop %d)", torque_for_emergency_stop);
    client->setTrqueForEmergencyStop(torque_for_emergency_stop); // unit [%]
    ROS_INFO("Initialize EtherCATJoint (OverLoadLevel %d)", over_load_level);
    client->setOverLoadLevel(over_load_level);          // unit [%]
    ROS_INFO("Initialize EtherCATJoint (OverSpeedLevel %d)", over_speed_level);
    client->setOverSpeedLevel(over_speed_level);        // [r/min]
    ROS_INFO("Initialize EtherCATJoint (MotorWorkingRange %.1f)", motor_working_range);
    client->setMotorWorkingRange(motor_working_range);  // (unit 0.1, full range is 1)

    ROS_INFO("Initialize EtherCATJoint (InterpolationTimePeriod)");
    client->setInterpolationTimePeriod(4000);     // 4 msec

    // servo on
    ROS_INFO("Initialize EtherCATJoint (servoOn)");
    client->servoOn();

    // get current positoin
    ROS_INFO("Initialize EtherCATJoint (readInputs)");
    input = client->readInputs();
    int32 current_position = input.position_actual_value;

    ROS_INFO("Initialize EtherCATJoint (set target position)");
    // set target position
    memset(&output, 0x00, sizeof(minas_control::MinasOutput));
    if ( operation_mode == 0x01 )
      { // (pp) position profile mode
	output.target_position = (current_position > 0)?(current_position - 0x100000):(current_position + 0x100000);
      }
    else
      { // (csp) cyclic synchronous position mode
	//output.target_position = current_position;
	output.target_position = 0;
	output.position_offset = current_position;
      }
    output.max_motor_speed = max_motor_speed;  // rad/min
    output.target_torque = 0;    // 0% (unit 0.1%)
    output.max_torque    = max_torque;    // 50% (unit 0.1%)
    output.controlword   = 0x001f; // move to operation enabled + new-set-point (bit4) + change set immediately (bit5)

    // change to cyclic synchronous position mode
    output.operation_mode = operation_mode;
    //output.operation_mode = 0x08; // (csp) cyclic synchronous position mode
    //output.operation_mode = 0x01; // (pp) position profile mode

    // set profile velocity
    ROS_INFO("Initialize EtherCATJoint (setProfileVelocity)");
    client->setProfileVelocity(0x20000000);

    ROS_INFO("Initialize EtherCATJoint (pp control model setup)");
    // pp control model setup (see statusword(6041.h) 3) p.107)
    client->writeOutputs(output);
    while ( ! (input.statusword & 0x1000) ) {// bit12 (set-point-acknowledge)
      input = client->readInputs();
    }
    ROS_INFO("Initialize EtherCATJoint (clear new set point)");
    output.controlword   &= ~0x0010; // clear new-set-point (bit4)
    client->writeOutputs(output);

    // set home_encoder_offset
    // encoder resolution is 17Bit(131072 per round)
    if (abs(home_encoder_offset) > 100000)
    {
      ROS_WARN("Invalid large home_encoder_offset value: %d", joint.home_encoder_offset_);
      ROS_WARN("Please check your home_encoder_offset parameter is correct.");
    }
    else
    {
      joint.home_encoder_offset_ = home_encoder_offset;
    }
    ROS_INFO("home_encoder_offset = %d", joint.home_encoder_offset_);

    ROS_WARN("target position = %08x", output.target_position);
    ROS_WARN("position offset = %08x", output.position_offset);
    joint.cmd_ = joint.pos_ = (current_position - joint.home_encoder_offset_) / PULSE_PER_REVOLUTE;
    joint.vel_ = joint.eff_ = 0;
    ROS_INFO("Initialize EtherCATJoint .. done");
  }

  EtherCATJointControlInterface::~EtherCATJointControlInterface()
  {
    ROS_INFO_STREAM_NAMED("minas", "~EtherCATJointControlInterface()");
    shutdown();
    delete(client);
  }

  void EtherCATJointControlInterface::shutdown()
  {
    ROS_INFO_STREAM_NAMED("minas", joint.name_ + " shutdown()");
    client->printPDSStatus(input);
    client->printPDSOperation(input);
    client->reset();
    client->servoOff();
  }

  void EtherCATJointControlInterface::read()
  {
    input = client->readInputs();
    output = client->readOutputs();
    joint.pos_ = int32_t(input.position_actual_value - joint.home_encoder_offset_) / PULSE_PER_REVOLUTE;
    joint.vel_ = int32_t(input.velocity_actual_value) / PULSE_PER_REVOLUTE;
    joint.eff_ = int32_t(input.torque_actual_value) / PULSE_PER_REVOLUTE;
  }

  void EtherCATJointControlInterface::write()
  {
    output.position_offset = uint32_t(joint.cmd_ * PULSE_PER_REVOLUTE + joint.home_encoder_offset_);
    client->writeOutputs(output);
  }

  //
  DummyJointControlInterface::DummyJointControlInterface(int slave_no, hardware_interface::JointStateInterface& jnt_stat, hardware_interface::PositionJointInterface& jnt_cmd) : JointControlInterface(slave_no, jnt_stat, jnt_cmd) {
    joint.cmd_ = joint.pos_ = joint.vel_ = joint.eff_ = joint.home_encoder_offset_ = 0;
  }

  void DummyJointControlInterface::read()
  {
    joint.pos_ = joint.cmd_;
    joint.vel_ = 0;
    joint.eff_ = 0;
  }

  void DummyJointControlInterface::write()
  {
  }

  //
  void MinasHardwareInterface::getParamFromROS(int joint_no, int &torque_for_emergency_stop, int &over_load_level, int &over_speed_level, double &motor_working_range, int &max_motor_speed, int &max_torque, int &home_encoder_offset)
  {
    std::string joint_name("~joint" + boost::lexical_cast<std::string>(joint_no));
    ros::param::param<int>(joint_name + "/torque_for_emergency_stop", torque_for_emergency_stop, 100); // 100%
    ros::param::param<int>(joint_name + "/over_load_level", over_load_level, 50); // 50%
    ros::param::param<int>(joint_name + "/over_speed_level", over_speed_level, 120); // r/min
    ros::param::param<double>(joint_name + "/motor_working_range", motor_working_range, 0.1); // 0.1
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/torque_for_emergency_stop : " << torque_for_emergency_stop);
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/over_load_level           : " << over_load_level);
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/over_speed_level          : " << over_speed_level);
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/motor_working_range       : " << motor_working_range);
    ros::param::param<int>(joint_name + "/max_motor_speed", max_motor_speed, 120); // rad/min
    ros::param::param<int>(joint_name + "/max_torque", max_torque, 500); // 50% (unit 0.1%)
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/max_motor_speed           : " << max_motor_speed);
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/max_torque                : " << max_torque);
    // Encoder offset at angle = 0
    ros::param::param<int>(joint_name + "/home_encoder_offset", home_encoder_offset, 0);
    ROS_INFO_STREAM_NAMED("minas", joint_name + "/home_encoder_offset       : " << home_encoder_offset);
  }

  MinasHardwareInterface::MinasHardwareInterface(std::string ifname, bool in_simulation)
    : manager(NULL)
  {
    /* start MinasClient */
    if (in_simulation) {
      ROS_INFO_STREAM_NAMED("minas","Minas Hardware Interface in simulation mode");
      for (int i = 1; i <= 6; i++ ) {
        int torque_for_emergency_stop, over_load_level, over_speed_level;
        double motor_working_range;
        int max_motor_speed, max_torque, home_encoder_offset;
        getParamFromROS(i, torque_for_emergency_stop, over_load_level, over_speed_level, motor_working_range, max_motor_speed, max_torque, home_encoder_offset);
	registerControl(new DummyJointControlInterface(i,
						       joint_state_interface,
						       joint_position_interface
						       ));
      }
    } else {

      manager = new ethercat::EtherCatManager(ifname);

      n_dof_ = manager->getNumClients();
      if ( n_dof_ != 6 ) {
	ROS_ERROR_STREAM_NAMED("minas", "Minas Hardware Interface expecting 6 clients");
      }
      int i;
      for (i = 1; i <= n_dof_; i++ )
	{
          int torque_for_emergency_stop, over_load_level, over_speed_level;
          double motor_working_range;
          int max_motor_speed, max_torque;
          int home_encoder_offset;
          getParamFromROS(i, torque_for_emergency_stop, over_load_level, over_speed_level, motor_working_range, max_motor_speed, max_torque, home_encoder_offset);
	  registerControl(new EtherCATJointControlInterface(manager, i,
							    joint_state_interface,
							    joint_position_interface,
                                                            torque_for_emergency_stop, over_load_level, over_speed_level, motor_working_range,
                                                            max_motor_speed, max_torque, home_encoder_offset
                                                            ));
	}
      for (; i <= 6; i++ )
	{
	  registerControl(new DummyJointControlInterface(i,
							 joint_state_interface,
							 joint_position_interface
							 ));
	  ROS_ERROR_STREAM_NAMED("minas", "Could not find EtherCAT client");
	  ROS_ERROR_STREAM_NAMED("minas", "Minas Hardware Interface uses Dummy joint " << i);
	}
    }

    registerInterface(&joint_state_interface);
    registerInterface(&joint_position_interface);
  }

  MinasHardwareInterface::~MinasHardwareInterface()
  {
    shutdown();
  }

  void MinasHardwareInterface::shutdown()
  {
    BOOST_FOREACH (JointControlInterface* control, controls) {
      control->shutdown();
    }
    controls.clear();
    if ( manager != NULL ) {
      ROS_INFO_STREAM_NAMED("minas", "Delete manager");
      delete(manager);
    }
    manager = NULL;
  }

  void MinasHardwareInterface::registerControl(JointControlInterface* control)
  {
    controls.push_back(control);
  }

  bool MinasHardwareInterface::read(const ros::Time time, const ros::Duration period)
  {
    BOOST_FOREACH (JointControlInterface* control, controls) {
      control->read();
    }
  }

  void MinasHardwareInterface::write(const ros::Time time, const ros::Duration period)
  {
    BOOST_FOREACH (JointControlInterface* control, controls) {
      control->write();
    }
  }

  inline ros::Time MinasHardwareInterface::getTime()
  {
    return ros::Time::now();
  }

  inline ros::Duration MinasHardwareInterface::getPeriod()
  {
    return ros::Duration(0.001);
  }

} // namespace


/*
 * osinterface_TimeUtils.cpp
 */

#include <time.h>

#include "osinterface_TimeUtils.h"

#include "osinterface_OsTypes.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/time_duration.hpp>

// Defines
//
#define NS_IN_SECS 1000000000

namespace mutgos
{
namespace osinterface
{
    boost::posix_time::ptime TimeUtils::epoch(
        boost::gregorian::date(1970, 1, 1));
    std::string TimeUtils::timezone;

    // -----------------------------------------------------------------------
    void TimeUtils::init(const std::string &tz)
    {
        timezone = tz;
    }

    // -----------------------------------------------------------------------
    OsTypes::TimeEpochType TimeUtils::get_ptime_as_epoch(
        const boost::posix_time::ptime ptime_source)
    {
        const boost::posix_time::time_duration diff(ptime_source - epoch);

        return (diff.ticks() / diff.ticks_per_second());
    }

    // -----------------------------------------------------------------------
    OsTypes::TimeEpochType TimeUtils::get_current_time_utc(void)
    {
        const boost::posix_time::time_duration time_t_diff =
            boost::posix_time::second_clock::universal_time() - epoch;

        return time_t_diff.total_seconds();
    }

    // -----------------------------------------------------------------------
    // Taken from and modified:
    //   https://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
    bool TimeUtils::timespec_substract(
        const timespec &lhs,
        const timespec &rhs,
        timespec &result)
    {
        timespec x = lhs;
        timespec y = rhs;

        // Perform the carry for the later subtraction by updating y.
        if (x.tv_nsec < y.tv_nsec)
        {
            const long nsec = (y.tv_nsec - x.tv_nsec) / NS_IN_SECS + 1;
            y.tv_nsec -= NS_IN_SECS * nsec;
            y.tv_sec += nsec;
        }

        if (x.tv_nsec - y.tv_nsec > NS_IN_SECS)
        {
            const long nsec = (x.tv_nsec - y.tv_nsec) / NS_IN_SECS;
            y.tv_nsec += NS_IN_SECS * nsec;
            y.tv_sec -= nsec;
        }

        // Compute the time remaining. tv_nsec is certainly positive.
        result.tv_sec = x.tv_sec - y.tv_sec;
        result.tv_nsec = x.tv_nsec - y.tv_nsec;

        // Return true if result is negative.
        return x.tv_sec < y.tv_sec;
    }

} /* namespace osinterface */
} /* namespace mutgos */


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.

#include "poplar/Graph.hpp"
#include "poplibs_support/Algorithm.hpp"
#include "poplibs_support/TestDevice.hpp"
#include "popsparse/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include "poputil/exceptions.hpp"
#include <poplibs_test/Util.hpp>

#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <cassert>
#include <numeric>

using namespace poplibs_support;
using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace poputil;

static constexpr int maxRepresentableIntegerInHalf = 1024;

boost::multi_array<double, 2> createInput(unsigned numXOrY, unsigned numZ,
                                          int startNum) {
  boost::multi_array<double, 2> in(boost::extents[numZ][numXOrY]);
  for (unsigned z = 0; z != numZ; ++z) {
    for (unsigned xy = 0; xy != numXOrY; ++xy, ++startNum) {
      in[z][xy] = startNum;
    }
  }
  return in;
}

static unsigned zBlockSize(const Type &type) { return type == FLOAT ? 8 : 16; }

// Return input value for a given output position
static double getOutputVal(const boost::multi_array_ref<double, 2> in,
                           unsigned blockSizeXY, const Type &type, unsigned xy,
                           unsigned z) {
  const auto numZ = in.shape()[0];
  const unsigned blockSizeZ = zBlockSize(type);
  const auto outFlattenedIndex = xy * numZ + z;
  const auto blockXY = outFlattenedIndex / (numZ * blockSizeXY);
  const auto xyBlockIndex = outFlattenedIndex % (numZ * blockSizeXY);
  const auto blockZ = xyBlockIndex / (blockSizeZ * blockSizeXY);
  const auto zBlockIndex = xyBlockIndex % (blockSizeZ * blockSizeXY);
  const auto zWithinZBlock = zBlockIndex % blockSizeZ;
  const auto xyWithinZBlock = zBlockIndex / blockSizeZ;
  const auto xyInIndex = blockXY * blockSizeXY + xyWithinZBlock;
  const auto zInIndex = blockZ * blockSizeZ + zWithinZBlock;
  return in[zInIndex][xyInIndex];
}

static bool supportedBlockSize(unsigned blockSize) {
  return blockSize == 4 || blockSize == 8 || blockSize == 16;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType = DeviceType::IpuModel2;
  Type dataType = FLOAT;
  unsigned numXOrY, numZ, blockSize;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("device-type",
     po::value<DeviceType>(&deviceType)->default_value(deviceType),
     "Device type")
    ("data-type",
     po::value<Type>(&dataType)->default_value(dataType),
     "Data type")
    ("x-y-size",
     po::value<unsigned>(&numXOrY)->required(),
     "X/Y size such that product of X/Y size and Z size is <= 2048")
    ("z-size",
     po::value<unsigned>(&numZ)->required(),
     "Z size such that product of X/Y size and Z size is <= 2048")
    ("block-size",
     po::value<unsigned>(&blockSize)->required(),
     "Block size (only 4, 8 and 16 are supported)")
  ;
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (dataType != HALF && dataType != FLOAT) {
    std::cerr << "Only half and float data types supported"
              << "\n";
    return 1;
  }

  if (!supportedBlockSize(blockSize)) {
    std::cerr << "Block size not supported"
              << "\n";
    return 1;
  }

  if (numXOrY % blockSize) {
    std::cerr << "X/Y size must be a multiple of block size"
              << "\n";
    return 1;
  }

  if (numZ % zBlockSize(dataType)) {
    std::cerr << "Z size must be a multiple of block size"
              << "\n";
    return 1;
  }

  if (numZ * numXOrY > 2 * maxRepresentableIntegerInHalf) {
    std::cerr << "Product of X/Y and Z must be less than 2048";
    return 1;
  }

  auto device = createTestDevice(deviceType, 1, 1);
  const auto &target = device.getTarget();
  Graph graph(target);
  popsparse::addCodelets(graph);

  // Allocate operands
  const auto xyIn = graph.addVariable(dataType, {numZ, numXOrY}, "xyIn");
  const auto xyOut = graph.addVariable(dataType, {numXOrY, numZ}, "xyOut");
  graph.setTileMapping(xyIn, 0);
  graph.setTileMapping(xyOut, 0);

  const auto numXOrYBlocks = numXOrY / blockSize;
  const auto cs = graph.addComputeSet("cs0");
  const auto vertexClass =
      templateVertex("popsparse::BlockTransposeGradW", dataType);
  const auto v = graph.addVertex(
      cs, vertexClass, {{"in", xyIn.flatten()}, {"out", xyOut.flatten()}});
  graph.setInitialValue(v["blockSizeXOrY"], blockSize);
  graph.setInitialValue(v["numXOrYBlocks"], numXOrYBlocks);
  graph.setInitialValue(v["numZ"], numZ);
  graph.setInitialValue(v["maxXOrYBlocksPerWorker"],
                        ceildiv(numXOrYBlocks, target.getNumWorkerContexts()));
  graph.setTileMapping(v, 0);

  Sequence prog;
  prog.add(Execute(cs));

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  std::unique_ptr<char[]> rawHostXYIn, rawHostXYOut;
  rawHostXYIn = allocateHostMemoryForTensor(xyIn, "xyIn", graph, uploadProg,
                                            downloadProg, tmap);
  rawHostXYOut = allocateHostMemoryForTensor(xyOut, "xyOut", graph, uploadProg,
                                             downloadProg, tmap);

  std::cout << "Building engine...\n";
  Engine engine(graph, Sequence(uploadProg, prog, downloadProg));
  std::cout << "Built\n";
  attachStreams(engine, tmap);

  auto hostXYIn = createInput(numXOrY, numZ, 0);
  boost::multi_array<double, 2> hostXYOut(boost::extents[numXOrY][numZ]);

  copy(target, hostXYIn, dataType, rawHostXYIn.get());

  std::cout << "Attaching to device and running...\n";
  device.bind([&](const Device &d) {
    engine.load(d);
    engine.setPrintStream(std::cerr);
    engine.run();
  });

  copy(target, dataType, rawHostXYOut.get(), hostXYOut);
  std::cout << "Finished running\n";

  bool matchesModel = true;
  for (unsigned xy = 0; xy != numXOrY; ++xy) {
    for (unsigned z = 0; z != numZ; ++z) {
      const auto modelOut = getOutputVal(hostXYIn, blockSize, dataType, xy, z);
      if (modelOut != hostXYOut[xy][z]) {
        std::cerr << "Mismatch at [" << xy << "," << z << "]";
        std::cerr << "expected " << modelOut << " actual " << hostXYOut[xy][z];
        std::cerr << "\n";
        matchesModel = false;
      }
    }
  }
  return !matchesModel;
}


#ifndef DUBINS_TRAFFIC_PROBLEM_H
#define DUBINS_TRAFFIC_PROBLEM_H

#include <iostream>
#include <vector>
#include <string>

#include <Eigen/Dense>

#include "roadnet.hpp"

namespace dubins_traffic {

/** Representation of problem instances.

    \ingroup dubins_traffic */
class Problem {
public:
    double intersection_radius;
    std::vector<size_t> goals;
    RoadNetwork rnd;

    Problem( const RoadNetwork &rnd_ );
    Problem( const Problem &to_be_copied );

    static Problem random( const RoadNetwork &rnd_,
                           const Eigen::Vector2i &number_goals_bounds );

    /** Create formula using SPIN LTL syntax <http://spinroot.com/spin/Man/ltl.html>.
       Support for other syntax is coming soon. */
    void to_formula( std::ostream &out ) const;

    /** Output description of problem in JSON to a stream */
    friend std::ostream & operator<<( std::ostream &out, const Problem &prob );

    /** Get description of problem in JSON. */
    std::string dumpJSON() const;
};

Problem::Problem( const RoadNetwork &rnd_ )
    : rnd(rnd_)
{
    intersection_radius = 0.0;
}

Problem::Problem( const Problem &to_be_copied )
    : intersection_radius( to_be_copied.intersection_radius ),
      goals( to_be_copied.goals ),
      rnd( to_be_copied.rnd )
{
    assert( intersection_radius > 0 );
}

Problem Problem::random( const RoadNetwork &rnd_,
                         const Eigen::Vector2i &number_goals_bounds )
{
    assert( number_goals_bounds(0) >= 0
            && number_goals_bounds(0) <= number_goals_bounds(1) );

    Problem pinstance( rnd_ );
    pinstance.intersection_radius = 1.0;

    int number_goals = number_goals_bounds(0);
    if (number_goals_bounds(1) != number_goals_bounds(0))
        number_goals += (std::rand()
                         % (1+number_goals_bounds(1)
                            - number_goals_bounds(0)));

    for (size_t goal_idx = 0; goal_idx < number_goals; goal_idx++)
        pinstance.goals.push_back( std::rand() % rnd_.number_of_intersections() );

    return pinstance;
}

void Problem::to_formula( std::ostream &out ) const
{
    if (goals.size() > 0) {
        for (size_t i = 0; i < goals.size(); i++) {
            if (i > 0)
                out << " && ";
            out << "([]<> ego_" << rnd.get_intersection_str( goals[i] ) << ")";
        }
    }
}

std::ostream & operator<<( std::ostream &out, const Problem &prob )
{
    out << "{" << std::endl << "\"version\": " << 0 << "," << std::endl;
    out << "\"goals\": [" << std::endl;
    for (int i = 0; i < prob.goals.size(); i++) {
        if (i > 0)
            out << ", ";
        Eigen::Vector2i intersection = prob.rnd.get_intersection( prob.goals[i] );
        out << "[" << intersection(0) << ", " << intersection(1) << "]";
        out << std::endl;
    }
    out << "]," << std::endl;
    out << "\"rnd\": " << prob.rnd << std::endl;
    out << ", \"intersection_radius\": " << prob.intersection_radius;
    out << std::endl << "}";
    return out;
}

std::string Problem::dumpJSON() const
{
    std::ostringstream out;
    out << *this;
    return out.str();
}

} // namespace dubins_traffic

#endif


#include "ros/ros.h"
#include "uwb_driver/UwbRange.h"
#include "geometry_msgs/PoseStamped.h"
#include <Eigen/Dense>
#include "eigen_conversions/eigen_msg.h"
#include <sstream>
#include <string.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <random>

using namespace Eigen;
// using namespace std;
using namespace tf;
ros::Publisher uwb_pub;
std::vector<int> nodesId;
std::vector<double> nodesPos;
std::vector<double> antennaOffset;
int node_num;
int antenna_num;
int count = 0;
int count_antenna = 0;
std::random_device rd;
std::mt19937 gen(rd());
std::normal_distribution<> d(0,0.02);

void gazeboCallback(const geometry_msgs::PoseStamped pose)
{
    // frame_id : world
    uwb_driver::UwbRange msg;
    msg.header = std_msgs::Header();
    msg.header.frame_id = "uwb";
    msg.header.stamp = pose.header.stamp;
    msg.requester_id  = nodesId.back();
    msg.requester_idx = nodesId.back()%100;
    msg.responder_id  = nodesId[count];
    msg.responder_idx = nodesId[count]%100;

    Eigen::Affine3d pose_eigen;
    poseMsgToEigen(pose.pose, pose_eigen);
    static tf::TransformBroadcaster br;
    tf::Transform transform;

    if(antenna_num == 0)
    {
        msg.antenna = 0;
        tf::transformEigenToTF (pose_eigen, transform);
        br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, "antenna"));
    }
    else
    {
        int antenna_id = count_antenna%antenna_num;
        msg.antenna = antenna_id + 1;
        double x = antennaOffset[antenna_id*3];
        double y = antennaOffset[antenna_id*3+1];
        double z = antennaOffset[antenna_id*3+2];
        Eigen::Affine3d trans = Eigen::Affine3d::Identity();
        trans.translate(Vector3d(x,y,z));
        pose_eigen = pose_eigen * trans;
        tf::transformEigenToTF(pose_eigen, transform);
        br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, "antenna"+std::to_string(msg.antenna)));
        // std::cout << " " <<"antenna"+std::to_string(msg.antenna)<< " " << std::endl;
    }

    count_antenna = count_antenna+1;

    double distance = sqrt(
        pow(pose_eigen(0,3) - nodesPos[count*3  ], 2)+
        pow(pose_eigen(1,3) - nodesPos[count*3+1], 2)+
        pow(pose_eigen(2,3) - nodesPos[count*3+2], 2));

    transform.setOrigin( tf::Vector3(nodesPos[count*3  ],nodesPos[count*3+1], nodesPos[count*3+2]));
    br.sendTransform(tf::StampedTransform(transform, pose.header.stamp, pose.header.frame_id, std::to_string(count)+"anchor"));

    msg.distance = distance + d(gen);
    msg.distance_err = 0.02;
    msg.responder_location.x = nodesPos[count*3];
    msg.responder_location.y = nodesPos[count*3+1];
    msg.responder_location.z = nodesPos[count*3+2];
    uwb_pub.publish(msg);

    if (antenna_num==0 || count_antenna%antenna_num == 0)
        count = (count+1)%node_num;

    ros::Duration(0.025).sleep();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uwb_driver_gazebo");

    ros::NodeHandle n;

    if(n.getParam("/uwb/nodesId", nodesId))
        for (auto it:nodesId)
            ROS_WARN("Get node ID: %d", it);

    if(n.getParam("/uwb/nodesPos", nodesPos))
        for(auto it:nodesPos)
            ROS_WARN("Get node position: %4.2f", it);

    if(n.getParam("/uwb/antennaOffset", antennaOffset))
        for(auto it:antennaOffset)
            ROS_WARN("Get antenna Offset: %4.2f", it);

    antenna_num = antennaOffset.size()/3;
    ROS_WARN("Using %d antennas", antenna_num);

    ros::Subscriber sub = n.subscribe("/ground_truth_to_tf/pose", 1, gazeboCallback);

    uwb_pub = n.advertise<uwb_driver::UwbRange>("/uwb_endorange_info", 0);

    node_num = nodesId.size() - 1;

    ROS_WARN("Publishing..");

    ros::spin();

    return 0;
}


#include <chrono>
#include <cmath>
#include <exception>
#include <thread>

#include <boost/log/trivial.hpp>
#include <casadi/casadi.hpp>

#include "mimir/algorithm/TestAlgorithm.hpp"
#include "mimir/StateMachineFwd.hpp"
#include "mimir/FkinDds.hpp"

// ReaderLifespan
#include "dds/core/detail/ProprietaryApi.hpp"

namespace mimir
{
  namespace algorithm
  {
    class TestAlgorithm::Impl
    {
    public:
      Impl(
          const YAML::Node& config,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber) :
        inputReader(dds::core::null),
        outputWriter(dds::core::null)
      {

        // Setup input reader
        const auto test_config = config["inputs"]["signal"];
        auto test_age = test_config["max_age_ms"].as<std::int64_t>();

        auto readerQos = subscriber.default_datareader_qos();
        //readerQos << dds::core::policy::Durability::TransientLocal()
        //          << dds::core::policy::Reliability::Reliable()
        readerQos << org::opensplice::core::policy::ReaderLifespan(
                      true,
                      dds::core::Duration::from_millisecs(test_age));

        const auto topicName = test_config["topic"].as<std::string>();
        const auto id = test_config["id"].as<std::string>(); // For filter topic
        auto topic = dds::topic::Topic<fkin::IdVec1d>(
            subscriber.participant(),
            topicName);
        auto filter = dds::topic::Filter("id = %0", {id});

        inputReader = dds::sub::DataReader<fkin::IdVec1d>(
            subscriber,
            dds::topic::ContentFilteredTopic<fkin::IdVec1d>(
                topic,
                topicName + id,
                filter),
            readerQos);

        // Setup output writer
        auto writerQos = publisher.default_datawriter_qos();
        //writerQos << dds::core::policy::Durability::TransientLocal();

        outputWriter = dds::pub::DataWriter<fkin::IdVec1d>(
            publisher,
            dds::topic::Topic<fkin::IdVec1d>(
                publisher.participant(),
                config["outputs"]["response"]["topic"].as<std::string>()),
            writerQos);

        outputId = config["outputs"]["response"]["id"].as<std::string>();

        // Setup model description
        casadi::SX x = casadi::SX::sym("x", 1);     // state
        casadi::SX x_d = casadi::SX::sym("x_d", 1); // set-point
        casadi::SX T = casadi::SX::sym("T", 1);     // time constant
        casadi::SX rhs = -(x-x_d)/T;                // prop-ctrl with time constant T
        casadi::SXDict ode = {{"x", x}, {"p", vertcat(x_d,T)}, {"ode", rhs}};

        integrator = casadi::integrator(
            "F", "cvodes", ode,
            {{"tf", config["time_step_ms"].as<double>()/1000.}});

        p_vec = casadi::DM(std::vector<double>{5.,5.});

      }
      ~Impl() {}
      dds::sub::DataReader<fkin::IdVec1d> inputReader;
      dds::pub::DataWriter<fkin::IdVec1d> outputWriter;
      std::string outputId;
      casadi::Function integrator;
      casadi::DM p_vec;
      casadi::DM x_k;
    };

    TestAlgorithm::TestAlgorithm(
        const YAML::Node& config,
        boost::statechart::fifo_scheduler<> & scheduler,
        boost::statechart::fifo_scheduler<>::processor_handle machine,
        dds::pub::Publisher publisher,
        dds::sub::Subscriber subscriber) :
      m_impl( new TestAlgorithm::Impl(config, publisher, subscriber)),
      m_scheduler(scheduler),
      m_stateMachine(machine),
      m_time_step(std::chrono::milliseconds(config["time_step_ms"].as<std::int32_t>())),
      m_next_step(std::chrono::steady_clock::now()),
      m_config(config)
    {}
    void TestAlgorithm::solve(const std::atomic<bool>& cancel_token)
    {
      try
      {
        BOOST_LOG_TRIVIAL(trace) << "Solving " << name();

        // Publish x_k(t_now) (we do not publish future samples)
        m_impl->outputWriter << fkin::IdVec1d(
            m_impl->outputId,
            fkin::Vector1d(m_impl->x_k.get_nonzeros()[0]));

        // Fetch input, sample and hold
        auto samples = m_impl->inputReader.read();
        // waitset with short timeout, allowing run in "open loop"

        // Set new desired value
        if(samples.length() > 0)
        {
          auto sample = (--samples.end());
          if(sample->info().valid())
          {
            m_impl->p_vec(0,0) = sample->data().vec().x();
            BOOST_LOG_TRIVIAL(trace) << "Got a sample: " << sample->data().vec().x();
          }
        }
        else
        {
          // TODO: In case of measurements: Disable feedback gain
          BOOST_LOG_TRIVIAL(trace) << "No valid sample for " << name() << ", using old";
        }

        if(cancel_token)
        {
          event(new mimir::EvInterrupt());
          return;
        }

        // Integrate
        auto result = m_impl->integrator(
            casadi::DMDict( {{"x0",m_impl->x_k}, {"p", m_impl->p_vec}} ));

        if(cancel_token)
        {
          event(new mimir::EvInterrupt());
          return;
        }

        // Update output (not published yet)
        m_impl->x_k = result["xf"];
        step_time();
        event(new mimir::EvReady());

      }
      catch (...)
      {
        // Need to post event in case of exception so that the state machine
        // knows that the job has finished.
        BOOST_LOG_TRIVIAL(fatal) << name() << " exception thrown";
        event(new mimir::EvError());
        throw;
      }
    }

    void TestAlgorithm::initialize(const std::atomic<bool>&)
    {
      try
      {
        BOOST_LOG_TRIVIAL(debug) << name() << " is initializing";

        // waitset to get initial measurements that meet requirements.
        // set initial state vector..
        m_impl->x_k = casadi::DM(std::vector<double>{0.});
        // set parameters as well?
        m_next_step = std::chrono::steady_clock::now();

        event(new mimir::EvReady());
      }
      catch (...)
      {
        event(new mimir::EvError());
        throw;
      }
    }

    void TestAlgorithm::timer(const std::atomic<bool>& cancel_token)
    {

      // There is no sanity checks in terms of real-timeliness of execution.
      // probably want external time point to wait until.
      typedef std::chrono::milliseconds scm;
      auto now = std::chrono::steady_clock::now();
      auto time_p = m_next_step;
      auto timeleft = std::chrono::duration_cast<scm>(time_p - now).count();
      BOOST_LOG_TRIVIAL(trace) << "Time left: " << timeleft;

      auto fraction = (m_time_step/100 > scm(0) ? m_time_step/100 :
       (m_time_step/50 > scm(0) ? m_time_step/50 :
        (m_time_step/25 > scm(0) ? m_time_step/25 :
         (m_time_step/10 > scm(0) ? m_time_step/10 :
          (m_time_step/2 > scm(0) ?  m_time_step/2  : scm(1))))));

      while(now < time_p)
      {
        auto diff = time_p - now;
        std::this_thread::sleep_for(diff < fraction ? diff : fraction);
        if(cancel_token)
          break;
        now = std::chrono::steady_clock::now();
      }

      if(cancel_token)
      {
        BOOST_LOG_TRIVIAL(trace) << "Timer Canceled";
        event(new mimir::EvInterrupt());
      }
      else
      {
        BOOST_LOG_TRIVIAL(trace) << "Timeout";
        event(new mimir::EvTimeout());
      }
    }

    void TestAlgorithm::event(boost::statechart::event_base * const event)
    {
      // who is responsible for the pointer?
      m_scheduler.queue_event(
          m_stateMachine,
          make_intrusive(event));
    }

    TestAlgorithm::~TestAlgorithm() = default;
  }
}


#include <boost/numeric/interval/arith3.hpp>


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <boost/bloom_filter/basic_bloom_filter.hpp>
#include <boost/bloom_filter/dynamic_bloom_filter.hpp>
#include <boost/bloom_filter/counting_bloom_filter.hpp>
#include <boost/unordered_set.hpp>
#include <set>
using boost::bloom_filters::basic_bloom_filter;
using boost::bloom_filters::dynamic_bloom_filter;
using boost::bloom_filters::counting_bloom_filter;
using boost::unordered_set;

typedef basic_bloom_filter<int, 1000> basic_bloom;
typedef dynamic_bloom_filter<int> basic_dynamic_bloom;
typedef counting_bloom_filter<int, 1000> counting_bloom;

int main()
{
  basic_bloom basic;
  basic_dynamic_bloom dynamic(1000);
  counting_bloom counting;
  boost::unordered_set<int> hash_set;
  std::set<int> set;

  std::cout << "\n============== Static size comparison ==============\n";

  std::cout << "Basic Bloom filter static size: " << sizeof(basic_bloom) << "\n"
            << "Basic dynamic Bloom filter static size: " << sizeof(basic_dynamic_bloom) << "\n"
            << "Counting Bloom filter static size: " << sizeof(counting_bloom) << "\n"
            << "Unordered set static size: " << sizeof(boost::unordered_set<int>) << "\n"
            << "Set static size: " << sizeof(std::set<int>) << std::endl;

  for (size_t i = 0; i < 1000; ++i) {
    basic.insert(i);
    dynamic.insert(i);
    counting.insert(i);
    hash_set.insert(i);
    set.insert(i);
  }

  std::cout << "\n============== Run-time size comparison ==============\n";

  std::cout << "Basic Bloom filter run-time size: " << 0 << "\n"
            << "Basic dynamic Bloom filter run-time size: " << dynamic.bit_capacity() << "\n"
            << "Counting Bloom filter run-time size: " << 0 << "\n"
            << "Unordered set run-time size: " << sizeof(int) * hash_set.size() << " + X\n"
            << "Set run-time size: " << sizeof(int) * set.size() << " + X"  << std::endl;

  std::cout << "\n============== Total size comparison ==============\n";

  std::cout << "Basic Bloom filter total size: " << sizeof(basic_bloom) + 0 << "\n"
            << "Basic dynamic Bloom filter total size: " << sizeof(basic_dynamic_bloom) + dynamic.bit_capacity() << "\n"
            << "Counting Bloom filter total size: " << sizeof(counting_bloom) + 0 << "\n"
            << "Unordered set total size: " << sizeof(boost::unordered_set<int>) + sizeof(int) * hash_set.size() << " + X\n"
            << "Set total size: " << sizeof(std::set<int>) + sizeof(int) * set.size() << " + X"  << std::endl;

  return 0;
}


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

#ifndef __AHL_ROBOT_SAMPLES_PR2_PARAM_HPP
#define __AHL_ROBOT_SAMPLES_PR2_PARAM_HPP

#include <memory>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include "ahl_robot_samples/Pr2ParamConfig.h"

namespace ahl_sample
{

  class Pr2Param
  {
  public:
    explicit Pr2Param()
    {
      const uint32_t dof_l = 11;
      const uint32_t dof_r = 11;

      show_target = true;

      sin_x_l = false;
      sin_y_l = false;
      sin_z_l = false;
      sin_x_r = false;
      sin_y_r = false;
      sin_z_r = false;

      xl = Eigen::Vector3d::Zero();
      rl = Eigen::Vector3d::Zero();
      Rl = Eigen::Matrix3d::Identity();
      Rf_l = Eigen::Matrix3d::Identity();
      Rm_l = Eigen::Matrix3d::Identity();
      fl = Eigen::Vector3d::Zero();
      ml = Eigen::Vector3d::Zero();

      xr = Eigen::Vector3d::Zero();
      rr = Eigen::Vector3d::Zero();
      Rr = Eigen::Matrix3d::Identity();
      Rf_r = Eigen::Matrix3d::Identity();
      Rm_r = Eigen::Matrix3d::Identity();
      fr = Eigen::Vector3d::Zero();
      mr = Eigen::Vector3d::Zero();

      q_l = Eigen::VectorXd::Zero(dof_l);
      q_r = Eigen::VectorXd::Zero(dof_r);

      ros::NodeHandle local_nh("~/pr2/");
      f_ = boost::bind(&Pr2Param::update, this, _1, _2);
      server_ = std::make_shared<Pr2ParamConfigServer>(local_nh);
      server_->setCallback(f_);
    }

    bool show_target;

    bool sin_x_l;
    bool sin_y_l;
    bool sin_z_l;
    bool sin_x_r;
    bool sin_y_r;
    bool sin_z_r;

    Eigen::Vector3d xl;
    Eigen::Vector3d rl;
    Eigen::Matrix3d Rl;
    Eigen::Matrix3d Rf_l;
    Eigen::Matrix3d Rm_l;
    Eigen::Vector3d fl; // force
    Eigen::Vector3d ml; // moment

    Eigen::Vector3d xr;
    Eigen::Vector3d rr;
    Eigen::Matrix3d Rr;
    Eigen::Matrix3d Rf_r;
    Eigen::Matrix3d Rm_r;
    Eigen::Vector3d fr; // force
    Eigen::Vector3d mr; // moment

    Eigen::VectorXd q_l;
    Eigen::VectorXd q_r;

  private:
    void update(ahl_robot_samples::Pr2ParamConfig& config, uint32_t level)
    {
      show_target = config.show_target;

      sin_x_l = config.sin_x_l;
      sin_y_l = config.sin_y_l;
      sin_z_l = config.sin_z_l;

      sin_x_r = config.sin_x_r;
      sin_y_r = config.sin_y_r;
      sin_z_r = config.sin_z_r;

      xl[0] = config.x_l;
      xl[1] = config.y_l;
      xl[2] = config.z_l;

      rl[0] = config.roll_l;
      rl[1] = config.pitch_l;
      rl[2] = config.yaw_l;

      Rl = Eigen::AngleAxisd(config.roll_l,  Eigen::Vector3d::UnitX())
         * Eigen::AngleAxisd(config.pitch_l, Eigen::Vector3d::UnitY())
         * Eigen::AngleAxisd(config.yaw_l,   Eigen::Vector3d::UnitZ());

      Rf_l = Eigen::AngleAxisd(config.Rf_roll_l,  Eigen::Vector3d::UnitX())
           * Eigen::AngleAxisd(config.Rf_pitch_l, Eigen::Vector3d::UnitY())
           * Eigen::AngleAxisd(config.Rf_yaw_l,   Eigen::Vector3d::UnitZ());

      Rm_l = Eigen::AngleAxisd(config.Rm_roll_l,  Eigen::Vector3d::UnitX())
           * Eigen::AngleAxisd(config.Rm_pitch_l, Eigen::Vector3d::UnitY())
           * Eigen::AngleAxisd(config.Rm_yaw_l,   Eigen::Vector3d::UnitZ());

      fl[0] = config.fx_l;
      fl[1] = config.fy_l;
      fl[2] = config.fz_l;

      ml[0] = config.mx_l;
      ml[1] = config.my_l;
      ml[2] = config.mz_l;

      xr[0] = config.x_r;
      xr[1] = config.y_r;
      xr[2] = config.z_r;

      rr[0] = config.roll_r;
      rr[1] = config.pitch_r;
      rr[2] = config.yaw_r;

      Rr = Eigen::AngleAxisd(config.roll_r,  Eigen::Vector3d::UnitX())
         * Eigen::AngleAxisd(config.pitch_r, Eigen::Vector3d::UnitY())
         * Eigen::AngleAxisd(config.yaw_r,   Eigen::Vector3d::UnitZ());

      Rf_r = Eigen::AngleAxisd(config.Rf_roll_r,  Eigen::Vector3d::UnitX())
           * Eigen::AngleAxisd(config.Rf_pitch_r, Eigen::Vector3d::UnitY())
           * Eigen::AngleAxisd(config.Rf_yaw_r,   Eigen::Vector3d::UnitZ());

      Rm_r = Eigen::AngleAxisd(config.Rm_roll_r,  Eigen::Vector3d::UnitX())
           * Eigen::AngleAxisd(config.Rm_pitch_r, Eigen::Vector3d::UnitY())
           * Eigen::AngleAxisd(config.Rm_yaw_r,   Eigen::Vector3d::UnitZ());

      fr[0] = config.fx_r;
      fr[1] = config.fy_r;
      fr[2] = config.fz_r;

      mr[0] = config.mx_r;
      mr[1] = config.my_r;
      mr[2] = config.mz_r;

      q_l[0]  = config.q_base1;
      q_l[1]  = config.q_base2;
      q_l[2]  = config.q_base3;
      q_l[3]  = config.q_base4;
      q_l[4]  = config.q_l1;
      q_l[5]  = config.q_l2;
      q_l[6]  = config.q_l3;
      q_l[7]  = config.q_l4;
      q_l[8]  = config.q_l5;
      q_l[9]  = config.q_l6;
      q_l[10] = config.q_l7;

      q_r[0]  = config.q_base1;
      q_r[1]  = config.q_base2;
      q_r[2]  = config.q_base3;
      q_r[3]  = config.q_base4;
      q_r[4]  = config.q_r1;
      q_r[5]  = config.q_r2;
      q_r[6]  = config.q_r3;
      q_r[7]  = config.q_r4;
      q_r[8]  = config.q_r5;
      q_r[9]  = config.q_r6;
      q_r[10] = config.q_r7;
    }

    using Pr2ParamConfigServer = dynamic_reconfigure::Server<ahl_robot_samples::Pr2ParamConfig>;
    using Pr2ParamConfigServerPtr = std::shared_ptr<Pr2ParamConfigServer>;

    Pr2ParamConfigServerPtr server_;
    dynamic_reconfigure::Server<ahl_robot_samples::Pr2ParamConfig>::CallbackType f_;
  };

  using Pr2ParamPtr = std::shared_ptr<Pr2Param>;

} // namespace ahl_sample

#endif // __AHL_ROBOT_SAMPLES_PR2_PARAM_HPP