#include <Eigen/Eigen>

#include <Libs/Optimize/Optimize.h>

Eigen::MatrixXd optimize_get_particle_system(shapeworks::Optimize *opt)
{
  shapeworks::MatrixContainer container = opt->GetParticleSystem();
  return container.matrix_;
}

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/operators.h>
#include <pybind11/eigen.h>
#include <pybind11/functional.h>

namespace py = pybind11;
using namespace pybind11::literals;

#include <bitset>
#include <sstream>

#include <itkImportImageFilter.h>
#include <vtkDoubleArray.h>
#include <vtkFloatArray.h>

#include "Shapeworks.h"
#include "ShapeworksUtils.h"
#include "Image.h"
#include "VectorImage.h"
#include "ImageUtils.h"
#include "Mesh.h"
#include "MeshUtils.h"
#include "Optimize.h"
#include "ParticleSystem.h"
#include "ShapeEvaluation.h"
#include "ParticleShapeStatistics.h"
#include "EigenUtils.h"
#include "pybind_utils.h"

using namespace shapeworks;

PYBIND11_MODULE(shapeworks_py, m)
{
  m.doc() = "ShapeWorks Python API";

  m.attr("Pi") = std::atan(1.0) * 4.0;

  // Transform
  py::class_<itk::SmartPointer<itk::Transform<double, 3u, 3u> >>(m, "Transform")
  .def("__repr__",
       [](const TransformPtr &transform) {
         std::stringstream stream;
         itk::Transform<double, 3, 3>::ParametersType p = transform->GetParameters();
         int r = 0;
         for (int i=0; i<4; i++)
         {
           for (int j=0; j<3; j++)
           {
             stream << p[r] << " ";
             r++;
           }
         }
         return stream.str();
       });

  // Constructs an itk::AffineTransform from the 3x3 scale-rotate-warp and 3x1 translation.
  m.def("createTransform",
        [](Eigen::Matrix<double, 3, 3> &mat, std::vector<double> v) -> decltype(auto) {
          Matrix33 mat33 = eigenToItk<double, 3, 3>(mat);
          return createTransform(mat33, makeVector({v[0], v[1], v[2]}));
        },
        "creates transform from 3x3 matrix and translation vector",
        "mat"_a, "translate"_a=std::vector<double>({0,0,0}));

  m.def("seed",
        &ShapeworksUtils::setRngSeed,
        "sets the seed for random number generation (internal use)",
        "seed"_a=std::chrono::system_clock::now().time_since_epoch().count());

  // Axis
  py::enum_<Axis>(m, "Axis")
  .value("invalid", Axis::invalid)
  .value("X", Axis::X)
  .value("Y", Axis::Y)
  .value("Z", Axis::Z)
  .export_values();

  m.def("axis_is_valid",
        [](const std::vector<double> &axis) -> decltype(auto) {
          return axis_is_valid(makeVector({axis[0], axis[1], axis[2]}));
        },
        "ensure an axis is valid",
        "axis"_a);

  m.def("degToRad",
        degToRad,
        "convert degrees to radians",
        "deg"_a);

  m.def("toAxis",
        toAxis,
        "convert to axis",
        "str"_a);

  // TransformType
  py::enum_<XFormType>(m, "TransformType")
  .value("CenterOfMass", XFormType::CenterOfMass)
  .value("IterativeClosestPoint", XFormType::IterativeClosestPoint)
  .export_values();

  // Image::InterpolationType (even though this is part of Image, it feels cleaner to keep it global in the module)
  py::enum_<Image::InterpolationType>(m, "InterpolationType")
  .value("Linear", Image::InterpolationType::Linear)
  .value("NearestNeighbor", Image::InterpolationType::NearestNeighbor)
  .export_values();

  // Image
  py::class_<Image>(m, "Image")
    .def(py::init<const std::string &>())
    .def(py::init<const Image&>())

    // constructor that wraps numpy float32 array
    .def(py::init
         // The reasons we don't simply specify py::array_t<float>
         // as a parameter are:
         // - to take ownership of the array
         // - to ensure dtype is same as Image::PixelType, and
         // - to ensure the array isn't silently cast by copying (the default pybind11 behavior)
         ([](py::array& np_array) {
           return Image(wrapNumpyArr(np_array));
         }),
         "Initialize an image from a numpy array (must be dtype float32).\nTransfers ownership of the array without copying.\nIf a copy is desired, construct using Image(np.array(arr)).")

    .def("assign",
         [](Image& image, py::array& np_array) -> decltype(auto) {

           // verify dims are the same as existing image; warn if not
           auto curr_dims = image.dims();
           Dims new_dims =
             {{ static_cast<Dims::SizeValueType>(np_array.shape()[2]),
                static_cast<Dims::SizeValueType>(np_array.shape()[1]),
                static_cast<Dims::SizeValueType>(np_array.shape()[0]) }};
           if (curr_dims[0] != new_dims[0] ||
               curr_dims[1] != new_dims[1] ||
               curr_dims[2] != new_dims[2]) {
             std::cerr << "curr_dims: " << curr_dims[0] << " " << curr_dims[1] << " " << curr_dims[2] << std::endl;
             std::cerr << "new_dims: " << new_dims[0] << " " << new_dims[1] << " " << new_dims[2] << std::endl;
             throw std::invalid_argument("dims of new image are not the same as old image.");
           }

           // get existing origin, shape, and coordsys
           auto origin = image.origin();
           auto spacing = image.spacing();
           auto coordsys = image.coordsys();

           // wrap the numpy array
           image = Image(wrapNumpyArr(np_array));

           // set new image to have the same origin, shape, and coordsys
           image.setOrigin(origin);
           image.setSpacing(spacing);
           image.setCoordsys(coordsys);

           return image;
         },
    "Initialize an image from a numpy array (must be dtype float32).\nTransfers ownership of the array without copying.\nIf a copy is desired, construct using Image(np.array(arr)).")

  .def("__neg__", [](Image& img) -> decltype(auto) { return -img; })
  .def(py::self + py::self)
  .def(py::self += py::self)
  .def(py::self - py::self)
  .def(py::self -= py::self)
  .def(py::self * Image::PixelType())
  .def(py::self *= Image::PixelType())
  .def(py::self / Image::PixelType())
  .def(py::self /= Image::PixelType())
  .def(py::self + Image::PixelType())
  .def(py::self += Image::PixelType())
  .def(py::self - Image::PixelType())
  .def(py::self -= Image::PixelType())
  .def(py::self == py::self)

  .def("__repr__",
       [](const Image &img) -> decltype(auto) {
         std::stringstream stream;
         stream << img;
         return stream.str();
       })

  .def("copy",
       [](Image& image) -> decltype(auto) { return Image(image); })

  .def("write",
       &Image::write,
       "writes the current image (determines type by its extension)",
       "filename"_a, "compressed"_a=true)

  .def("antialias",
       &Image::antialias,
       "antialiases binary volumes (layers is set to 3 when not specified)",
       "iterations"_a=50, "maxRMSErr"_a=0.01f, "layers"_a=3)

  .def("resample",
       [](Image& image,
          const TransformPtr transform,
          const std::vector<double>& p,
          const std::vector<unsigned>& d,
          const std::vector<double>& v,
          const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> &direction,
          Image::InterpolationType interp) -> decltype(auto) {
         return image.resample(transform,
                               Point({p[0], p[1], p[2]}),
                               Dims({d[0], d[1], d[2]}),
                               makeVector({v[0], v[1], v[2]}),
                               eigenToItk(direction),
                               interp);
       },
       "resamples by applying transform then sampling from given origin along direction axes at spacing physical units per pixel for dims pixels using specified interpolator",
       "transform"_a, "origin"_a, "dims"_a, "spacing"_a, "direction"_a,
       "interp"_a = Image::InterpolationType::NearestNeighbor)

  .def("resample",
       [](Image& image,
          const std::vector<double>& v,
          Image::InterpolationType interp) -> decltype(auto) {
         return image.resample(makeVector({v[0], v[1], v[2]}), interp);
       },
       "resamples image using new physical spacing, updating logical dims to keep all image data for this spacing",
       "physicalSpacing"_a, "interp"_a=Image::InterpolationType::Linear)

  .def("resample",
       py::overload_cast<double, Image::InterpolationType>(&Image::resample),
       "isotropically resamples image using giving isospacing",
       "isoSpacing"_a=1.0, "interp"_a=Image::InterpolationType::Linear)

  .def("resize",
       [](Image& image, std::vector<unsigned>& d, Image::InterpolationType interp) -> decltype(auto) {
         return image.resize(Dims({d[0], d[1], d[2]}), interp);
       },
       "change logical dims (computes new physical spacing)",
       "logicalDims"_a, "interp"_a=Image::InterpolationType::Linear)

  .def("recenter",
       &Image::recenter,
       "recenters an image by changing its origin in the image header to the physical coordinates of the center of the image")

  .def("pad",
       py::overload_cast<int, Image::PixelType>(&Image::pad),
       "pads an image by same number of pixels in all directions with constant value",
       "pad"_a, "value"_a=0.0)

  .def("pad",
       py::overload_cast<int, int, int, Image::PixelType>(&Image::pad),
       "pads an image by desired number of pixels in each direction with constant value",
       "padx"_a, "pady"_a, "padz"_a, "value"_a=0.0)

  .def("pad",
       py::overload_cast<IndexRegion&, Image::PixelType>(&Image::pad),
       "pads an image to include the given region with constant value",
       "region"_a, "value"_a=0.0)

  .def("translate",
       [](Image& image, const std::vector<double>& v) -> decltype(auto) {
         return image.translate(makeVector({v[0], v[1], v[2]}));
       },
       "translates image", "v"_a)

  .def("scale",
       [](Image& image, const std::vector<double>& scale_vec) -> decltype(auto) {
         return image.scale(makeVector({scale_vec[0], scale_vec[1], scale_vec[2]}));
       },
       "scale image by scale_vec around center (not origin)",
       "scale_vec"_a)

  .def("rotate",
       py::overload_cast<const double, const Vector3&>(&Image::rotate),
       "rotate around center (not origin) using axis (default z-axis) by angle (in radians)",
       "angle"_a, "axis"_a)

  .def("rotate",
       py::overload_cast<const double, Axis>(&Image::rotate),
       "rotate around center (not origin) using axis (default z-axis) by angle (in radians)",
       "angle"_a, "axis"_a)

  .def("rotate",
       [](Image& image, const double angle, const std::vector<double>& v) -> decltype(auto) {
         return image.rotate(angle, makeVector({v[0], v[1], v[2]}));
       },
       "rotate around center (not origin) using axis (default z-axis) by angle (in radians)",
       "angle"_a, "axis"_a)

  .def("applyTransform",
       py::overload_cast<TransformPtr, Image::InterpolationType>(&Image::applyTransform),
       "applies the given transformation to the image by using resampling filter",
       "transform"_a, "interp"_a=Image::InterpolationType::Linear)

  .def("applyTransform",
       [](Image& image, const TransformPtr transform,
          const std::vector<double>& p,
          const std::vector<unsigned>& d,
          const std::vector<double>& v,
          const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> &direction,
          Image::InterpolationType interp) {
         return image.applyTransform(transform,
                                     Point({p[0], p[1], p[2]}),
                                     Dims({d[0], d[1], d[2]}),
                                     makeVector({v[0], v[1], v[2]}),
                                     eigenToItk(direction),
                                     interp);
       },
       "applies the given transformation to the image by using resampling filter with new origin, dims, spacing and direction values",
       "transform"_a, "origin"_a, "dims"_a, "spacing"_a, "direction"_a,
       "interp"_a=Image::InterpolationType::NearestNeighbor)

  .def("extractLabel",
       &Image::extractLabel,
       "extracts/isolates a specific pixel label from a given multi-label volume and outputs the corresponding binary image",
       "label"_a=1.0)

  .def("closeHoles",
       &Image::closeHoles,
       "closes holes in a volume defined by values larger than specified value",
       "foreground"_a=0.0)

  .def("binarize",
       &Image::binarize,
       "sets portion of image greater than min and less than or equal to max to the specified value",
       "minVal"_a=0.0, "maxVal"_a=std::numeric_limits<Image::PixelType>::max(),
       "innerVal"_a=1.0, "outerVal"_a=0.0)

  .def("computeDT",
       &Image::computeDT,
       "computes signed distance transform volume from an image at the specified isovalue",
       "isovalue"_a=0.0)

  .def("applyCurvatureFilter",
       &Image::applyCurvatureFilter,
       "denoises an image using curvature driven flow using curvature flow image filter",
       "iterations"_a=10)

  .def("applyGradientFilter",
       &Image::applyGradientFilter,
       "computes gradient magnitude at each pixel using gradient magnitude filter")

  .def("applySigmoidFilter",
       &Image::applySigmoidFilter,
       "computes sigmoid function pixel-wise using sigmoid image filter",
       "alpha"_a=10.0, "beta"_a=10.0)

  .def("applyTPLevelSetFilter",
       &Image::applyTPLevelSetFilter,
       "segments structures in image using topology preserving geodesic active contour level set filter",
       "featureImage"_a, "scaling"_a=20.0)

  .def("applyIntensityFilter",
       &Image::applyIntensityFilter,
       "applies intensity windowing image filter",
       "min"_a=0.0, "max"_a=0.0)

  .def("gaussianBlur",
       &Image::gaussianBlur,
       "applies gaussian blur",
       "sigma"_a=0.0)

  .def("crop",
       &Image::crop,
       "crops the image down to the given (physica) region, with optional padding",
       "region"_a, "padding"_a=0)

  .def("clip",
       [](Image& image,
          const std::vector<double>& o,
          std::vector<double>& p1,
          std::vector<double>& p2,
          const Image::PixelType val) {
         return image.clip(makePlane(Point({o[0], o[1], o[2]}),
                                     Point({p1[0], p1[1], p1[2]}),
                                     Point({p2[0], p2[1], p2[2]})), val);
       },
       "sets values on the back side of cutting plane (containing three non-colinear points) to val (default 0.0)",
       "o"_a, "p1"_a, "p2"_a, "val"_a=0.0)

  .def("clip",
       [](Image& image,
          const std::vector<double>& n,
          std::vector<double>& q,
          const Image::PixelType val) {
         return image.clip(makePlane(Point({q[0], q[1], q[2]}), makeVector({n[0], n[1], n[2]})), val);
       },
       "sets values on the back side of cutting plane (normal n containing point p) to val (default 0.0)",
       "n"_a, "q"_a, "val"_a=0.0)

  .def("setSpacing",
       [](Image& self, std::vector<double>& v) -> decltype(auto) {
         return self.setSpacing(makeVector({v[0], v[1], v[2]}));
       },
       "set image spacing, the size of each pixel",
       "spacing"_a=std::vector<double>({1.0, 1.0, 1.0}))

  .def("setOrigin",
       [](Image& image, std::vector<double>& p) {
         return image.setOrigin(Point({p[0], p[1], p[2]}));
       },
       "sets the image origin in physical space to the given value",
       "origin"_a=std::vector<double>({0,0,0}))

    .def("setCoordsys",
         [](Image& image, const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> &coordsys) {
           if (coordsys.rows() != 3 || coordsys.cols() != 3) {
             throw std::invalid_argument("coordsys must be a 3x3 row-major numpy array");
           }
           return image.setCoordsys(eigenToItk(coordsys));
         },
         "sets the orientation of this image",
         "coordsys"_a=std::vector<double>({1,0,0, 0,1,0, 0,0,1}))

  .def("reflect",
       &Image::reflect,
       "reflect image with respect to logical image center and the specified axis",
       "axis"_a)

  .def("dims",
       [](const Image& self) -> decltype(auto) { return py::array(3, self.dims().data()); },
       "logical dimensions of the image")

  .def("size",
       [](const Image& self) -> decltype(auto) { return py::array(3, self.size().GetDataPointer()); },
       "physical dimensions of the image (dims * spacing)")

  .def("spacing",
       [](const Image& self) -> decltype(auto) { return py::array(3, self.spacing().GetDataPointer()); },
       "physical spacing of the image")

  .def("origin",
       [](const Image& self) -> decltype(auto) { return py::array(3, self.origin().GetDataPointer()); },
       "physical coordinates of image origin")

  .def("center",
       [](const Image& self) -> decltype(auto) { return py::array(3, self.center().GetDataPointer()); },
       "physical coordinates of center of this image")

  .def("coordsys",
       [](const Image &self) -> decltype(auto) {
         return itkToEigen(self.coordsys());
       },
       "return 3x3 coordinate system in which this image lives in physical space")

  .def("centerOfMass",
       [](const Image& self, double minVal, double maxVal) -> decltype(auto) {
         return py::array(3, self.centerOfMass().GetDataPointer());
       },
       "returns average physical coordinate of pixels in range (minval, maxval]",
       "minVal"_a=0.0, "maxVal"_a=1.0)

  .def("min",
       &Image::min,
       "minimum of image")

  .def("max",
       &Image::max,
       "maximum of image")

  .def("mean",
       &Image::mean,
       "mean of image")

  .def("std",
       &Image::std,
       "standard deviation of image")

  .def("logicalBoundingBox",
       &Image::logicalBoundingBox,
       "returns the index coordinates of this image's region")

  // py::overload_cast doesn't work with const functions, so we directly static_cast for these two functions
  .def("physicalboundingBox",
       static_cast<PhysicalRegion (Image::*)() const>(&Image::physicalBoundingBox),
       "returns region of physical space occupied by this image")

  .def("physicalBoundingBox",
       static_cast<PhysicalRegion (Image::*)(Image::PixelType) const>(&Image::physicalBoundingBox),
       "returns region of physical space occupied by the region of data <= the given isoValue",
       "isovalue"_a=1.0)

  .def("logicalToPhysical",
       [](Image& self, IndexRegion region) -> decltype(auto) {
         return self.logicalToPhysical(region);
       },
       "converts from a logical region (index coordinates) to a physical region",
       "region"_a)

  .def("logicalToPhysical",
       [](Image& self, std::vector<long>& c) -> decltype(auto) {
         return py::array(3, self.logicalToPhysical(Coord({c[0], c[1], c[2]})).GetDataPointer());
       },
       "converts a logical (index) coordinate to physical space",
       "c"_a)

  .def("physicalToLogical",
       [](Image& self, PhysicalRegion region) -> decltype(auto) {
         return self.physicalToLogical(region);
       },
       "converts from a physical region to a logical region (index coordinates)",
       "region"_a)

  .def("physicalToLogical",
       [](Image& self, std::vector<double>& p) -> decltype(auto) {
         return py::array(3, self.physicalToLogical(Point({p[0], p[1], p[2]})).data());
       },
       "converts a physical coordinate to a logical (index) space",
       "p"_a)

  .def("compare",
       &Image::compare,
       "compares two images",
       "other"_a, "verifyall"_a=true, "tolerance"_a=0.0, "precision"_a=1e-12)

  .def("toArray",
       [](const Image &image, bool copy, bool for_viewing) -> decltype(auto) {

         // We pass the array in column-major ('F') order when it will be used
         // for viewing (updating both shape and strides).
         const auto dims = image.dims();
         auto shape = std::vector<size_t>{dims[2], dims[1], dims[0]};
         if (for_viewing) {
           shape = std::vector<size_t>{dims[0], dims[1], dims[2]};
         }

         auto strides = std::vector<size_t>{
           dims[0] * dims[1] * sizeof(Image::PixelType),
           dims[0] * sizeof(Image::PixelType),
           sizeof(Image::PixelType)};
         if (for_viewing)
           strides = std::vector<size_t>{
             sizeof(Image::PixelType),
             dims[0] * sizeof(Image::PixelType),
             dims[0] * dims[1] * sizeof(Image::PixelType)};

         const auto py_dtype = py::dtype::of<Image::PixelType>();

#if 0
         std::cout << "Image info: " << std::endl
                   << "\tshape: " << shape[0] << " x " << shape[1] << " x " << shape[2] << std::endl
                   << "\tstrides: " << strides[0] << ", " << strides[1] << ", " << strides[2]
                   << "\tdtype: " << typeid(Image::PixelType).name()
                                  << " (" << sizeof(Image::PixelType) << " bytes)" << std::endl
                   << "\tpy_dtype: " << py_dtype << std::endl
                   << "\tcopy_requested: " << copy << std::endl
                   << "\tfor_viewing: " << for_viewing << std::endl;
#endif

         // When a valid object is passed as 'base', it tells pybind not to take
         // ownership of the data because 'base' will (allegedly) own it. Note
         // that ANY valid object is good for this purpose. This means that
         // image will continue to own its own array, which will be deleted
         // along with image (or when most itk operations are performed since
         // they tend to allocate fresh data).
         py::str dummyDataOwner;
         py::array img{
           py_dtype,
           shape,
           strides,
           image.getITKImage()->GetBufferPointer(),
           (copy ? pybind11::handle() : dummyDataOwner)
         };
         assert(copy == img.owndata());

         // prevent pyvista.wrap from copying (transpose will still work fine)
         if (for_viewing)
           pybind11::detail::array_proxy(img.ptr())->flags |= pybind11::detail::npy_api::NPY_ARRAY_F_CONTIGUOUS_;
         else
           pybind11::detail::array_proxy(img.ptr())->flags |= pybind11::detail::npy_api::NPY_ARRAY_C_CONTIGUOUS_;

         return img;
       },
       "returns raw array of image data, directly sharing data by default, copying if specified.\nNOTE: many Image operations reallocate image array, so while the array returned from this function is writable, it is best used immediately for Python operations; use for_viewing argument to get array in column-major ('F') order ('sw2vtkImage' already does this).",
       "copy"_a=false, "for_viewing"_a=false)

  .def("createTransform",
       py::overload_cast<XFormType>(&Image::createTransform),
       "creates a transform based on transform type",
       "type"_a=XFormType::CenterOfMass)

  .def("createTransform",
       py::overload_cast<const Image&, XFormType, float, unsigned>(&Image::createTransform),
       "creates a transform based on transform type",
       "target"_a, "type"_a=XFormType::IterativeClosestPoint, "isoValue"_a=0.0, "iterations"_a=20)

  .def("topologyPreservingSmooth",
       &Image::topologyPreservingSmooth,
       "creates a feature image (by applying gradient then sigmoid filters), then passes it to the TPLevelSet filter [curvature flow filter is often applied to the image before this filter]",
       "scaling"_a=20.0, "sigmoidAlpha"_a=10.5, "sigmoidBeta"_a=10.0)

  .def("compare",
       &Image::compare,
       "compares two images",
       "other"_a, "verifyall"_a=true, "tolerance"_a=0.0, "precision"_a=1e-12)

  .def("toMesh",
       [](Image &image, Image::PixelType isovalue) {
         return image.toMesh(isovalue);
       },
       "converts image to mesh at specified isovalue",
       "isovalue"_a)
  ;

  // PhysicalRegion
  py::class_<PhysicalRegion>(m, "PhysicalRegion")

  .def(py::init<>())

  .def(py::init
       ([](std::vector<double> min, std::vector<double> max) {
          return PhysicalRegion(Point({min[0], min[1], min[2]}),
                                Point({max[0], max[1], max[2]}));
        }))

  .def(py::self == py::self)

  .def("__repr__",
       [](const PhysicalRegion &region) {
         std::stringstream stream;
         stream << region;
         return stream.str();
       })

  .def_property("min",
                // read (return special array so members of returned point can be modified, e.g., min[0] = 1.0)
                py::cpp_function([](PhysicalRegion &region) -> py::array_t<double> {
                    py::str dummyDataOwner; // pretend the data has an owner and it won't be copied (pybind trick)
                    py::array arr(py::dtype::of<Point::ValueType>(),      // dtype
                                  std::vector<ssize_t>({3}),              // shape
                                  std::vector<ssize_t>(),                 // spacing
                                  region.min.GetDataPointer(),            // data ptr
                                  dummyDataOwner);     // "inspire" py::array not to copy data
                    return arr;
                  }, py::return_value_policy::move),

                // assign
                py::cpp_function([](PhysicalRegion &region, std::vector<double> min) -> decltype(auto) {
                                   region.min = Point({min[0], min[1], min[2]});
                                   return min;
                                 }, py::return_value_policy::reference),
                "min point of region")

  .def_property("max",
                // read (return special array so members of returned point can be modified, e.g., max[0] = 1.0)
                py::cpp_function([](PhysicalRegion &region) -> py::array_t<double> {
                    py::str dummyDataOwner; // pretend the data has an owner and it won't be copied (pybind trick)
                    py::array arr(py::dtype::of<Point::ValueType>(),      // dtype
                                  std::vector<ssize_t>({3}),              // shape
                                  std::vector<ssize_t>(),                 // spacing
                                  region.max.GetDataPointer(),            // data ptr
                                  dummyDataOwner);     // "inspire" py::array not to copy data
                    return arr;
                  }, py::return_value_policy::move),

                // assign
                py::cpp_function([](PhysicalRegion &region, std::vector<double> max) -> decltype(auto) {
                                   region.max = Point({max[0], max[1], max[2]});
                                   return max;
                                 }, py::return_value_policy::reference),
                "max point of region")

  .def("valid",
       &PhysicalRegion::valid,
       "ensure if region is valid")

  .def("origin",
       [](const PhysicalRegion &region) -> decltype(auto) {
         return py::array(3, region.origin().GetDataPointer());
       },
       "return origin of region")

  .def("size",
       [](const PhysicalRegion &region) -> decltype(auto) {
         return py::array(3, region.size().GetDataPointer());
       },
       "return size of region")

  .def("shrink",
       &PhysicalRegion::shrink,
       "shrink this region down to the smallest portions of both",
       "other"_a)

  .def("expand",
       py::overload_cast<const PhysicalRegion&>(&PhysicalRegion::expand),
       "expand this region up to the largest portions of both",
       "other"_a)

  .def("expand",
       py::overload_cast<const Point&>(&PhysicalRegion::expand),
       "expand this region to include this point",
       "point"_a)

  .def("pad",
       &PhysicalRegion::pad,
       "grows or shrinks the region by the specified amount",
       "padding"_a)
  ;

  // IndexRegion
  py::class_<IndexRegion>(m, "IndexRegion")

  .def(py::init<>())

  .def(py::init
       ([](std::vector<double> min, std::vector<double> max) {
          return IndexRegion(Coord({static_cast<long>(min[0]),
                                    static_cast<long>(min[1]),
                                    static_cast<long>(min[2])}),
                             Coord({static_cast<long>(max[0]),
                                    static_cast<long>(max[1]),
                                    static_cast<long>(max[2])}));
        }))

  .def(py::init
       ([](std::vector<double> dims) {
          if (dims[0] < 0.0 || dims[1] < 0.0 || dims[2] < 0.0)
            throw std::invalid_argument("cannot convert negative values to IndexRegion");
          return IndexRegion(Dims({static_cast<unsigned long>(dims[0]),
                                   static_cast<unsigned long>(dims[1]),
                                   static_cast<unsigned long>(dims[2])}));
        }))

  .def(py::self == py::self)

  .def("__repr__",
       [](const IndexRegion &region) {
         std::stringstream stream;
         stream << region;
         return stream.str();
       })

  .def_property("min",
                // read (return special array so members of returned point can be modified, e.g., min[0] = 1.0)
                py::cpp_function([](IndexRegion &region) -> py::array_t<Coord::IndexValueType> {
                    py::str dummyDataOwner; // pretend the data has an owner and it won't be copied (pybind trick)
                    py::array arr(py::dtype::of<Coord::IndexValueType>(), // dtype
                                  std::vector<ssize_t>({3}),              // shape
                                  std::vector<ssize_t>(),                 // spacing
                                  region.min.data(),                      // data ptr
                                  dummyDataOwner);     // "inspire" py::array not to copy data
                    return arr;
                  }, py::return_value_policy::move),

                // assign
                py::cpp_function([](IndexRegion &region, std::vector<double> min) -> decltype(auto) {
                                   region.min = Coord({static_cast<Coord::IndexValueType>(min[0]),
                                                       static_cast<Coord::IndexValueType>(min[1]),
                                                       static_cast<Coord::IndexValueType>(min[2])});
                                   return min;
                                 }, py::return_value_policy::reference),
                "min point of region")

  .def_property("max",
                // read (return special array so members of returned point can be modified, e.g., max[0] = 1.0)
                py::cpp_function([](IndexRegion &region) -> py::array_t<Coord::IndexValueType> {
                    py::str dummyDataOwner; // pretend the data has an owner and it won't be copied (pybind trick)
                    py::array arr(py::dtype::of<Coord::IndexValueType>(), // dtype
                                  std::vector<ssize_t>({3}),              // shape
                                  std::vector<ssize_t>(),                 // spacing
                                  region.max.data(),                      // data ptr
                                  dummyDataOwner);     // "inspire" py::array not to copy data
                    return arr;
                  }, py::return_value_policy::move),

                // assign
                py::cpp_function([](IndexRegion &region, std::vector<double> max) -> decltype(auto) {
                                   region.max = Coord({static_cast<Coord::IndexValueType>(max[0]),
                                                       static_cast<Coord::IndexValueType>(max[1]),
                                                       static_cast<Coord::IndexValueType>(max[2])});
                                   return max;
                                 }, py::return_value_policy::reference),
                "max point of region")

  .def("valid",
       &IndexRegion::valid,
       "ensure if region is valid")

  .def("origin",
       [](const IndexRegion &region) -> decltype(auto) {
         return py::array(3, region.origin().data());
       },
       "return origin of region")

  .def("size",
       [](const IndexRegion &region) -> decltype(auto) {
         return py::array(3, region.size().data());
       },
       "return size of region")

  .def("pad",
       &IndexRegion::pad,
       "grows or shrinks the region by the specified amount",
       "padding"_a)
  ;

  // VectorImage
  py::class_<VectorImage>(m, "VectorImage")

  .def(py::init
       ([](const Image &dt) {
          return VectorImage(dt);
        }),
       "create a vector image from an image (usually a distance transform) that can be sampled at any point in space",
       "image"_a)

  .def("evaluate",
       [](VectorImage &image, std::vector<double> &pt) -> decltype(auto) {
         auto v = image.evaluate(Point({pt[0], pt[1], pt[2]}));
         return std::vector<double>({v[0], v[1], v[2]});
       },
       "evaluate the vector image at any given point in space",
       "pt"_a)
  ;

  // ImageUtils
  py::class_<ImageUtils>(m, "ImageUtils")

  .def_static("boundingBox",
              py::overload_cast<const std::vector<std::string>&, Image::PixelType>(&ImageUtils::boundingBox),
              "compute largest bounding box surrounding the specified isovalue of the specified set of filenames",
              "filenames"_a, "isoValue"_a=1.0)

  .def_static("boundingBox",
              py::overload_cast<const std::vector<std::reference_wrapper<const Image>>&, Image::PixelType>(&ImageUtils::boundingBox),
              "compute largest bounding box surrounding the specified isovalue of the specified set of images",
              "images"_a, "isoValue"_a=1.0)

  .def_static("createWarpTransform",
              [](const std::string &source_landmarks,
                 const std::string &target_landmarks,
                 const int stride) -> decltype(auto) {
                auto xform_ptr = ImageUtils::createWarpTransform(source_landmarks,
                                                                 target_landmarks,
                                                                 stride);
                return xform_ptr;
              },
              "computes a warp transform from the source to the target landmarks",
              "source_landmarks"_a, "target_landmarks"_a, "stride"_a=1)
  ;

  // Mesh
  py::class_<Mesh> mesh(m, "Mesh");

  // Mesh::AlignmentType
  py::enum_<Mesh::AlignmentType>(mesh, "AlignmentType")
  .value("Rigid", Mesh::AlignmentType::Rigid)
  .value("Similarity", Mesh::AlignmentType::Similarity)
  .value("Affine", Mesh::AlignmentType::Affine)
  .export_values();
  ;

  // Mesh::DistanceMethod
  py::enum_<Mesh::DistanceMethod>(mesh, "DistanceMethod")
  .value("PointToPoint", Mesh::DistanceMethod::PointToPoint)
  .value("PointToCell", Mesh::DistanceMethod::PointToCell)
  .export_values();
  ;

  // Mesh::CurvatureType
  py::enum_<Mesh::CurvatureType>(mesh, "CurvatureType")
  .value("Principal", Mesh::CurvatureType::Principal)
  .value("Gaussian", Mesh::CurvatureType::Gaussian)
  .value("Mean", Mesh::CurvatureType::Mean)
  .export_values();
  ;

  // Mesh bindings
  mesh.def(py::init<const std::string &>())

  .def(py::init<vtkSmartPointer<vtkPolyData>>())
  .def(py::self == py::self)
  .def(py::self += py::self)

  .def("__repr__",
       [](const Mesh &mesh) -> decltype(auto) {
         std::stringstream stream;
         stream << mesh;
         return stream.str();
       })

  .def("copy",
       [](Mesh& mesh) -> decltype(auto) { return Mesh(mesh); })

  .def("write",
       &Mesh::write,
       "writes mesh, format specified by filename extension",
       "pathname"_a)

  .def("coverage",
       &Mesh::coverage,
       "determines coverage between current mesh and another mesh (e.g. acetabular cup / femoral head)",
       "otherMesh"_a, "allowBackIntersections"_a=true, "angleThreshold"_a=0, "backSearchRadius"_a=0)

  .def("smooth",
       &Mesh::smooth,
       "applies laplacian smoothing",
       "iterations"_a=0, "relaxation"_a=0.0)

  .def("smoothSinc",
       &Mesh::smoothSinc,
       "applies windowed sinc smoothing",
       "iterations"_a=0, "passband"_a=0.0)

  .def("decimate",
       &Mesh::decimate,
       "applies filter to reduce number of triangles in mesh",
       "reduction"_a=0.5, "angle"_a=15.0, "preserveTopology"_a=true)

   .def("cvdDecimate",
       &Mesh::cvdDecimate,
       "applies cvd (centroidal voronoi diagram) decimation filter",
       "percentage"_a=0.5)

  .def("invertNormals",
       &Mesh::invertNormals,
       "handle flipping normals")

  .def("reflect",
       [](Mesh& mesh, const Axis &axis, std::vector<double>& v) -> decltype(auto) {
         return mesh.reflect(axis, makeVector({v[0], v[1], v[2]}));
       },
       "reflect meshes with respect to a specified center and specific axis",
       "axis"_a, "origin"_a=std::vector<double>({0.0, 0.0, 0.0}))

  .def("createTransform",
       &Mesh::createTransform,
       "creates a transform based on transform type",
       "target"_a, "type"_a=XFormType::IterativeClosestPoint,
       "align"_a=Mesh::AlignmentType::Similarity, "iterations"_a=10)

  .def("applyTransform",
       &Mesh::applyTransform,
       "applies the given transformation to the mesh",
       "transform"_a)

  .def("fillHoles",
       &Mesh::fillHoles,
       "finds holes in a mesh and closes them")

  .def("probeVolume",
       &Mesh::probeVolume,
       "samples data values at specified point locations",
       "image"_a)

  .def("clip",
       [](Mesh& mesh, const std::vector<double>& p, const std::vector<double>& n) -> decltype(auto) {
         return mesh.clip(makePlane(Point({p[0], p[1], p[2]}), makeVector({n[0], n[1], n[2]})));
       },
       "clips a mesh using a cutting plane",
       "point"_a,
       "normal"_a)

  .def("clip",
       [](Mesh& mesh, const std::vector<double>& o, const std::vector<double>& p1, const std::vector<double>& p2) -> decltype(auto) {
         return mesh.clip(makePlane(Point({o[0], o[1], o[2]}), Point({p1[0], p1[1], p1[2]}), Point({p2[0], p2[1], p2[2]})));
       },
       "clips a mesh using a cutting plane",
       "o"_a,
       "p1"_a,
       "p2"_a)

  .def("translate",
       [](Mesh& mesh, const std::vector<double>& v) -> decltype(auto) {
         return mesh.translate(makeVector({v[0], v[1], v[2]}));
       },
       "translates mesh",
       "v"_a)

  .def("scale",
       [](Mesh& mesh, const std::vector<double>& v) -> decltype(auto) {
         return mesh.scale(makeVector({v[0], v[1], v[2]}));
       },
       "scale mesh",
       "v"_a)

  .def("boundingBox",
       &Mesh::boundingBox,
       "computes bounding box of current mesh")

  .def("fixElement",
       &Mesh::fixElement,
       "fix element winding of mesh")

  .def("distance",
       &Mesh::distance,
       "computes surface to surface distance using the specified method (PointToPoint or PointToCell)",
       "target"_a, "method"_a=Mesh::DistanceMethod::PointToPoint)

  .def("clipClosedSurface",
       [](Mesh& mesh, const std::vector<double>& p, const std::vector<double>& n) -> decltype(auto) {
         return mesh.clipClosedSurface(makePlane(Point({p[0], p[1], p[2]}), makeVector({n[0], n[1], n[2]})));
       },
       "clips a mesh using a cutting plane resulting in a closed surface",
       "point"_a,
       "normal"_a)

  .def("computeNormals",
       &Mesh::computeNormals,
       "computes and adds oriented point and cell normals")

  .def("closestPoint",
       [](Mesh &mesh, std::vector<double> p) -> decltype(auto) {
         return py::array(3, mesh.closestPoint(Point({p[0], p[1], p[2]})).GetDataPointer());
       },
       "returns closest point to given point on mesh",
       "point"_a)

  .def("closestPointId",
       [](Mesh &mesh, std::vector<double> p) -> decltype(auto) {
         return mesh.closestPointId(Point({p[0], p[1], p[2]}));
       },
       "returns closest point id in this mesh to the given point in space",
       "point"_a)

  .def("geodesicDistance",
       py::overload_cast<int, int>(&Mesh::geodesicDistance),
       "computes geodesic distance between two vertices (specified by their indices) on mesh",
       "source"_a, "target"_a)

  .def("geodesicDistance",
       [](Mesh &mesh, const std::vector<double> p) -> decltype(auto) {
          auto array = mesh.geodesicDistance(Point({p[0], p[1], p[2]}));
          return arrToPy(array, MOVE_ARRAY);
       },
       "computes geodesic distance between a point (landmark) and each vertex on mesh",
       "landmark"_a)

  .def("geodesicDistance",
       [](Mesh &mesh, const std::vector<std::vector<double>> p) -> decltype(auto) {
          std::vector<Point> points;
          for (int i=0; i<p.size(); i++)
          {
            points.push_back(Point({p[i][0], p[i][0], p[i][2]}));
          }
          auto array = mesh.geodesicDistance(points);
          return arrToPy(array, MOVE_ARRAY);
       },
       "computes geodesic distance between a set of points (curve) and all vertices on mesh",
       "curve"_a)

  .def("curvature",
       [](Mesh &mesh, const Mesh::CurvatureType type) -> decltype(auto) {
          auto array = mesh.curvature(type);
          return arrToPy(array, MOVE_ARRAY);
     },
     "computes and adds curvature (principal (default) or gaussian or mean)",
     "type"_a=Mesh::CurvatureType::Principal)

  .def("toImage",
       [](Mesh& mesh, PhysicalRegion &region, std::vector<double>& spacing) -> decltype(auto) {
         return mesh.toImage(region, Point({spacing[0], spacing[1], spacing[2]}));
       },
       "rasterizes specified region to create binary image of desired dims (default: unit spacing)",
       "region"_a=PhysicalRegion(),
       "spacing"_a=std::vector<double>({1.0, 1.0, 1.0}))

  .def("toDistanceTransform",
       [](Mesh& mesh, PhysicalRegion &region, std::vector<double>& spacing) -> decltype(auto) {
         return mesh.toDistanceTransform(region, Point({spacing[0], spacing[1], spacing[2]}));
       },
       "converts specified region to distance transform image (default: unit spacing)",
       "region"_a=PhysicalRegion(),
       "spacing"_a=std::vector<double>({1.0, 1.0, 1.0}))

  .def("center",
       [](Mesh &mesh) -> decltype(auto) {
         return py::array(3, mesh.center().GetDataPointer());
       },
       "center of mesh")

  .def("centerOfMass",
       [](Mesh &mesh) -> decltype(auto) {
         return py::array(3, mesh.centerOfMass().GetDataPointer());
       },
       "center of mass of mesh")

  .def("numPoints",
       &Mesh::numPoints,
       "number of points")

  .def("numFaces",
       &Mesh::numFaces,
       "number of faces")

  .def("points",
       &Mesh::points,
       "matrix with number of points with (x,y,z) coordinates of each point")

  .def("faces",
       &Mesh::faces,
       "matrix with number of faces with indices of the three points from which each face is composed")

  .def("getPoint",
       [](Mesh &mesh, int id) -> decltype(auto) {
         return py::array(3, mesh.getPoint(id).GetDataPointer());
       },
       "(x,y,z) coordinates of vertex at given index",
       "id"_a)

  .def("getFace",
       [](Mesh &mesh, int id) -> decltype(auto) {
         return py::array(3, mesh.getFace(id).GetDataPointer());
       },
       "return indices of the three points with which the face at the given index is composed",
       "id"_a)

  .def("getFieldNames",
       &Mesh::getFieldNames,
       "print all field names in mesh")

  .def("setField",
       [](Mesh &mesh, const std::string& name, py::array& array) -> decltype(auto) {
         auto vtkarr = pyToArr(array);
         return mesh.setField(name, vtkarr);
       },
       "sets the given field for points with array",
       "name"_a, "array"_a)

  .def("getField",
       [](const Mesh &mesh, std::string name) -> decltype(auto) {
         auto array = mesh.getField<vtkDataArray>(name);
         if (!array) {
           throw std::invalid_argument("field '" + name + "' does not exist");
         }
         return arrToPy(array, SHARE_ARRAY);
       },
       "gets the field",
       "name"_a)

  .def("setFieldValue",
       &Mesh::setFieldValue,
       "sets the given index of field to value",
       "idx"_a, "value"_a, "name"_a="")

  .def("getFieldValue",
       &Mesh::getFieldValue,
       "gets the value at the given index of field",
       "idx"_a, "name"_a)

    .def("getMultiFieldValue",
         &Mesh::getMultiFieldValue,
         "gets the vector value at the given index of field",
         "idx"_a, "name"_a)

  .def("getFieldRange",
       &Mesh::getFieldRange,
       "returns the range of the given field",
       "name"_a)

  .def("getFieldMean",
       &Mesh::getFieldMean,
       "returns the mean the given field",
       "name"_a)

  .def("getFieldStd",
       &Mesh::getFieldStd,
       "returns the standard deviation of the given field",
       "name"_a)

  .def("compareField",
       &Mesh::compareField,
       "compares two meshes based on fields",
       "other_mesh"_a, "name1"_a, "name2"_a="", "eps"_a=-1.0)
  ;

  // MeshUtils
  py::class_<MeshUtils>(m, "MeshUtils")

  .def_static("boundingBox",
              py::overload_cast<const std::vector<std::string>&, bool>(&MeshUtils::boundingBox),
              "calculate bounding box incrementally for meshes",
              "filenames"_a, "center"_a=false)

  .def_static("boundingBox",
              py::overload_cast<const std::vector<std::reference_wrapper<const Mesh>>&, bool>(&MeshUtils::boundingBox),
              "calculate bounding box incrementally for meshes",
              "meshes"_a, "center"_a=false)

  .def_static("findReferenceMesh",
              &MeshUtils::findReferenceMesh,
              "find reference mesh from a set of meshes",
              "meshes"_a)

  .def_static("generateNormals",
              &MeshUtils::generateNormals,
              "generates and adds normals for points and faces for each mesh in given set of meshes",
              "meshes"_a, "forceRegen"_a=false)

  .def_static("computeMeanNormals",
               [](const std::vector<std::string>& filenames, bool autoGenerateNormals) -> decltype(auto) {
                  auto array = MeshUtils::computeMeanNormals(filenames, autoGenerateNormals);
                  return arrToPy(array, MOVE_ARRAY);
               },
               "computes average normals for each point in given set of meshes",
               "filenames"_a, "autoGenerateNormals"_a=true)

  .def_static("computeMeanNormals",
               [](const std::vector<std::reference_wrapper<const Mesh>>& meshes) -> decltype(auto) {
                  auto array = MeshUtils::computeMeanNormals(meshes);
                  return arrToPy(array, MOVE_ARRAY);
               },
               "computes average normals for each point in given set of meshes",
               "meshes"_a)
  ;

  // ParticleSystem
  py::class_<ParticleSystem>(m, "ParticleSystem")

  .def(py::init<const std::vector<std::string> &>())

  .def("Particles",
       &ParticleSystem::Particles)

  .def("Paths",
       &ParticleSystem::Paths)

  .def("N",
       &ParticleSystem::N)

  .def("D",
       &ParticleSystem::D)
  ;

  // ShapeEvaluation
  py::class_<ShapeEvaluation>(m, "ShapeEvaluation")

  .def_static("ComputeCompactness",
              &ShapeEvaluation::ComputeCompactness,
              "particleSystem"_a, "nModes"_a, "saveTo"_a="")

  .def_static("ComputeGeneralization",
              &ShapeEvaluation::ComputeGeneralization,
              "particleSystem"_a, "nModes"_a, "saveTo"_a="")

  .def_static("ComputeSpecificity",
              &ShapeEvaluation::ComputeSpecificity,
              "particleSystem"_a, "nModes"_a, "saveTo"_a="")

  .def_static("ComputeFullCompactness",
              &ShapeEvaluation::ComputeFullCompactness,
              "particleSystem"_a,"progress_callback"_a=nullptr)

  .def_static("ComputeFullGeneralization",
              &ShapeEvaluation::ComputeFullGeneralization,
              "particleSystem"_a,"progress_callback"_a=nullptr)

  .def_static("ComputeFullSpecificity",
              &ShapeEvaluation::ComputeFullSpecificity,
              "particleSystem"_a,"progress_callback"_a=nullptr)
  ;

  py::class_<ParticleShapeStatistics>(m, "ParticleShapeStatistics")

  .def(py::init<>())

  .def("PCA",
       py::overload_cast<ParticleSystem, int>(&ParticleShapeStatistics::DoPCA),
       "calculates the eigen values and eigen vectors of the data",
       "particleSystem"_a, "domainsPerShape"_a=1)

  .def("principalComponentProjections",
       &ParticleShapeStatistics::PrincipalComponentProjections,
       "projects the original data on the calculated principal components")

  .def("sampleSize",
       &ParticleShapeStatistics::SampleSize,
       "returns the sample size of the particle system")

  .def("numDims",
       &ParticleShapeStatistics::NumberOfDimensions,
       "returns the number of features of the particle system")

  .def("eigenVectors",
       [](ParticleShapeStatistics &stats) -> decltype(auto) {
         return vnlToEigen(stats.Eigenvectors());
       })

  .def("eigenValues",
       &ParticleShapeStatistics::Eigenvalues,
       "return the eigen values")

  .def("pcaLoadings",
       &ParticleShapeStatistics::PCALoadings,
       "returns the  coefficients of the linear combination of \
       the original variables from which the principal \
       components are constructed")

  .def("percentVarByMode",
       &ParticleShapeStatistics::PercentVarByMode,
       "return the variance accounted for by the principal components")
  ;

  // Optimize (TODO)
  py::class_<Optimize>(m, "Optimize")

  .def(py::init<>())

  .def("LoadParameterFile",
       &Optimize::LoadParameterFile)

  .def("Run",
       &Optimize::Run)

  .def("SetIterationCallbackFunction",
       &Optimize::SetIterationCallbackFunction)

  .def("GetParticleSystem",
       &optimize_get_particle_system)
  ;

} // PYBIND11_MODULE(shapeworks_py)


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

