/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * 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 Politecnico di Milano 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 HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_MUL_MUL_CALCULATOR_HPP
#define AMA_TENSOR_MUL_MUL_CALCULATOR_HPP 1

#include <ama/tensor/iexp/iexp_calculator.hpp>
#include <ama/tensor/iexp/indices.hpp>
#include <ama/common/size_t.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/size.hpp>

/* forward declaration */
namespace ama
{
  template <typename S, size_t D, size_t CT, size_t CO> class tensor;
  namespace tensor_
  {
    template <typename LEFT, typename RIGHT> class mul_outer;
  }
}

namespace ama
{
  namespace tensor_
  {

    namespace mpl = ::boost::mpl;

    /* get the covariant and the controvariant part of product */
    template <typename LEFT, typename RIGHT>
    struct mul_index
    {
      /* the controvariant and covariant indices */
      typedef typename mpl::fold<
            typename RIGHT::controvariant_list
          , typename LEFT::controvariant_list
          , mpl::push_back<mpl::_1, mpl::_2>
          >::type controvariant_list;
      typedef typename mpl::fold<
            typename RIGHT::covariant_list
          , typename LEFT::covariant_list
          , mpl::push_back<mpl::_1, mpl::_2>
          >::type covariant_list;

      /* the dimension of controvariant and covariant components */
      typedef mpl::size<controvariant_list> controvariant_type;
      typedef mpl::size<covariant_list> covariant_type;

      /* the full index list */
      typedef typename mpl::fold<
            covariant_list
          , controvariant_list
          , mpl::push_back<mpl::_1, mpl::_2>
          >::type index_list;

      /* tensor type */
      typedef typename LEFT::value_type value_type;
      typedef typename LEFT::dimension_type dimension_type;
      typedef tensor<
            value_type
          , dimension_type::value
          , controvariant_type::value
          , covariant_type::value
          > tensor_type;
    };

    template <typename LEFT, typename RIGHT>
    struct mul_calculator:
          mpl::if_<
                has_repeated_indices<typename mul_index<LEFT, RIGHT>::index_list>
              , typename mpl::if_<
                      all_repeated_indices<typename mul_index<LEFT, RIGHT>::index_list>
                    , typename LEFT::value_type
                    , iexp_temporary<
                            typename iexp_reduce<
                                  typename mul_index<LEFT, RIGHT>::tensor_type
                                , typename mul_index<LEFT, RIGHT>::index_list
                                >::tensor_type
                          , typename iexp_reduce<
                                  typename mul_index<LEFT, RIGHT>::tensor_type
                                , typename mul_index<LEFT, RIGHT>::index_list
                                >::controvariant
                          , typename iexp_reduce<
                                  typename mul_index<LEFT, RIGHT>::tensor_type
                                , typename mul_index<LEFT, RIGHT>::index_list
                                >::covariant
                          >
                    >::type
              , mul_outer<LEFT, RIGHT>
              >
    {
      /* TODO check indices the repeated indices must be in different part */
    };

  }
}

#endif /* AMA_TENSOR_MUL_MUL_CALCULATOR_HPP */


#pragma once

#include <vector>
#include <armadillo>

#include "material.hpp"

/*!
 * \brief The PipeWall class is a container class that defines the thickness
 * and Material properties of each layer a pipe consists of.
 */
class PipeWall
{
public:
    class Layer;

    /*!
     * \brief Construct PipeWall with a given number of layers.
     * \param nWallLayers Number of layers.
     */
    explicit PipeWall(const arma::uword nWallLayers):
        m_layers(std::vector<Layer>(nWallLayers))
    {}

    /*!
     * \brief Construct PipeWall from a vector of PipeWall::Layer.
     * \param wallLayers The layers of the wall.
     */
    explicit PipeWall(const std::vector<Layer>& wallLayers):
        m_layers(wallLayers)
    {}

    //! Get layer with index i. Returns const reference to member.
    const Layer& layer(const arma::uword i) const
    {
        return m_layers.at(i);
    }

    //! Get layer with index i. Returns (non-const) reference to member.
    Layer& layer(const arma::uword i)
    {
        return m_layers.at(i);
    }

    //! Get all pipe wall layers. Returns const reference to member.
    const std::vector<Layer>& layers() const { return m_layers; }

    //! Get number of layers in the pipe wall.
    arma::uword size() const { return m_layers.size(); }

    // initialized in .cpp
    static const PipeWall defaultPipeWall; //!< Predefined pipe wall.

private:
    //! m_layers Vector of PipeWall::Layer that the wall consists of.
    std::vector<Layer> m_layers;
};

/*!
 * \brief The PipeWall::Layer class is a simple container class that defines the
 * thickness and all other material properties of a single layer of pipe wall.
 */
class PipeWall::Layer : public Material
{
public:
    /*!
     * \brief Default constructor, sets all properties to -1.
     */
    constexpr Layer():
        Material(-1, -1, -1),
        m_thickness(-1)
    {}

    /*!
     * \brief Basic constructor that requires all material properties.
     * \param thickness Layer thickness [m]
     * \param conductivity Thermal conductivity [W/(m K)]
     * \param density Density [kg/m3]
     * \param heatCapacity Heat capacity at constant pressure (\f$c_p\f$) [J/(kg K)]
     */
    constexpr Layer(
            const double thickness,
            const double conductivity,
            const double density,
            const double heatCapacity):
        Material(conductivity, density, heatCapacity),
        m_thickness(thickness)
    {}

    /*!
     * \brief Construct from thickness and Material instance.
     * \param thickness Layer thickness [m]
     * \param material Pipe wall material.
     */
    constexpr Layer(
            const double thickness,
            const Material& material):
        Material(material),
        m_thickness(thickness)
    {}

    double thickness() const { return m_thickness; } //!< Thickness getter
    double conductivity() const { return m_conductivity; } //!< Conductivity getter
    double density() const { return m_density; } //!< Density getter
    double heatCapacity() const { return m_heatCapacity; } //!< Heat capacity getter

    // setters
    double& thickness() { return m_thickness; } //!< Thickness setter
    double& conductivity() { return m_conductivity; } //!< Conductivity setter
    double& density() { return m_density; } //!< Density setter
    double& heatCapacity() { return m_heatCapacity; } //!< Heat capacity setter

private:
    double m_thickness; //!< Layer wall thickness [m]
};