/* Author: Yu Fu */

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float64MultiArray.h>
#include <open_manipulator_msgs/SetKinematicsPose.h>
#include <open_manipulator_msgs/SetJointPosition.h>
#include <open_manipulator_msgs/KinematicsPose.h>
#include <open_manipulator_msgs/JointPosition.h>
#include <geometry_msgs/Pose.h>
#include <vector>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <Eigen/Eigen>
#include "omtb_control/omtb_kinematics.h"
#include "semantic_pick/ObjectPosition.h"

using namespace std;
#define CLASS_UPPER_LIMIT 7

geometry_msgs::Pose get_pose(float position[])
{
  geometry_msgs::Pose object_pose;
  object_pose.position.x = position[0];
  object_pose.position.y = position[1];
  object_pose.position.z = position[2];
  float dist = sqrt((object_pose.position.x * object_pose.position.x) +
                   (object_pose.position.y * object_pose.position.y));
  float roll = 0.0;
  float pitch = 0.0;
  float yaw = 0.0;
  if (object_pose.position.y > 0)
      yaw = acos(object_pose.position.x / dist);
  else
      yaw = (-1) * acos(object_pose.position.x / dist);
  float cy = cos(yaw * 0.5);
  float sy = sin(yaw * 0.5);
  float cr = cos(roll * 0.5);
  float sr = sin(roll * 0.5);
  float cp = cos(pitch * 0.5);
  float sp = sin(pitch * 0.5);

  object_pose.orientation.w = cy * cr * cp + sy * sr * sp;
  object_pose.orientation.x = cy * sr * cp - sy * cr * sp;
  object_pose.orientation.y = cy * cr * sp + sy * sr * cp;
  object_pose.orientation.z = sy * cr * cp - cy * sr * sp;
  return object_pose;
}