#include <fstream>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <qualisys/QualisysCalib.h>

using namespace std;
using namespace ros;
using namespace Eigen;

// Support for new yaml-cpp, taken from
// https://github.com/ros-planning/moveit_setup_assistant/commit/c9238ca#diff-1
#ifdef HAVE_NEW_YAMLCPP
// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template <typename T>
static void operator>>(const YAML::Node &node, T &i)
{
  i = node.as<T>();
}
#endif

// yaml-cpp 0.5 also changed how you load the YAML document.  This
// function hides the changes.
static void loadYaml(std::istream &in_stream, YAML::Node &doc_out)
{
#ifdef HAVE_NEW_YAMLCPP
  doc_out = YAML::Load(in_stream);
#else
  YAML::Parser parser(in_stream);
  parser.GetNextDocument(doc_out);
#endif
}

namespace qualisys
{

QualisysCalib::QualisysCalib(const ros::NodeHandle& n):
  nh(n),
  enable_calibration(false),
  calib_stand_ready(false){
  return;
}

bool QualisysCalib::init() {

  // Load the name of files
  nh.param("calib_marker_pos_file", calib_marker_pos_file,
      std::string("QuadrotorCalib.yaml"));
  nh.param("zero_pose_dir", zero_pose_dir,
      std::string("calib"));

  // Load the positon of markers on the calib stand
  if(!loadCalibMarkerPos(calib_marker_pos_file, marker_pos_map)) {
    ROS_ERROR("Error loading calib marker positions from file: %s",
              calib_marker_pos_file.c_str());
    return false;
  }

  // Resize the number of markers on the stand accordingly.
  calib_ref_points.resize(marker_pos_map.size());
  calib_actual_points.resize(marker_pos_map.size());

  // Create publishers, subscribers, and services
  zero_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("zero_pose", 10);
  Subscriber calib_sub = nh.subscribe("qualisys_calib", 10,
      &QualisysCalib::calibStandCallback, this,
      ros::TransportHints().tcp().tcpNoDelay());
  Subscriber subject_sub = nh.subscribe("qualisys_subject", 10,
      &QualisysCalib::subjectCallback, this,
      ros::TransportHints().tcp().tcpNoDelay());
  ServiceServer toggle_calib_srv = nh.advertiseService("toggle_calibration",
      &QualisysCalib::toggleCalibCallback, this);

  return true;
}

bool QualisysCalib::loadZeroPoseFromFile(const std::string &filename,
    Eigen::Affine3d &zero_pose)
{
  zero_pose = Eigen::Affine3d::Identity();

  std::ifstream fin(filename.c_str());
  if(!fin.is_open())
  {
#ifdef DEBUG
    std::cerr << "Error opening file: " << filename
              << ", setting zero pose to identity" << std::endl;
#endif
    return false;
  }

  bool ret = false;

  try
  {
    YAML::Node doc;
    loadYaml(fin, doc);

    try
    {
      Eigen::Vector3d v;
      Eigen::Quaterniond q;
      doc["translation"]["x"] >> v(0);
      doc["translation"]["y"] >> v(1);
      doc["translation"]["z"] >> v(2);
      doc["rotation"]["x"] >> q.x();
      doc["rotation"]["y"] >> q.y();
      doc["rotation"]["z"] >> q.z();
      doc["rotation"]["w"] >> q.w();
      zero_pose.translate(v);
      zero_pose.rotate(q);
      ret = true;
    }
    catch(YAML::KeyNotFound &e)
    {
      ret = false;
    }
  }
  catch(YAML::ParserException &e)
  {
    ret = false;
  }
  fin.close();

  if(!ret)
  {
#if DEBUG
    std::cerr << "Error parsing calib file: " << filename
              << ", setting zero_pose to Identity" << std::endl;
#endif
  }
  return ret;
}

bool QualisysCalib::saveZeroPoseToFile(const Eigen::Affine3d &zero_pose,
    const std::string &filename)
{
  YAML::Emitter out;

  Eigen::Vector3d v(zero_pose.translation());
  Eigen::Quaterniond q(zero_pose.rotation());

  out << YAML::BeginMap;
  out << YAML::Key << "translation";
  out << YAML::Value << YAML::BeginMap << YAML::Key << "x" << YAML::Value
      << v.x() << YAML::Key << "y" << YAML::Value << v.y() << YAML::Key << "z"
      << YAML::Value << v.z() << YAML::EndMap;
  out << YAML::Key << "rotation";
  out << YAML::Value << YAML::BeginMap << YAML::Key << "x" << YAML::Value
      << q.x() << YAML::Key << "y" << YAML::Value << q.y() << YAML::Key << "z"
      << YAML::Value << q.z() << YAML::Key << "w" << YAML::Value << q.w()
      << YAML::EndMap;
  out << YAML::EndMap;

  std::ofstream fout(filename.c_str(),
                     std::ios_base::out | std::ios_base::trunc);
  if(!fout.is_open())
  {
#ifdef DEBUG
    std::cerr << "Error opening file: " << filename << std::endl;
#endif
    return false;
  }

  bool ret = true;

  fout << out.c_str() << std::endl;
  if(!fout.good())
  {
#ifdef DEBUG
    std::cerr << "Error writing to file: " << filename << std::endl;
#endif
    ret = false;
  }

  fout.close();

  return ret;
}

bool QualisysCalib::getTransform(const std::vector<Eigen::Vector3d> &reference_points,
    const std::vector<Eigen::Vector3d> &actual_points,
    Eigen::Affine3d &transform)
{
  transform = Eigen::Affine3d::Identity();

  if(reference_points.size() != actual_points.size())
  {
    return false;
  }

  // Algorithm from http://dx.doi.org/10.1016/0021-9290(94)00116-L
  const size_t num_points = reference_points.size();
  Eigen::Vector3d reference_mean = Eigen::Vector3d::Zero();
  Eigen::Vector3d actual_mean = Eigen::Vector3d::Zero();
  for(size_t i = 0; i < num_points; i++)
  {
    reference_mean += reference_points[i];
    actual_mean += actual_points[i];
  }
  reference_mean /= num_points;
  actual_mean /= num_points;

  Eigen::Matrix3d C = Eigen::Matrix3d::Zero();
  for(size_t i = 0; i < num_points; i++)
  {
    C += (actual_points[i] - actual_mean) *
         (reference_points[i] - reference_mean).transpose();
  }
  C /= num_points;

  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      C, Eigen::ComputeFullU | Eigen::ComputeFullV);

  const Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d S = Eigen::Matrix3d::Identity();
  const Eigen::Matrix3d V = svd.matrixV();
  if(U.determinant() * V.determinant() < 0)
  {
    S(2, 2) = -1;
  }

  const Eigen::Matrix3d rotation = U * S * V.transpose();

  const Eigen::Vector3d translation = actual_mean - rotation * reference_mean;

  transform.translate(translation);
  transform.rotate(rotation);

  return true;
}

bool QualisysCalib::loadCalibMarkerPos(const std::string &filename,
    std::map<std::string, Eigen::Vector3d> &marker_pos_map)
{
  std::ifstream fin(filename.c_str());
  if(!fin.is_open())
  {
#ifdef DEBUG
    std::cerr << "Error opening calib marker position file: " << filename
              << std::endl;
#endif
    return false;
  }

  bool ret = false;

  try
  {
    YAML::Node doc;
    loadYaml(fin, doc);

    try
    {
      const YAML::Node &markers = doc["markers"];

      for(unsigned int i = 0; i < markers.size(); i++)
      {
        std::string name;
        markers[i]["name"] >> name;
        const YAML::Node &pos = markers[i]["position"];
        Eigen::Vector3d position;
        pos[0] >> position.x();
        pos[1] >> position.y();
        pos[2] >> position.z();
        // Use a simple way to add a pair to the map
        marker_pos_map[name] = position;
        //marker_pos_map.insert(
        //    std::make_pair<std::string, Eigen::Vector3d>(name, position));
      }
      ret = true;
    }
    catch(YAML::KeyNotFound &e)
    {
      ret = false;
    }
  }
  catch(YAML::ParserException &e)
  {
    ret = false;
  }
  fin.close();

  if(!ret)
  {
#if DEBUG
    std::cerr << "Error parsing calib marker position file: " << filename
              << std::endl;
#endif
    marker_pos_map.clear();
  }
  return ret;
}

void QualisysCalib::calibStandCallback(
    const qualisys::Subject::ConstPtr &msg)
{
  calib_ref_points.clear();
  calib_actual_points.clear();

  std::map<std::string, Eigen::Vector3d>::iterator it;
  for(size_t i = 0; i < msg->markers.size(); i++)
  {
    if(!msg->markers[i].occluded)
    {
      it = marker_pos_map.find(msg->markers[i].name);
      if(it != marker_pos_map.end())
      {
        calib_ref_points.push_back(it->second);
        calib_actual_points.push_back(Eigen::Vector3d(
            msg->markers[i].position.x, msg->markers[i].position.y,
            msg->markers[i].position.z));
      }
      else
      {
        ROS_WARN_THROTTLE(
            1, "Marker %s is not present in calib_marker_pos_file, skipping it",
            msg->markers[i].name.c_str());
      }
    }
    else
    {
      ROS_WARN_THROTTLE(1, "Marker %s is occluded, skipping it",
                        msg->markers[i].name.c_str());
    }
  }
  if(!QualisysCalib::getTransform(calib_ref_points, calib_actual_points,
                                        calib_transform))
  {
    ROS_WARN("QualisysCalib::getTransform failed");
    calib_stand_ready = false;
  }
  else
  {
    calib_stand_ready = true;
  }
}

void QualisysCalib::subjectCallback(const qualisys::Subject::ConstPtr &msg)
{
  static bool calibrating = false;
  static double t_x, t_y, t_z;
  static double q_x, q_y, q_z, q_w;
  static unsigned int count;

  if(enable_calibration && !calibrating)
  {
    // Start calib
    t_x = t_y = t_z = 0;
    q_x = q_y = q_z = q_w = 0;
    count = 0;
    calibrating = true;
  }
  else if(!enable_calibration && calibrating)
  {
    // Stop calib
    calibrating = false;

    //vicon::SetPose pose;
    Eigen::Affine3d zero_pose;
    zero_pose.setIdentity();
    Eigen::Quaterniond q(q_w / count, q_x / count, q_y / count, q_z / count);
    q.normalize();
    Eigen::Vector3d t(t_x/count, t_y/count, t_z/count);
    zero_pose.translate(t);
    zero_pose.rotate(q);

    model_name = msg->name;
    string model_zero_pose_file = zero_pose_dir+"/"+model_name+".yaml";
    saveZeroPoseToFile(zero_pose, model_zero_pose_file);
    //pose.request.subject_name = msg->name;
    //pose.request.pose.position.x = t_x / count;
    //pose.request.pose.position.y = t_y / count;
    //pose.request.pose.position.z = t_z / count;
    //pose.request.pose.orientation.x = q.x();
    //pose.request.pose.orientation.y = q.y();
    //pose.request.pose.orientation.z = q.z();
    //pose.request.pose.orientation.w = q.w();
    //set_zero_pose_srv.call(pose);
  }

  if(calibrating)
  {
    Eigen::Affine3d zero_pose, current_pose;
    Eigen::Vector3d t(msg->position.x, msg->position.y, msg->position.z);
    Eigen::Quaterniond q(msg->orientation.w, msg->orientation.x,
                         msg->orientation.y, msg->orientation.z);
    model_name = msg->name;
    current_pose.setIdentity();
    current_pose.translate(t);
    current_pose.rotate(q);

    zero_pose = calib_transform.inverse() * current_pose;

    t = zero_pose.translation();
    q = Eigen::Quaterniond(zero_pose.rotation());

    // Accumulate (to calculate mean)
    t_x += t(0);
    t_y += t(1);
    t_z += t(2);
    q_x += q.x();
    q_y += q.y();
    q_z += q.z();
    q_w += q.w();
    count++;

    geometry_msgs::PoseStamped::Ptr zero_pose_msg(
        new geometry_msgs::PoseStamped);
    zero_pose_msg->header.stamp = msg->header.stamp;
    zero_pose_msg->header.frame_id = "/qualisys";

    zero_pose_msg->pose.position.x = t_x / count;
    zero_pose_msg->pose.position.y = t_y / count;
    zero_pose_msg->pose.position.z = t_z / count;

    q = Eigen::Quaterniond(q_w / count, q_x / count, q_y / count, q_z / count);
    q.normalize();

    zero_pose_msg->pose.orientation.x = q.x();
    zero_pose_msg->pose.orientation.y = q.y();
    zero_pose_msg->pose.orientation.z = q.z();
    zero_pose_msg->pose.orientation.w = q.w();

    zero_pose_pub.publish(zero_pose_msg);
  }
}