void gripper_control(ros::ServiceClient gripper_client, float angle)
{
  //close gripper
  open_manipulator_msgs::SetJointPosition gripper_test;
  open_manipulator_msgs::JointPosition gripper_joint;
  gripper_joint.position = {angle};
  gripper_joint.max_velocity_scaling_factor = 0.1;
  gripper_joint.max_accelerations_scaling_factor = 0.1;
  gripper_test.request.planning_group = "arm";
  gripper_test.request.joint_position = gripper_joint;
  if (gripper_client.call(gripper_test)) {
      if (gripper_test.response.is_planned)
      {
        cout<<"success"<<endl;
      }
      else
      {
        cout<<"fail"<<endl;
      }
  }
  else {
      ROS_ERROR("Failed to call service!!");
  }
  ros::Duration(3.0).sleep();
}

void manipulator_move(float position[], Eigen::Isometry3d end_to_world, Eigen::Isometry3d link1_to_world, ros::ServiceClient arm_client)
{
  end_to_world.pretranslate(Eigen::Vector3d(position[0], position[1], position[2]));
  Eigen::Isometry3d end_to_link1 = end_to_world*link1_to_world.inverse();
  Eigen::Vector3d goal_to_link1_vector = end_to_link1.translation();
  std::vector<double> goal_vector;
  goal_vector.push_back(goal_to_link1_vector(0));
  goal_vector.push_back(goal_to_link1_vector(1));
  goal_vector.push_back(goal_to_link1_vector(2));
  OmtbKinematics solver;
  if (solver.inverse_kinematics(goal_vector))
  {
    open_manipulator_msgs::SetJointPosition arm_test;
    open_manipulator_msgs::JointPosition arm_joint;
    arm_joint.position = solver.joint_position_;
    arm_joint.max_velocity_scaling_factor = 0.1;
    arm_joint.max_accelerations_scaling_factor = 0.1;

    arm_test.request.joint_position = arm_joint;
    arm_test.request.planning_group = "arm";

    if (arm_client.call(arm_test)) {
        if (arm_test.response.is_planned)
        {
          cout<<"success"<<endl;
        }
        else
        {
          cout<<"fail"<<endl;
        }
    }
    else {
        ROS_ERROR("Failed to call service!!");
    }
  }
  ros::Duration(3.0).sleep();
}

void manipulator_srv(semantic_pick::ObjectPosition srv, Eigen::Isometry3d end_to_world, Eigen::Isometry3d link1_to_world, ros::ServiceClient arm_client)
{
  //move manipulator
  float object_max_height=0;
  int object_max_index=0;
  for (int i=0;i<srv.response.object_class.size();i++)
  {
    if (srv.response.position_z[i]>object_max_height)
    {
      object_max_index=i;
      object_max_height=srv.response.position_z[i];
    }
  }
  float position[3] = {srv.response.position_x[object_max_index], srv.response.position_y[object_max_index], srv.response.position_z[object_max_index]};
  manipulator_move(position, end_to_world, link1_to_world, arm_client);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "omtb_moveit");
  ros::NodeHandle node_handle("");
//  std::string  planning_group = argv[1];
//  std::string service_name;
//  service_name << planning_group << '/moveit/set_kinematics_pose';

  ros::ServiceClient arm_client = node_handle.serviceClient<open_manipulator_msgs::SetJointPosition>("robot1/arm/moveit/set_joint_position");
  ros::ServiceClient gripper_client = node_handle.serviceClient<open_manipulator_msgs::SetJointPosition>("robot1/gripper");
  ros::ServiceClient client = node_handle.serviceClient<semantic_pick::ObjectPosition>("provide_objects");

  static tf::TransformListener listener;
  tf::StampedTransform transform;
  try
  {
    listener.waitForTransform("robot1/odom", "robot1/link1", ros::Time(0), ros::Duration(5.0));
    listener.lookupTransform("robot1/odom", "robot1/link1", ros::Time(0), transform);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }
  Eigen::Quaterniond link1_to_world_q(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
  Eigen::Isometry3d link1_to_world(link1_to_world_q);
  link1_to_world.pretranslate( Eigen::Vector3d( transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()));
  Eigen::Isometry3d end_to_world=Eigen::Isometry3d::Identity();
  semantic_pick::ObjectPosition srv;
  if (client.call(srv))
  {
    if (srv.response.object_class.size()==0)
    {
      cout << "No object found!" <<endl;
    }
    else
    {
      gripper_control(gripper_client, 0.15);
      manipulator_srv(srv, end_to_world, link1_to_world, arm_client);
      gripper_control(gripper_client, -0.1);
      float position1[3] = {0.25, 0.1, 0.25};
      manipulator_move( position1, end_to_world, link1_to_world, arm_client);

      //open gripper
      float position2[3] = {0.25, 0.1, 0.1};
      manipulator_move( position2, end_to_world, link1_to_world, arm_client);
      gripper_control(gripper_client, 0.15);
    }
  }
  else
  {
    cout << "Can not get object position ROS service!" <<endl;
  }

}