bool QualisysCalib::toggleCalibCallback(std_srvs::Empty::Request &req,
    std_srvs::Empty::Response &res)
{
  if(!calib_stand_ready)
  {
    ROS_ERROR("Do not have calib stand pose, cannot calibrate");
    enable_calibration = false;
    return false;
  }

  enable_calibration = !enable_calibration;
  return true;
}
}


// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// 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 ETH Zurich, Wyss Zurich, Zurich Eye 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 ETH Zurich, Wyss Zurich, Zurich Eye 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.

#pragma once

#include <functional>
#include <Eigen/Dense>
#include <ze/common/types.hpp>
#include <ze/common/transformation.hpp>

namespace ze {

//! Manifold traits define a retract (boxPlus), and local (boxMinus) operation
//! for scalars, vectors, rotations and transformations. The traits are used for
//! simplified computation of numerical derivatives to verify the analytical ones
//! and in the optimizer. These traits are inspired by GTSAM and for rotations
//! and transformation we use the same convention (updates are applied on the
//! right hand side / in the body frame).
template<typename T> struct traits;

// -----------------------------------------------------------------------------
// Manifold traits for scalars.
namespace internal {
template<typename Scalar>
struct ScalarTraits
{
  enum { dimension = 1 }; //! @todo(cfo): static constexpr int fails on GCC 4.8, works for Eigen because no derived class.
  typedef Eigen::Matrix<Scalar, 1, 1> TangentVector;
  typedef Eigen::Matrix<Scalar, 1, 1> Jacobian;

  static int getDimension(const Scalar /*v*/)
  {
    return 1;
  }

  static bool equals(Scalar v1, Scalar v2, Scalar tol = 1e-8)
  {
    return std::abs(v1 - v2) < tol;
  }

  static TangentVector local(const Scalar origin, Scalar other,
                             Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      (*H1)[0] = -1.0; // dlocal(origin, other) / dorigin
    }
    if (H2)
    {
      (*H2)[0] =  1.0; // dlocal(origin, other) / dother
    }
    TangentVector result;
    result(0) = other - origin;
    return result;
  }

  static Scalar retract(const Scalar origin, const TangentVector& v,
                        Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      (*H1)[0] = 1.0; // dretract(origin, v) / dorigin
    }
    if (H2)
    {
      (*H2)[0] = 1.0; // dretract(origin, v) / dv
    }
    return origin + v[0];
  }
};
} // namespace internal

// Define scalar traits for float and double
template<> struct traits<double> : public internal::ScalarTraits<double> {};
template<> struct traits<float> : public internal::ScalarTraits<float> {};

// -----------------------------------------------------------------------------
// Manifold traits for fixed-size Eigen matrices and vectors with double precision.
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> >
{
  //static constexpr int dimension = M * N;
  enum { dimension = M * N };
  typedef Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> Matrix;
  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;

  static int getDimension(const Matrix& /*v*/)
  {
    return M * N;
  }

  static bool equals(const Matrix& v1, const Matrix& v2, real_t tol = 1e-8)
  {
    if (v1.size() != v2.size())
    {
      return false;
    }
    return (v1 - v2).array().abs().maxCoeff() < tol;
    // TODO(cfo): Check for nan entries.
  }

  static TangentVector local(
      const Matrix& origin, Matrix other,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      *H1 = -Jacobian::Identity(); // dLocal(origin, other)/dOrigin
    }
    if (H2)
    {
      *H2 =  Jacobian::Identity(); // dLocal(origin, other)/dOther
    }
    TangentVector result;
    Eigen::Map<Matrix>(result.data()) = other - origin;
    return result;
  }

  static Matrix retract(
      const Matrix& origin, const TangentVector& v,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      *H1 = Jacobian::Identity(); // dretract(origin, v) / dorigin
    }
    if (H2)
    {
      *H2 = Jacobian::Identity(); // dretract(origin, v) / dv
    }
    return origin + Eigen::Map<const Matrix>(v.data());
  }
};

// -----------------------------------------------------------------------------
// Manifold traits for dynamic-size Eigen matrices and vectors with double precision.
namespace internal {

// traits for dynamic Eigen matrices
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct DynamicMatrixTraits {

  typedef Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> DynamicMatrix;

  enum Dimension : int { dimension = Eigen::Dynamic };

  typedef VectorX TangentVector;
  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
  typedef DynamicMatrix ManifoldType;

  static int getDimension(const DynamicMatrix& m)
  {
    return m.rows() * m.cols();
  }

  static Jacobian eye(const DynamicMatrix& m)
  {
    int dim = getDimension(m);
    return Eigen::Matrix<real_t, dimension, dimension>::Identity(dim, dim);
  }

  static TangentVector local(
      const DynamicMatrix& origin, const DynamicMatrix& other,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      *H1 = -eye(origin); // dlocal(origin, other) / dorigin
    }
    if (H2)
    {
      *H2 =  eye(origin); // dlocal(origin, other) / dother
    }
    TangentVector v(getDimension(origin));
    Eigen::Map<DynamicMatrix>(v.data(), origin.rows(), origin.cols()) = other - origin;
    return v;
  }

  static DynamicMatrix retract(
      const DynamicMatrix& origin, const TangentVector& v,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    if (H1)
    {
      *H1 = eye(origin); // dretract(origin, v) / dorigin
    }
    if (H2)
    {
      *H2 = eye(origin); // dretract(origin, v) / dv
    }
    return origin + Eigen::Map<const DynamicMatrix>(v.data(), origin.rows(), origin.cols());
  }
};

} // namespace internal

// traits for fully dynamic matrix
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic, Options, MaxRows, MaxCols> > :
    public internal::DynamicMatrixTraits<Eigen::Dynamic, Eigen::Dynamic, Options, MaxRows, MaxCols> {
};

// traits for dynamic column vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<real_t, Eigen::Dynamic, 1, Options, MaxRows, MaxCols> > :
    public internal::DynamicMatrixTraits<Eigen::Dynamic, 1, Options, MaxRows, MaxCols> {
};

// traits for dynamic row vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<real_t, 1, Eigen::Dynamic, Options, MaxRows, MaxCols> > :
    public internal::DynamicMatrixTraits<1, Eigen::Dynamic, Options, MaxRows, MaxCols> {
};

// -----------------------------------------------------------------------------
// Manifold traits for SO(3)
template<> struct traits<Quaternion>
{
  enum { dimension = 3 }; // The dimension of the manifold.

  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;

  static int getDimension(const Quaternion& /*v*/)
  {
    return 3;
  }

  static bool equals(
      const Quaternion& q1, const Quaternion& q2, real_t tol = 1e-8)
  {
    return (q1.getUnique().vector()
            - q2.getUnique().vector()).array().abs().maxCoeff() < tol;
  }

  static TangentVector local(
      const Quaternion& origin, const Quaternion& other,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    const Quaternion h = origin.inverse() * other;
    const TangentVector v = h.log();
    if(H1 || H2)
    {
      Jacobian D_v_h = logmapDerivativeSO3(v);
      if(H1)
      {
        // dlocal(origin, other) / dorigin, using that Adjoint(h.inverse()) = h.inverse()
        *H1 = - D_v_h * h.inverse().getRotationMatrix();
      }
      if(H2)
      {
        // dlocal(origin, other) / dother
        *H2 = D_v_h;
      }
    }
    return v;
  }

  static Quaternion retract(
      const Quaternion& origin, const Vector3& v,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    const Quaternion g = Quaternion::exp(v);
    const Quaternion h = origin * g;
    if (H1)
    {
      // dretract(origin, v) / dorigin
      *H1 = g.inverse().getRotationMatrix(); // Adjoint(g.inverse()) = g.inverse()
    }
    if (H2)
    {
      // dretract(origin, v) / dv
      *H2 = expmapDerivativeSO3(v);
    }
    return h;
  }
};

// -----------------------------------------------------------------------------
// Manifold traits for SE(3)
template<> struct traits<Transformation>
{
  enum { dimension = 6 }; // The dimension of the manifold.

  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;

  static int getDimension(const Transformation& /*v*/)
  {
    return 6;
  }

  static bool equals(
      const Transformation& T1, const Transformation& T2, real_t tol = 1e-8)
  {
    return (T1.getRotation().getUnique().vector()
            - T2.getRotation().getUnique().vector()).array().abs().maxCoeff() < tol
        && (T1.getPosition() - T2.getPosition()).array().abs().maxCoeff() < tol;
  }

  static TangentVector local(
      const Transformation& origin, const Transformation& other,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    const Transformation h = origin.inverse() * other;
    const TangentVector v = (Vector6() << h.getPosition(), h.getRotation().log()).finished();
    if (H1 || H2)
    {
      Matrix3 J_r_inv = logmapDerivativeSO3(v.tail<3>());
      if (H1)
      {
        // dlocal(origin, other) / dorigin
        H1->block<3,3>(0,0) = -I_3x3;
        H1->block<3,3>(0,3) = skewSymmetric(v.head<3>());
        H1->block<3,3>(3,0) = Z_3x3;
        H1->block<3,3>(3,3) = - J_r_inv * h.getRotation().inverse().getRotationMatrix();
      }
      if(H2)
      {
        // dlocal(origin, other) / dother
        H2->block<3,3>(0,0) = h.getRotationMatrix();
        H2->block<3,3>(0,3) = Z_3x3;
        H2->block<3,3>(3,0) = Z_3x3;
        H2->block<3,3>(3,3) = J_r_inv;
      }
    }
    return v;
  }

  static Transformation retract(
      const Transformation& origin, const Vector6& v,
      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
  {
    Transformation g(Quaternion::exp(v.tail<3>()), v.head<3>());
    Transformation h  = origin * g;

    if (H1 || H2)
    {
      const Matrix3 R_CB = Quaternion::exp(v.tail<3>()).inverse().getRotationMatrix();
      if (H1)
      {
        // dretract(origin, other) / dorigin

        // Remember: Computation of the translation components must be in the
        // body frame of the result.
        // Retraction: T_AC = T_AB * exp(xi_BC) = T_AB * T_BC
        // Let's look at the translation component:
        //    A_t_AC = A_t_AB + R_AB * B_t_BC
        // Perturb it the translation (in the body frame B):
        //    A_t_AC = A_t_AB + (R_AB * B_dt) + R_AB * B_t_BC
        //           = A_t_AC + (R_AB * B_dt)
        // !!! What we want is the perturbation in the body frame of the result
        // i.e. in the frame C:
        //    A_t_AC = A_t_AC + (R_AB * B_dt)
        //    A_t_AC = A_t_AC + (R_AC * R_AC^-1) * (R_AB * B_dt) <- trick
        //           = A_t_AC + R_AC * (R_CB * B_dt)
        //           -> Jacobian = R_CB
        H1->block<3,3>(0,0) = R_CB;
        H1->block<3,3>(0,3) = - R_CB * skewSymmetric(v.head<3>());
        H1->block<3,3>(3,0) = Z_3x3;
        H1->block<3,3>(3,3) = g.getRotation().inverse().getRotationMatrix();
      }
      if(H2)
      {
        H2->block<3,3>(0,0) = R_CB;
        H2->block<3,3>(0,3) = Z_3x3;
        H2->block<3,3>(3,0) = Z_3x3;
        H2->block<3,3>(3,3) = expmapDerivativeSO3(v.tail<3>());
      }
    }
    return h;
  }
};

} // namespace ze


#include "rgbd_primitives.hpp"
#define PCL_VERBOSITY_LEVEL L_ERROR

#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