//          Copyright Rein Halbersma 2010-2020.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <dctl/core/model/detail/stride.hpp>    // stride
#include <core/board/sequence.hpp>              // nano, micro, checkers, Roman, spantsiretti, international, frisian, ktar<10, 11>,
                                                // ktar<10, 12>, Compact_10_12, Compact_12_10, basic_board<12, 10>, canadian, srilankan, dumm
#include <dctl/core/board/angle.hpp>            // angle, reverse
#include <boost/test/unit_test.hpp>             // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE, BOOST_CHECK_EQUAL
#include <algorithm>                            // all_of
#include <vector>                               // vector

using namespace dctl::core;
using namespace literals;

BOOST_AUTO_TEST_SUITE(ActionsStride)

BOOST_AUTO_TEST_CASE_TEMPLATE(ReverseAngleSymmetryForAllDirections, T, BoardSequence)
{
        auto const directions = std::vector<angle>
        {
                   0_deg,  45_deg,  90_deg, 135_deg,
                 180_deg, 225_deg, 270_deg, 315_deg
        };

        BOOST_CHECK(
                std::all_of(directions.begin(), directions.end(), [](auto dir) {
                        return stride<T>{}(dir) == stride<T>{}(reverse(dir));
                })
        );
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_prismatic_unaligned_hpp__
#define __pinocchio_joint_prismatic_unaligned_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
  typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;

  namespace internal
  {
    template<typename Scalar, int Options>
    struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
    {
      typedef MotionTpl<Scalar,Options> ReturnType;
    };

    template<typename Scalar, int Options, typename MotionDerived>
    struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
    {
      typedef MotionTpl<Scalar,Options> ReturnType;
    };
  }

  template<typename _Scalar, int _Options>
  struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
    typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionPrismaticUnalignedTpl

  template<typename _Scalar, int _Options>
  struct MotionPrismaticUnalignedTpl : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);

    MotionPrismaticUnalignedTpl() {}

    template<typename Vector3Like, typename S2>
    MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
                                const S2 rate)
    : axis(axis), rate(rate)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }

//    operator MotionPlain() const
//    { return MotionPlain(axis*rate,MotionPlain::Vector3::Zero());}

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      other.linear() += axis * rate;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear().noalias() = axis*rate;
      other.angular().setZero();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.linear().noalias() = rate * (m.rotation() * axis); // TODO: check efficiency
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      v.linear().noalias() = rate * (m.rotation().transpose() * axis);

      // Angular
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.angular().cross(axis);
      mout.linear() *= rate;

      // Angular
      mout.angular().setZero();
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    // data
    Vector3 axis;
    Scalar rate;
  }; // struct MotionPrismaticUnalignedTpl

  template<typename Scalar, int Options, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionPrismaticUnalignedTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2)
  {
    typedef typename MotionDerived::MotionPlain ReturnType;
    return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
  }

  template<typename Scalar, int Options> struct ConstraintPrismaticUnaligned;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintPrismaticUnaligned<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix3 Angular_t;
    typedef Vector3 Linear_t;
    typedef const Matrix3 ConstAngular_t;
    typedef const Vector3 ConstLinear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef ForceTpl<Scalar,Options> Force;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // traits ConstraintPrismaticUnaligned

  template<typename _Scalar, int _Options>
  struct ConstraintPrismaticUnaligned : ConstraintBase< ConstraintPrismaticUnaligned<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    SPATIAL_TYPEDEF_TEMPLATE(ConstraintPrismaticUnaligned);
    enum { NV = 1, Options = _Options };

    typedef typename traits<ConstraintPrismaticUnaligned>::JointMotion JointMotion;
    typedef typename traits<ConstraintPrismaticUnaligned>::JointForce JointForce;
    typedef typename traits<ConstraintPrismaticUnaligned>::DenseBase DenseBase;

    ConstraintPrismaticUnaligned() {}

    template<typename Vector3Like>
    ConstraintPrismaticUnaligned(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }

    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
      return JointMotion(axis,v[0]);
    }

    template<typename S1, int O1>
    Vector6 se3Action(const SE3Tpl<S1,O1> & m) const
    {
      Vector6 res;
      res.template head<3>().noalias() = m.rotation()*axis;
      res.template tail<3>().setZero();
      return res;
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      typedef typename traits<ConstraintPrismaticUnaligned>::Scalar Scalar;
      typedef typename traits<ConstraintPrismaticUnaligned>::Force Force;
      typedef typename traits<ConstraintPrismaticUnaligned>::Vector6 Vector6;

      const ConstraintPrismaticUnaligned & ref;
      TransposeConst(const ConstraintPrismaticUnaligned & ref) : ref(ref) {}

      template<typename Derived>
      Eigen::Matrix<
      typename EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstLinearType),
      1,1>
      operator* (const ForceDense<Derived> & f) const
      {
        typedef Eigen::Matrix<
        typename EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<Derived>::ConstLinearType),
        1,1> ReturnType;

        ReturnType res; res[0] = ref.axis.dot(f.linear());
        return res;
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename D>
      friend
#if EIGEN_VERSION_AT_LEAST(3,2,90)
      const Eigen::Product<
      Eigen::Transpose<const Vector3>,
      typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
      >
#else
      const typename Eigen::ProductReturnType<
      Eigen::Transpose<const Vector3>,
      typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
      >::Type
#endif
      operator* (const TransposeConst & tc, const Eigen::MatrixBase<D> & F)
      {
        EIGEN_STATIC_ASSERT(D::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
        /* Return ax.T * F[1:3,:] */
        return tc.ref.axis.transpose () * F.template topRows<3> ();
      }

    };
    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
    DenseBase matrix_impl() const
    {
      DenseBase S;
      S << axis, Vector3::Zero();
      return S;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & v) const
    {
      DenseBase res;
      res << v.angular().cross(axis), Vector3::Zero();

      return res;
    }

    // data
    Vector3 axis;

  }; // struct ConstraintPrismaticUnaligned

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionPrismaticUnalignedTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S1,6,1>
  operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticUnaligned<S2,O2> & cpu)
  {
    typedef InertiaTpl<S1,O1> Inertia;
    /* YS = [ m -mcx ; mcx I-mcxcx ] [ v ; 0 ] = [ mv ; mcxv ] */
    const S1 & m                             = Y.mass();
    const typename Inertia::Vector3 & c      = Y.lever();

    Eigen::Matrix<S1,6,1> res;
    res.template head<3>().noalias() = m*cpu.axis;
    res.template tail<3>() = c.cross(res.template head<3>());
    return res;
  }

  /* [ABA] Y*S operator (Inertia Y,Constraint S) */
  template<typename M6, typename S2, int O2>