// random numbers on range 0->1
boost::variate_generator<boost::mt19937, boost::uniform_01<> >
    rand01(boost::mt19937(time(0)),
              boost::uniform_01<>());

rgbd_primitives::rgbd_primitives(){

}

void set_color(pcl::PointXYZRGB &pt){
 pt.r =55.0;
 pt.g = 125.0;
 pt.b = 50.0;
}

// Duplicates function in pointcloud_math
bool mergePolygonMesh(pcl::PolygonMesh::Ptr &meshA, pcl::PolygonMesh::Ptr meshB){
  pcl::PointCloud<pcl::PointXYZRGB> cloudA;
  // HACKY BUG FIX:
  // issue: if meshA->cloud contains no data, then it contains no cloud.fields
  //        so it will complain and give a warning when we try to copy to cloudA
  //        Failed to find match for field 'x'.
  //        Failed to find match for field 'y'.
  //        Failed to find match for field 'z'.
  //        Failed to find match for field 'rgb'.
  // Instead dont try to copy if empty...
  if ( meshA->cloud.fields.size()  !=0){
    pcl::fromPCLPointCloud2(meshA->cloud, cloudA);
  }
  int original_size = cloudA.points.size() ;

  //cout << original_size << " is the cloud before (insize) size\n";
  //cout <<  meshA->polygons.size () << "polygons before\n";

  int N_polygonsB = meshB->polygons.size ();
  pcl::PointCloud<pcl::PointXYZRGB> cloudB;
  pcl::fromPCLPointCloud2(meshB->cloud, cloudB);
  Eigen::Vector4f tmp;
  for(size_t i=0; i< N_polygonsB; i++){ // each triangle/polygon
    pcl::Vertices apoly_in = meshB->polygons[i];//[i];
    int N_points = apoly_in.vertices.size ();
    for(size_t j=0; j< N_points; j++){ // each point
      // increment the vertex numbers by the size of the original clouds
      apoly_in.vertices[j] += original_size;
    }
    meshA->polygons.push_back(apoly_in);
  }
  cloudA += cloudB;
  pcl::toPCLPointCloud2 (cloudA, meshA->cloud);
  //cout <<  meshA->polygons.size () << "polygons after\n";
  //cout << cloudA.points.size() << " is the cloud inside size\n";
  return true;
}

// Duplicates function in pointcloud_math - and surely this can be done in a functioncall
Eigen::Isometry3f IsometryDoubleToFloat(Eigen::Isometry3d pose_in){

  Eigen::Quaterniond r(pose_in.rotation());
  Eigen::Quaternionf rf(r.w() , r.x() , r.y() , r.z() );

  Eigen::Isometry3f pose_out;
  pose_out.setIdentity();
  pose_out.translation()  << pose_in.translation().x() , pose_in.translation().y() , pose_in.translation().z();
  pose_out.rotate(rf);
  return pose_out;
}