#if EIGEN_VERSION_AT_LEAST(3,2,90)
  const typename Eigen::Product<
  Eigen::Block<const M6,6,3>,
  typename ConstraintPrismaticUnaligned<S2,O2>::Vector3,
  Eigen::DefaultProduct>
#else
  const typename Eigen::ProductReturnType<
  Eigen::Block<const M6,6,3>,
  const typename ConstraintPrismaticUnaligned<S2,O2>::Vector3
  >::Type
#endif
  operator*(const Eigen::MatrixBase<M6> & Y, const ConstraintPrismaticUnaligned<S2,O2> & cpu)
  {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
    return Y.template block<6,3> (0,Inertia::LINEAR) * cpu.axis;
  }

  namespace internal
  {
    template<typename Scalar, int Options>
    struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
    { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

    template<typename Scalar, int Options, typename MotionDerived>
    struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
    { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  }

  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 1,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataDerived;
    typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintPrismaticUnaligned<Scalar,Options> Constraint_t;
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
    typedef MotionPrismaticUnalignedTpl<Scalar,Options> Motion_t;
    typedef BiasZeroTpl<Scalar,Options> Bias_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options>
  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataPrismaticUnalignedTpl
  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
    JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Transformation_t M;
    Constraint_t S;
    Motion_t v;
    Bias_t c;

    F_t F;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataPrismaticUnalignedTpl() {}

    template<typename Vector3Like>
    JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : M()
    , S(axis)
    , v(axis,(Scalar)NAN)
    , U(), Dinv(), UDinv()
    {}

  }; // struct JointDataPrismaticUnalignedTpl

  template<typename Scalar, int Options>
  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
  { typedef JointPrismaticUnalignedTpl<Scalar,Options> JointDerived; };

  JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
  template<typename _Scalar, int _Options>
  struct JointModelPrismaticUnalignedTpl
  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointPrismaticUnalignedTpl<_Scalar,_Options> JointDerived;
    SE3_JOINT_TYPEDEF_TEMPLATE;

    typedef JointModelBase<JointModelPrismaticUnalignedTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;

    JointModelPrismaticUnalignedTpl() {}
    JointModelPrismaticUnalignedTpl(Scalar x, Scalar y, Scalar z)
    : axis(x,y,z)
    {
      axis.normalize();
      assert(axis.isUnitary() && "Translation axis is not unitary");
    }

    template<typename Vector3Like>
    JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
    : axis(axis)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(axis.isUnitary() && "Translation axis is not unitary");
    }

    JointDataDerived createData() const { return JointDataDerived(axis); }

    using Base::isEqual;
    bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
    {
      return Base::isEqual(other) && axis == other.axis;
    }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar Scalar;
      const Scalar & q = qs[idx_q()];

      data.M.translation().noalias() = axis * q;
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      typedef typename TangentVector::Scalar S2;
      const S2 & v = vs[idx_v()];
      data.v.rate = v;
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
      data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    Scalar finiteDifferenceIncrement() const
    {
      using math::sqrt;
      return sqrt(Eigen::NumTraits<Scalar>::epsilon());
    }

    static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelPrismaticUnalignedTpl<NewScalar,Options> cast() const
    {
      typedef JointModelPrismaticUnalignedTpl<NewScalar,Options> ReturnType;
      ReturnType res(axis.template cast<NewScalar>());
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

    // data

    ///
    /// \brief 3d main axis of the joint.
    ///
    Vector3 axis;
  }; // struct JointModelPrismaticUnalignedTpl

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__


/*
 * This file is subject to the terms and conditions defined in
 * file 'LICENSE', which is part of this source code package.
 *
 * Partialy financiated as part of the protocol between UTAustin I Portugal - UTA-P.
 * [2017] - [2018] University of Minho, Filipe Costa Oliveira
 * All Rights Reserved.
 */

#ifndef SRC_BASECELL_H__
#define SRC_BASECELL_H__

/* BEGIN BOOST */
#include <boost/iostreams/device/mapped_file.hpp> // for mmap
#include <boost/iostreams/stream.hpp>             // for stream
#include <boost/math/constants/constants.hpp>

/* END BOOST */

#include <math.h>                        // for fabs, ceil, pow, M_PI
#include <algorithm>                     // for max_element, min_element
#include <cassert>                       // for assert
#include <cstdio>                        // for perror, size_t, NULL, SEEK_END
#include <iomanip>                       // for operator<<, setfill, setw
#include <iostream>                      // for operator<<, basic_ostream
#include <iterator>                      // for distance
#include <limits>                        // for numeric_limits
#include <string>                        // for allocator, char_traits, to_s...
#include <vector>                        // for vector, vector<>::iterator
#include <iosfwd>                    // for string

#include <QMainWindow>
#include <QtWidgets>
#include <QObject>

#include <opencv2/core/hal/interface.h>  // for CV_8UC1, CV_32F, CV_32FC1
#include <opencv2/imgproc/imgproc_c.h>   // for cvGetSpatialMoment
#include <opencv2/imgproc/types_c.h>     // for ::CV_THRESH_BINARY, CvMoments
#include <opencv2/core.hpp>              // for minMaxLoc, normalize, Exception
#include <opencv2/core/base.hpp>         // for NormTypes::NORM_MINMAX, Code...
#include <opencv2/core/cvstd.hpp>        // for Ptr
#include <opencv2/core/cvstd.inl.hpp>    // for operator<<, String::String
#include <opencv2/core/ptr.inl.hpp>      // for Ptr::operator->, Ptr::Ptr<T>
#include <opencv2/core/version.hpp>      // for CV_MAJOR_VERSION
#include <opencv2/features2d.hpp>        // for SimpleBlobDetector::Params
#include <opencv2/imgcodecs.hpp>         // for imwrite
#include <opencv2/imgproc.hpp>           // for Canny, boundingRect, drawCon...
#include <opencv2/video/tracking.hpp>    // for findTransformECC, ::MOTION_E...
#include <opencv2/core/mat.hpp>      // for Mat
#include <opencv2/core/mat.inl.hpp>  // for Mat::~Mat
#include <opencv2/core/matx.hpp>     // for Vec4d
#include <opencv2/core/types.hpp>    // for Point3d, Point, Rect, Point2d

#include <opencv2/core/mat.hpp>      // for Mat
#include <opencv2/core/mat.inl.hpp>  // for Mat::~Mat
#include <opencv2/core/matx.hpp>     // for Vec4d
#include <opencv2/core/types.hpp>    // for Point3d
#include <vector>                    // for vector
#include "atom.hpp"                  // for Atom
#include "chem_database.hpp"         // for Chem_Database
#include "string_additions.hpp"
#include "mc_driver.hpp"
#include "symbcalc.hpp"
#include "application_log.hpp"

struct cmpPoint3dOnlyByXY {
  bool operator()(const cv::Point3d& a, const cv::Point3d& b) const {
    return (( a.x < b.x ) && (a.x < b.x) );
  }
};

class BaseCell : public QObject {
  Q_OBJECT
public:

    /// Possible shapes for the unit cell
  enum CellShape {
      /// Orthorhombic cell, with the three angles equals to 90°
    ORTHORHOMBIC = 0,
      /// Triclinic cell, with any values for the angles.
    TRICLINIC = 1
  };

  BaseCell( Chem_Database *chem_db );

  //others
  void extract_space_group();
  bool clear_atom_positions();

  //setters
  bool set_cif_path( std::string path );
  bool set_cel_filename( std::string filename );
  bool set_xyz_filename( std::string filename );
  bool set_length_a_Angstroms( double a );
  bool set_length_b_Angstroms( double b );
  bool set_length_c_Angstroms( double c );
  bool set_length_a_Nanometers( double a );
  bool set_length_b_Nanometers( double b );
  bool set_length_c_Nanometers( double c );
  bool set_angle_alpha( double alpha );
  bool set_angle_beta( double beta );
  bool set_angle_gamma( double gamma );
  bool set_cell_volume_Angstroms( double volume );
  bool set_cell_volume_Nanometers( double volume );
  bool set_cel_margin_nm( double margin );
  bool set_zone_axis( cv::Point3d uvw );
  bool set_upward_vector( cv::Point3d hkl );
  bool set_upward_vector_u( double u );
  bool set_upward_vector_v( double v );
  bool set_upward_vector_w( double w );
  bool set_zone_axis_u( double u );
  bool set_zone_axis_v( double v );
  bool set_zone_axis_w( double w );
  bool set_base_bin_start_dir_path( boost::filesystem::path base_dir );
  bool set_atom_empirical_radiis_vec( int distinct_atom_pos, double radius );

  //getters
  Chem_Database* get_chem_database(){ return chem_database; }
  std::string get_cif_path();
  std::string get_cell_link(){ return cell_link; }
  std::string get_cif_path_full();
  bool get_flag_cif_path(){ return _flag_cif_path; }
  bool get_flag_cif_format(){ return _flag_cif_format; }
  bool get_flag_cel_format(){ return _flag_cel_format; }
  std::string get_cel_path(){ return cel_path; }
  double get_angle_alpha(){ return angle_alpha; }
  double get_angle_beta(){ return angle_beta; }
  double get_angle_gamma(){ return angle_gamma; }
  double get_volume_Angstroms(){ return cell_volume_Angstroms; }
  double get_volume_Nanometers(){ return cell_volume_Nanometers; }

    /** reciprocal-lattice (Miller) indices  **/
  bool get_flag_length_a(){ return _flag_length_a; }
  bool get_flag_length_b(){ return _flag_length_b; }
  bool get_flag_length_c(){ return _flag_length_c; }
  bool get_flag_length(){ return _flag_length; }

  bool get_flag_angle_alpha(){ return _flag_angle_alpha; }
  bool get_flag_angle_beta(){ return _flag_angle_beta; }
  bool get_flag_angle_gamma(){ return _flag_angle_gamma; }

  bool get_flag_upward_vector_u(){ return _flag_upward_vector_u; }
  bool get_flag_upward_vector_v(){ return _flag_upward_vector_v; }
  bool get_flag_upward_vector_w(){ return _flag_upward_vector_w; }
  bool get_flag_upward_vector(){ return _flag_upward_vector; }
  bool get_flag_zone_axis_u(){ return _flag_zone_axis_u; }
  bool get_flag_zone_axis_v(){ return _flag_zone_axis_v; }
  bool get_flag_zone_axis_w(){ return _flag_zone_axis_w; }
  bool get_flag_zone_axis(){ return _flag_zone_axis; }
  bool get_flag_atom_positions_vec(){ return _flag_atom_positions; }
  bool get_flag_orientation_matrix(){ return _flag_orientation_matrix; }
  bool get_flag_inverse_orientation_matrix(){ return _flag_inverse_orientation_matrix; }
  bool get_flag_lattice_mapping_matrix(){ return _flag_lattice_mapping_matrix_Nanometers; }
  bool get_flag_lattice_mapping_matrix_Nanometers(){ return _flag_lattice_mapping_matrix_Nanometers; }
  bool get_flag_fractional_norm(){ return _flag_fractional_norm; }
  void save_atom_positions_cols_vec_keys();

  std::vector<cv::Point3d> get_atom_positions_cols_vec_keys();

  void form_matrix_from_miller_indices();
  void form_matrix_from_lattice_parameters();
  bool set_orientation_matrix( cv::Mat );
  cv::Mat get_orientation_matrix(){ return orientation_matrix; }
  cv::Mat get_inverse_orientation_matrix(){ return inverse_orientation_matrix; }

  cv::Mat get_lattice_mapping_matrix_factors(){ return lattice_mapping_matrix_factors; }
  cv::Mat get_lattice_mapping_matrix_Nanometers(){ return lattice_mapping_matrix_Nanometers; }
  cv::Mat get_lattice_mapping_matrix_Angstroms(){ return lattice_mapping_matrix_Angstroms; }
  cv::Point3d get_lattice_mapping_dimension_vector_Nanometers();

    /** getters **/
  double get_length_a_Angstroms(){ return ( length_a_Angstroms + (2 * cel_margin_Angstroms) ); }
  double get_length_b_Angstroms(){ return ( length_b_Angstroms + (2 * cel_margin_Angstroms) ); }
  double get_length_c_Angstroms(){ return  length_c_Angstroms; }
  double get_length_a_Nanometers(){ return ( length_a_Nanometers + (2 * cel_margin_Nanometers) ); }
  double get_length_b_Nanometers(){ return ( length_b_Nanometers + (2 * cel_margin_Nanometers) ); }
  double get_length_c_Nanometers(){ return length_c_Nanometers; }
  double get_max_length_abc_Nanometers();

    /** vector t **/
    // project x axis
  cv::Point3d get_vector_t(){ return vector_t; }

    /** Zone Axis / Lattice vector **/
    // project y axis
  cv::Point3d get_zone_axis(){ return zone_axis; }
  double get_zone_axis_u(){ return  zone_axis_u; }
  double get_zone_axis_v(){ return zone_axis_v; }
  double get_zone_axis_w(){ return zone_axis_w; }

    /** Upward vector  **/
    // project z axis
  cv::Point3d get_upward_vector(){ return upward_vector; }
  double get_upward_vector_u(){ return upward_vector_u; }
  double get_upward_vector_v(){ return upward_vector_v; }
  double get_upward_vector_w(){ return upward_vector_w; }

  double get_fractional_norm_a_atom_pos_Nanometers(){ return fractional_norm_a_atom_pos; }
  double get_fractional_norm_b_atom_pos_Nanometers(){ return fractional_norm_b_atom_pos; }
  double get_fractional_norm_c_atom_pos_Nanometers(){ return fractional_norm_c_atom_pos; }

  double get_cel_margin_nm(){ return cel_margin_Nanometers; }

  std::vector< std::vector<cv::Point3d> > get_atom_positions_vec(){ return atom_positions; }
  std::map<cv::Point3d, std::vector< std::vector<cv::Point3d> > , cmpPoint3dOnlyByXY> get_atom_positions_cols_vec();
  int get_atom_positions_vec_size();
  int get_atom_fractional_cell_coordinates_vec_size();
  std::vector<std::string> get_atom_symbols_vec();
  std::vector<cv::Vec4d> get_atom_cpk_rgba_colors_vec( );
  std::vector<double> get_atom_empirical_radiis_vec();
  std::vector<double> get_atom_occupancies_vec();
  std::vector<double> get_atom_debye_waller_factors_vec();
  bool get_flag_base_bin_start_dir_path(){ return _flag_base_bin_start_dir_path; }

  int get_nx_px(){ return cel_nx_px; }
  int get_ny_px(){ return cel_ny_px; }

    /* Loggers */
  bool set_application_logger( ApplicationLog::ApplicationLog* logger );
  void print_var_state();
    // friend std::ostream& operator<< (std::ostream& stream, const SuperCell::SuperCell& image);
  virtual std::ostream& output(std::ostream& stream) const;

    public slots:

  void emit_atom_empirical_radiis_changed();

  signals:
  void start_update_atoms();
  void end_update_atoms( int n_atoms );
  void atom_positions_changed();
  void atom_empirical_radiis_changed();

  void atom_orientation_changed();
  void orientation_matrix_changed();
  void upward_vector_changed();
  void zone_axis_vector_changed();

private:
  void update_length_flag();

    /* Loggers */
  ApplicationLog::ApplicationLog* logger = nullptr;
  bool _flag_logger = false;

protected:
    // Specifies the input super-cell file containing the atomic structure data in CIF file format.
  std::string cif_path;
  bool _flag_cif_path = false;
  bool _flag_cif_format = false;

  std::string cell_link;

  std::string cel_filename;
  std::string cel_path;
  bool _flag_cel_filename = false;
  bool _flag_cel_path = false;
  bool _flag_cel_format = false;

  std::string xyz_filename;
  bool _flag_xyz_filename = false;
  std::string xyz_path;
  bool _flag_xyz_path = false;
  bool _flag_xyz_format = false;

  double length_a_Angstroms = 0.0f;
  double length_b_Angstroms = 0.0f;
  double length_c_Angstroms = 0.0f;
  double length_a_Nanometers = 0.0f;
  double length_b_Nanometers = 0.0f;
  double length_c_Nanometers = 0.0f;
  bool _flag_length_a = false;
  bool _flag_length_b = false;
  bool _flag_length_c = false;
  bool _flag_length = false;

  double cell_volume_Angstroms = 0.0f;
  double cell_volume_Nanometers = 0.0f;
  bool _flag_cell_volume =false;

  double angle_alpha = 0.0f;
  double angle_beta = 0.0f;
  double angle_gamma = 0.0f;

  bool _flag_angle_alpha = false;
  bool _flag_angle_beta = false;
  bool _flag_angle_gamma = false;

     /* Base dir path */
  boost::filesystem::path base_bin_start_dir_path;
  bool _flag_base_bin_start_dir_path = false;

    /// Cell type
  CellShape cell_shape;

    /** reciprocal-lattice (Miller) indices  **/
  cv::Point3d vector_t = cv::Point3d( 0.0f, 0.0f, 1.0f );

    /** Upward vector **/
    // projected z-axis:
  cv::Point3d upward_vector = cv::Point3d( 0.0f, 1.0f, 0.0f );
  double upward_vector_u = 0.0f;
  double upward_vector_v = 1.0f;
  double upward_vector_w = 0.0f;
  bool _flag_upward_vector_u = true;
  bool _flag_upward_vector_v = true;
  bool _flag_upward_vector_w = true;
  bool _flag_upward_vector = true;

    /** Zone Axis / Lattice vector **/
    // projected y-axis
  cv::Point3d  zone_axis = cv::Point3d( 0.0f, 0.0f, 1.0f );
  double zone_axis_u = 0.0f;
  double zone_axis_v = 0.0f;
  double zone_axis_w = 1.0f;
  bool _flag_zone_axis_u = true;
  bool _flag_zone_axis_v = true;
  bool _flag_zone_axis_w = true;
  bool _flag_zone_axis = true;

  std::map< cv::Point3d, std::vector< std::vector<cv::Point3d> > , cmpPoint3dOnlyByXY > atom_positions_cols;
  std::vector< std::vector<cv::Point3d> > atom_positions;
  std::vector<std::string> atom_symbols;
  bool _flag_atom_positions = false;
  std::vector<double> atom_debye_waller_factors;
  std::vector<double> atom_occupancies;

    /** .cel **/
  std::vector< std::vector<cv::Point3d> > atom_fractional_cell_coordinates;
  bool _flag_atom_fractional_cell_coordinates = false;

  double fractional_norm_a_atom_pos = 0.0f;
  double fractional_norm_b_atom_pos = 0.0f;
  double fractional_norm_c_atom_pos = 0.0f;
  bool _flag_fractional_norm = false;

    // this margin is used to generate .cel files
  double cel_margin_Nanometers = 0.0f;
  double cel_margin_Angstroms = 0.0f;
  bool _flag_cel_margin = false;

  double ab_margin = 0.0f;
  int cel_margin_a_px = 0.0f;
  int cel_margin_b_px = 0.0f;

  int cel_nx_px = 0;
  int cel_ny_px = 0;

    // used in the simulated super-cell. this is calculated based on _cel_nXY_px - 2 * _cel_margin_AB_px
  int cel_wout_margin_nx_px = 0;
  int cel_wout_margin_ny_px = 0;

  cv::Rect ignore_cel_margin_rectangle;

    /** Orientation **/
  cv::Mat orientation_matrix ;
  bool _flag_orientation_matrix = true;
  cv::Mat3d inverse_orientation_matrix;
  bool _flag_inverse_orientation_matrix = false;

  cv::Mat lattice_mapping_matrix_factors;
  cv::Mat lattice_mapping_matrix_Angstroms;
  cv::Mat lattice_mapping_matrix_Nanometers;
  bool _flag_lattice_mapping_matrix_Nanometers = false;

  int min_width_px = 0;
  int min_height_px = 0;
  int left_padding_w_margin_px = 0;
  int top_padding_w_margin_px = 0;
  int left_padding_px = 0;
  int top_padding_px = 0;

  int width_px = 0;
  int height_px = 0;

    /** Chem Database **/
  Chem_Database* chem_database;

  bool update_volume();
  bool update_cell_shape();
};

#endif