pcl::PolygonMesh::Ptr rgbd_primitives::getCylinderWithTransform(Eigen::Isometry3d transform, double base, double top, double height){

  pcl::PolygonMesh::Ptr mesh_cylinder = getCylinder(base ,top , 0, height, 36,1);
  bool add_lids=true; // TODO: add as an argument
  if (add_lids){
    pcl::PolygonMesh::Ptr mesh_base_lid = getCylinder(base ,0 , 0, 0, 36,1);
    mergePolygonMesh(mesh_cylinder, mesh_base_lid);
    pcl::PolygonMesh::Ptr mesh_top_lid = getCylinder(base ,0 , height, height, 36,1);
    mergePolygonMesh(mesh_cylinder, mesh_top_lid);
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::fromPCLPointCloud2(mesh_cylinder->cloud, *cloud);

  // Adjust object to be centered on z-axis (standard used by URDF)
  pcl::transformPointCloud (*cloud, *cloud,
        Eigen::Vector3f(0,0, -height/2.0), Eigen::Quaternionf(1.0, 0.0,0.0,0.0)); // !! modifies cloud

  Eigen::Isometry3f pose_f = IsometryDoubleToFloat(transform);
  Eigen::Quaternionf quat_f(pose_f.rotation());
  pcl::transformPointCloud (*cloud, *cloud,
      pose_f.translation(), quat_f); // !! modifies cloud

  pcl::toPCLPointCloud2(*cloud, mesh_cylinder->cloud);

  return mesh_cylinder;
}

pcl::PolygonMesh::Ptr rgbd_primitives::getCubeWithTransform(Eigen::Isometry3d transform, double xdim, double ydim, double zdim){

  pcl::PolygonMesh::Ptr mesh_cylinder = getCube(xdim, ydim, zdim);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::fromPCLPointCloud2(mesh_cylinder->cloud, *cloud);

  // Adjust object to be centered on z-axis (standard used by URDF)
  pcl::transformPointCloud (*cloud, *cloud,
        Eigen::Vector3f( -xdim/2.0 , -ydim/2.0, -zdim/2.0), Eigen::Quaternionf(1.0, 0.0,0.0,0.0)); // !! modifies cloud

  Eigen::Isometry3f pose_f = IsometryDoubleToFloat(transform);
  Eigen::Quaternionf quat_f(pose_f.rotation());
  pcl::transformPointCloud (*cloud, *cloud,
      pose_f.translation(), quat_f); // !! modifies cloud

  pcl::toPCLPointCloud2(*cloud, mesh_cylinder->cloud);

  return mesh_cylinder;
}

pcl::PolygonMesh::Ptr rgbd_primitives::getSphereWithTransform(Eigen::Isometry3d transform, double radius){

  pcl::PolygonMesh::Ptr mesh_cylinder = getSphere( radius);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::fromPCLPointCloud2(mesh_cylinder->cloud, *cloud);

  Eigen::Isometry3f pose_f = IsometryDoubleToFloat(transform);
  Eigen::Quaternionf quat_f(pose_f.rotation());
  pcl::transformPointCloud (*cloud, *cloud,
      pose_f.translation(), quat_f); // !! modifies cloud

  pcl::toPCLPointCloud2(*cloud, mesh_cylinder->cloud);

  return mesh_cylinder;

}

// create a polygon mesh of a cylinder (similar to clones gluCylinder)
// This actually creates a triangle fan in a ring.
// It can be used for
// - cylinders
// - cones (if a radius is set to zero)
// - pyramids (if slices set to 4)
// - discs (if base_height and top_height are equal and a radius is zero)
// @base Specifies the radius of the cylinder at z = base_height (usually zero)
// @top Specifies the radius of the cylinder at z = top_height.
// @base_height Specifies the base height of the cylinder.
// @top_height Specifies the top height of the cylinder.
// @slices Specifies the number of subdivisions around the z axis.
// @stacks - not used
pcl::PolygonMesh::Ptr rgbd_primitives::getCylinder(double base, double top, double base_height, double top_height, int slices, int stacks){
  pcl::PolygonMesh mesh;
  pcl::PolygonMesh::Ptr mesh_ptr (new pcl::PolygonMesh (mesh));
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pts (new pcl::PointCloud<pcl::PointXYZRGB> ());
  std::vector <pcl::Vertices> verts;

  double delta = (2*M_PI/slices);
  for (double i=0, i_int=0;i< 2*M_PI; i=i+ delta, i_int++){
    pcl::PointXYZRGB pt1, pt2, pt3, pt4, pt5, pt6;
    pt1.x = base*cos(i);       pt1.y = base*sin(i);       pt1.z = base_height;
    pt2.x =  top*cos(i);       pt2.y =  top*sin(i);       pt2.z = top_height;
    pt3.x = base*cos(i+delta); pt3.y = base*sin(i+delta); pt3.z = base_height;
    set_color(pt1); set_color(pt2); set_color(pt3);
    pts->points.push_back(pt1);
    pts->points.push_back(pt2);
    pts->points.push_back(pt3);
    pcl::Vertices vert;
    vert.vertices.push_back(i_int*6 ); vert.vertices.push_back(i_int*6+1); vert.vertices.push_back(i_int*6+2);
    verts.push_back(vert);

    pt4.x = base*cos(i+delta)  ; pt4.y = base*sin(i+delta); pt4.z = base_height;
    pt5.x =  top*cos(i+delta)  ; pt5.y =  top*sin(i+delta); pt5.z = top_height;
    pt6.x =  top*cos(i)        ; pt6.y =  top*sin(i);       pt6.z = top_height;
    set_color(pt4); set_color(pt5); set_color(pt6);
    pts->points.push_back(pt4);
    pts->points.push_back(pt5);
    pts->points.push_back(pt6);
    pcl::Vertices vertB;
    vertB.vertices.push_back(i_int*6 +3); vertB.vertices.push_back(i_int*6+4); vertB.vertices.push_back(i_int*6+5);
    verts.push_back(vertB);
  }

  /*
  std::cout << "blah\n";
  std::cout << verts[0] << " 0verts\n";
  std::cout << verts[1] << " 1verts\n";
  std::cout << pts->points[0] << " pts\n";
  */
  mesh_ptr->polygons = verts;
  pcl::toPCLPointCloud2 (*pts, mesh_ptr->cloud);
  //std::cout << *mesh_ptr << "\n";
  return mesh_ptr;
}

pcl::PolygonMesh::Ptr rgbd_primitives::getCube(double xdim, double ydim, double zdim){
  pcl::PolygonMesh mesh;
  pcl::PolygonMesh::Ptr mesh_ptr (new pcl::PolygonMesh (mesh));
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pts (new pcl::PointCloud<pcl::PointXYZRGB> ());
  std::vector <pcl::Vertices> verts;

  int i_int=0;

  // Top and Bottom:
  for (double z=0 ; z <= zdim; z=z+zdim, i_int++){
    pcl::PointXYZRGB pt1, pt2, pt3, pt4, pt5, pt6;
    pt1.x = 0;       pt1.y = 0;    pt1.z = z;
    pt2.x = 0;       pt2.y = ydim; pt2.z = z;
    pt3.x = xdim;    pt3.y = ydim; pt3.z = z;
    pt4.x = xdim ; pt4.y =ydim; pt4.z = z;
    pt5.x = xdim ; pt5.y =0   ; pt5.z = z;
    pt6.x =  0   ; pt6.y =0   ; pt6.z = z;
    set_color(pt1); set_color(pt2); set_color(pt3); set_color(pt4); set_color(pt5); set_color(pt6);
    pts->points.push_back(pt1);    pts->points.push_back(pt2);
    pts->points.push_back(pt3);    pts->points.push_back(pt4);
    pts->points.push_back(pt5);    pts->points.push_back(pt6);
    pcl::Vertices vert;
    vert.vertices.push_back(i_int*6 ); vert.vertices.push_back(i_int*6+1); vert.vertices.push_back(i_int*6+2);
    verts.push_back(vert);
    pcl::Vertices vertB;
    vertB.vertices.push_back(i_int*6 +3); vertB.vertices.push_back(i_int*6+4); vertB.vertices.push_back(i_int*6+5);
    verts.push_back(vertB);
  }

  // Front and Back
  for (double x=0 ; x <= xdim; x=x+xdim, i_int++){
    pcl::PointXYZRGB pt1, pt2, pt3, pt4, pt5, pt6;
    pt1.z = 0;       pt1.y = 0   ; pt1.x = x;
    pt2.z = 0;       pt2.y = ydim; pt2.x = x;
    pt3.z = zdim;    pt3.y = ydim; pt3.x = x;
    pt4.z = zdim;    pt4.y =ydim ; pt4.x = x;
    pt5.z = zdim;    pt5.y =0    ; pt5.x = x;
    pt6.z =  0;      pt6.y =0    ; pt6.x = x;
    set_color(pt1); set_color(pt2); set_color(pt3); set_color(pt4); set_color(pt5); set_color(pt6);
    pts->points.push_back(pt1);    pts->points.push_back(pt2);
    pts->points.push_back(pt3);    pts->points.push_back(pt4);
    pts->points.push_back(pt5);    pts->points.push_back(pt6);
    pcl::Vertices vert;
    vert.vertices.push_back(i_int*6 ); vert.vertices.push_back(i_int*6+1); vert.vertices.push_back(i_int*6+2);
    verts.push_back(vert);
    pcl::Vertices vertB;
    vertB.vertices.push_back(i_int*6 +3); vertB.vertices.push_back(i_int*6+4); vertB.vertices.push_back(i_int*6+5);
    verts.push_back(vertB);
  }

  // Left and Right
  for (double y=0 ; y <= ydim; y=y+ydim, i_int++){
    pcl::PointXYZRGB pt1, pt2, pt3, pt4, pt5, pt6;
    pt1.x = 0;       pt1.z = 0  ;  pt1.y = y;
    pt2.x = 0;       pt2.z =zdim;  pt2.y = y;
    pt3.x = xdim;    pt3.z =zdim;  pt3.y = y;
    pt4.x = xdim ;   pt4.z =zdim;  pt4.y = y;
    pt5.x = xdim ;   pt5.z =0   ;  pt5.y = y;
    pt6.x =  0   ;   pt6.z =0   ;  pt6.y = y;
    set_color(pt1); set_color(pt2); set_color(pt3); set_color(pt4); set_color(pt5); set_color(pt6);
    pts->points.push_back(pt1);    pts->points.push_back(pt2);
    pts->points.push_back(pt3);    pts->points.push_back(pt4);
    pts->points.push_back(pt5);    pts->points.push_back(pt6);
    pcl::Vertices vert;
    vert.vertices.push_back(i_int*6 ); vert.vertices.push_back(i_int*6+1); vert.vertices.push_back(i_int*6+2);
    verts.push_back(vert);
    pcl::Vertices vertB;
    vertB.vertices.push_back(i_int*6 +3); vertB.vertices.push_back(i_int*6+4); vertB.vertices.push_back(i_int*6+5);
    verts.push_back(vertB);
  }

  /*
  std::cout << "blah\n";
  std::cout << verts[0] << " 0verts\n";
  std::cout << verts[1] << " 1verts\n";
  std::cout << pts->points[0] << " pts\n";
  */
  mesh_ptr->polygons = verts;
  pcl::toPCLPointCloud2 (*pts, mesh_ptr->cloud);
  //std::cout << *mesh_ptr << "\n";
  return mesh_ptr;
}

// Get a Sphere polygon mesh
// NB: uses getCylinder to get slices of the cylinder
pcl::PolygonMesh::Ptr rgbd_primitives::getSphere(double radius){
  pcl::PolygonMesh::Ptr mesh_ptr (new pcl::PolygonMesh ());

  // TODO: this would be much more efficient if the getCylinder()
  // and mergePolygonMesh methods didn't convert between PCL types many times

  // TODO: this has a fixed set of 18 stacks and doesnt use the parameter
  // I should modify this code to support that
  for (int d=-90; d <=80; d=d+10){
    double  h0 =  radius * sin( d*M_PI/180);
    double w0 = sqrt(radius*radius - h0*h0);

    double h1 =  radius * sin( (d+10)*M_PI/180);
    double w1 = sqrt(radius*radius - h1*h1);

    pcl::PolygonMesh::Ptr mesh_base_lid = getCylinder(w0 ,w1, h0, h1, 36,1);
    mergePolygonMesh(mesh_ptr, mesh_base_lid);
  }

  return mesh_ptr;
}

double areaOfTriangle(pcl::PointXYZRGB p0, pcl::PointXYZRGB p1, pcl::PointXYZRGB p2){
  // heron's formula
  double d01 = sqrt( pow(p0.x-p1.x,2) + pow(p0.y-p1.y,2) + pow(p0.z-p1.z,2) );
  double d02 = sqrt( pow(p0.x-p2.x,2) + pow(p0.y-p2.y,2) + pow(p0.z-p2.z,2) );
  double d12 = sqrt( pow(p1.x-p2.x,2) + pow(p1.y-p2.y,2) + pow(p1.z-p2.z,2) );
  double p = ( d01 + d02 + d12 ) /2.0;
  return sqrt(p*(p- d01)*(p- d02)*(p- d12));
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgbd_primitives::sampleMesh(pcl::PolygonMesh::Ptr &mesh, double pts_per_msquared){
  // this functions samples points per triangle - so will be biased against small sized of triangles
  // TODO: fix this!

  int N_polygonsB = mesh->polygons.size ();
  pcl::PointCloud<pcl::PointXYZRGB> cloudB;
  pcl::fromPCLPointCloud2(mesh->cloud, cloudB);
  Eigen::Vector4f tmp;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pts (new pcl::PointCloud<pcl::PointXYZRGB> ());
  for(size_t i=0; i< N_polygonsB; i++){ // each triangle/polygon N_polygonsB
    pcl::Vertices apoly_in = mesh->polygons[i];
    double area = areaOfTriangle(cloudB.points[ apoly_in.vertices[0] ] ,
                                          cloudB.points[ apoly_in.vertices[1] ] ,
                                          cloudB.points[ apoly_in.vertices[2] ]);

    // determine the number of points to sample on the surface:
    int n_pts = floor(area*pts_per_msquared + 0.5);// rounding at .5
    //std::cout << area << " - " << n_pts<< " | "<< (area*pts_per_msquared) << " | " << pts_per_msquared<< "\n";
    for (size_t i=0;i <n_pts; i++){
      pcl::PointXYZRGB     pt;
      pt = samplePointInTriangle(  cloudB.points[ apoly_in.vertices[0] ] ,
                                          cloudB.points[ apoly_in.vertices[1] ] ,
                                          cloudB.points[ apoly_in.vertices[2] ] );
      pts->points.push_back(pt);
    }
  }
  pts->width = pts->points.size();
  pts->height = 1;

  return pts;
}

//% from wykobi library
// sample a random point inside a triangle
pcl::PointXYZRGB  rgbd_primitives::samplePointInTriangle(pcl::PointXYZRGB p0, pcl::PointXYZRGB p1,
                                             pcl::PointXYZRGB p2){
  double a = rand01(); // 0-1
  double b = rand01(); // 0-1
  if ((a + b) > 1){
    a=1-a;
    b=1-b;
  }
  double c = (1 - a - b);

  pcl::PointXYZRGB pt;
  pt.x = p0.x * a + p1.x * b + p2.x * c ;
  pt.y = p0.y * a + p1.y * b + p2.y * c ;
  pt.z = p0.z * a + p1.z * b + p2.z * c ;
  return pt;
}