/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2012 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if !defined(TREE_LENGTH_DISTRIBUTION_HPP)
#define TREE_LENGTH_DISTRIBUTION_HPP

#if defined(_MSC_VER)
#	pragma warning(disable: 4267)	// warning about loss of data when converting size_t to int
#endif

#include <cmath>
#include "ncl/nxsdefs.h"

#include <boost/shared_ptr.hpp>
#include <boost/format.hpp>
#include "basic_cdf.hpp"
#include "basic_lot.hpp"
#include "basic_tree.hpp"
#include "probability_distribution.hpp"
#include "multivariate_probability_distribution.hpp"

namespace phycas
{

/*------------------------------------------------------------------------------------------------------------------------------------------------------------------
|	This is the joint distribution of tree length and edge lengths. The marginal distribution of tree length is Gamma(alphaT, betaT), while the marginal
|   of edge lengths conditional on tree length is Dirichlet(a_1, a_2, ..., a_m, b_1, b_2, ..., b_n) where a_1=a_2=...=a_m=alpha, b_1=b_2=...=b_n=c*alpha,
|   m is the number of external edges, and n is the number of internal edges. This prior was described in the following paper:
|
|   Rannala, Bruce, Tianqi Zhu, and Ziheng Yang. 2012. Tail paradox, partial identifiability, and influential priors in Bayesian branch length inference.
|   Mol. Biol. Evol. 29(1):325-335.
|
|   Note 1: this class follows the conventions of the paper in that the gamma distribution mean equals shape/scale, whereas everywhere else a gamma distribution
|   appears in Phycas it has the property that mean equals shape*scale.
|
|   Note 2: this class is unusual in that it represents a probability distribution but is not derived from either ProbabilityDistribution or
|   MultivariateProbabilityDistribution. This is because it is a compound distribution combining a univariate (gamma) distribution with a multivariate
|   (Dirichlet) distribution, and requires a tree as input to its GetLnPDF function.
*/
class TreeLengthDistribution   // TREE_LENGTH_DISTRIBUTION
	{
	public:
											TreeLengthDistribution();
											TreeLengthDistribution(double alphaT, double betaT, double alpha, double c);
                        					TreeLengthDistribution(const TreeLengthDistribution & other);
		virtual								~TreeLengthDistribution() {}

		virtual void						SetLot(Lot * other);
		virtual void						ResetLot();
		virtual void						SetSeed(unsigned rnseed);

        TreeLengthDistribution * 			cloneAndSetLot(Lot * other) const;
        TreeLengthDistribution * 			Clone() const;

		virtual std::string                 GetDistributionName() const;
		virtual std::string 				GetDistributionDescription() const;
		virtual std::vector<double>			Sample(unsigned num_external, unsigned num_internal);
		virtual double						GetLnPDF(TreeShPtr t) const;
		virtual double						GetRelativeLnPDF(TreeShPtr t) const;

        double                              getShape() const {return _alphaT;}
        double                              getScale() const {return _betaT;}
        double                              getExtEdgelenParam() const {return _alpha;}
        double                              getIntExtEdgelenRatio() const {return _c;}

    protected:

        void                                SetupSamplingDistributions(unsigned num_external, unsigned num_internal);

    private:

		double								_alphaT;                /**< The shape parameter of the gamma tree length distribution */
		double								_betaT;                 /**< The scale paramter of the gamma tree length distribution */
		double								_alpha;                 /**< The parameter governing the Dirichlet distribution for external edge lengths (internal edge lengths have parameter c*alpha) */
		double								_c;                     /**< The ratio of the mean internal/external edge length (normally less than 1.0) */

        CDF                                 _cdf;                   /**< Used in GetLnPDF to compute log of gamma function */
		Lot                                 _myLot;                 /**< Own random number generator */
		Lot *                               _lot;                   /**< Points to either _myLot or an external random number generator object */

        ProbDistShPtr                       _tldist;                /**< Points to a gamma distribution of tree lengths used when sampling from this distribution */
        MultivarProbDistShPtr               _eldist;                /**< Points to a Dirichlet distribution of edge length proportions used when sampling from this distribution */
        unsigned                            _num_internal_edges;    /**< The number of internal edges specified the last time Sample was called */
        unsigned                            _num_external_edges;    /**< The number of external edges specified the last time Sample was called */
	};

typedef boost::shared_ptr<TreeLengthDistribution> TreeLengthDistributionShPtr;

} // namespace phycas

#endif


#include <vector>

#include <aslam/cameras/camera-pinhole.h>
#include <aslam/common/memory.h>
#include <aslam/common/stl-helpers.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>

#include "aslam/calibration/target-observation.h"
#include "aslam/calibration/helpers.h"

namespace aslam {
namespace calibration {

// Initializes the intrinsics vector based on one views of a calibration targets.
// On success it returns true. These functions are based on functions from Lionel Heng and
// the excellent camodocal: https://github.com/hengli/camodocal.
// This algorithm can be used with high distortion lenses.
//
// C. Hughes, P. Denny, M. Glavin, and E. Jones,
// Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
// Extraction, PAMI 2010
// Find circles from rows of chessboard corners, and for each pair
// of circles, find vanishing points: v1 and v2.
// f = ||v1 - v2|| / PI;
bool initFocalLengthVanishingPoints(
    const std::vector<TargetObservation::Ptr>& observations, Eigen::VectorXd* intrinsics) {
  CHECK_NOTNULL(intrinsics);
  CHECK(!observations.empty()) << "Need at least one observation.";

  const double cu = (observations.at(0)->getImageWidth() - 1.0) / 2.0;
  const double cv = (observations.at(0)->getImageHeight() - 1.0) / 2.0;

  const size_t num_obs = observations.size();
  std::vector<double> f_guesses;

  for (size_t obs_idx = 0; obs_idx < num_obs; ++obs_idx) {
    TargetObservation::ConstPtr obs = observations[obs_idx];
    CHECK(obs);
    TargetBase::ConstPtr current_target = obs->getTarget();
    CHECK(current_target) << "The TargetObservation has no target object.";

    // We can only process complete image observations.
    if (!obs->allCornersObservered()) {
      continue;
    }

    // Try to fit circles to each row of observations.
    Aligned<std::vector, Eigen::Vector2d> center(current_target->rows());
    std::vector<double> radius(current_target->rows());

    for (size_t r = 0u; r < current_target->rows(); ++r) {
      std::vector<cv::Point2d> points_on_circle;
      for (size_t c = 0u; c < current_target->cols(); ++c) {
        const size_t corner_idx = r * current_target->cols() + c;
        Eigen::Vector2d obs_corner;
        bool success = obs->getObservedCornerById(corner_idx, &obs_corner);
        if (success == true){
          points_on_circle.emplace_back(
              obs_corner[0],
              obs_corner[1]);
        }
      }
      InitializerHelpers::fitCircle(points_on_circle, &center[r](0), &center[r](1), &radius[r]);
    }

    // Intersect all circles to find the focal length guesses.
    for (size_t j = 0u; j < current_target->rows(); ++j) {
      for (size_t k = j + 1u; k < current_target->rows(); ++k) {
        // Find the distance between pair of vanishing points which
        // correspond to intersection points of 2 circles.
        std::vector<cv::Point2d> intersection_points;
        CHECK_LT(j, center.size());
        CHECK_LT(k, center.size());
        CHECK_LT(j, radius.size());
        CHECK_LT(k, radius.size());
        InitializerHelpers::intersectCircles(center[j](0), center[j](1), radius[j],
                                             center[k](0), center[k](1), radius[k],
                                             &intersection_points);
        if (intersection_points.size() >= 2) {
          const double f_guess = cv::norm(intersection_points[0] - intersection_points[1]) / M_PI;
          f_guesses.emplace_back(f_guess);
        }
      }
    }
  }

  // Gets the median of the guesses.
  if (f_guesses.empty()) {
    return false;
  }
  const double f0 = aslam::common::median(f_guesses.begin(), f_guesses.end());

  // Sets the first intrinsics estimate.
  intrinsics->resize(aslam::PinholeCamera::parameterCount());
  (*intrinsics)(PinholeCamera::kFu) = f0;
  (*intrinsics)(PinholeCamera::kFv) = f0;
  (*intrinsics)(PinholeCamera::kCu) = cu;
  (*intrinsics)(PinholeCamera::kCv) = cv;

  return true;
}

// Initializes the intrinsics vector based on one view of a gridded calibration target.
// On success it returns true. These functions are based on functions from Lionel Heng
// and the excellent camodocal https://github.com/hengli/camodocal.
//
// Z. Zhang
// A Flexible New Technique for Camera Calibration,
// Extraction, PAMI 2000
// Intrinsics estimation with image of absolute conic;
bool initFocalLengthAbsoluteConic(
  const std::vector<TargetObservation::Ptr>& observations, Eigen::VectorXd* intrinsics) {
 CHECK_NOTNULL(intrinsics);
 CHECK(!observations.empty()) << "Need at least one observation.";

 const double cu = (observations.at(0)->getImageWidth() - 1.0) / 2.0;
 const double cv = (observations.at(0)->getImageHeight() - 1.0) / 2.0;

 const size_t num_obs = observations.size();
 cv::Mat A(num_obs * 2, 2, CV_64F);
 cv::Mat b(num_obs * 2, 1, CV_64F);

 for (size_t obs_idx = 0u; obs_idx < num_obs; ++obs_idx) {
   TargetObservation::ConstPtr obs = observations[obs_idx];
   CHECK(obs);
   TargetBase::ConstPtr current_target = obs->getTarget();
   CHECK(current_target) << "The TargetObservation has no target object.";

   // We can only process complete image observations.
   if (!obs->allCornersObservered()) {
     continue;
   }

   std::vector<cv::Point2f> image_corners(obs->numObservedCorners());
   std::vector<cv::Point2f> M(obs->numObservedCorners());

   for (size_t j = 0; j < image_corners.size(); ++j) {
     Eigen::Vector2d obs_corner;
     bool success = obs->getObservedCornerById(j, &obs_corner);
     if (success == true){
       image_corners[j] = cv::Point2f(obs_corner[0],
                                      obs_corner[1]);
       M[j] = cv::Point2f(current_target->point(j)[0],
                          current_target->point(j)[1]);
     }
   }

   cv::Mat H = cv::findHomography(M, image_corners);

   H.at<double>(0,0) -= H.at<double>(2,0) * cu;
   H.at<double>(0,1) -= H.at<double>(2,1) * cu;
   H.at<double>(0,2) -= H.at<double>(2,2) * cu;
   H.at<double>(1,0) -= H.at<double>(2,0) * cv;
   H.at<double>(1,1) -= H.at<double>(2,1) * cv;
   H.at<double>(1,2) -= H.at<double>(2,2) * cv;

   double h[3], v[3], d1[3], d2[3];
   double n[4] = {0,0,0,0};

   for (int j = 0; j < 3; ++j) {
     double t0 = H.at<double>(j,0);
     double t1 = H.at<double>(j,1);
     h[j] = t0; v[j] = t1;
     d1[j] = (t0 + t1) * 0.5;
     d2[j] = (t0 - t1) * 0.5;
     n[0] += t0 * t0; n[1] += t1 * t1;
     n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];
   }

   for (int j = 0; j < 4; ++j) {
     n[j] = 1.0 / sqrt(n[j]);
   }

   for (int j = 0; j < 3; ++j) {
     h[j] *= n[0]; v[j] *= n[1];
     d1[j] *= n[2]; d2[j] *= n[3];
   }

   A.at<double>(obs_idx * 2, 0) = h[0] * v[0];
   A.at<double>(obs_idx * 2, 1) = h[1] * v[1];
   A.at<double>(obs_idx * 2 + 1, 0) = d1[0] * d2[0];
   A.at<double>(obs_idx * 2 + 1, 1) = d1[1] * d2[1];
   b.at<double>(obs_idx * 2, 0) = -h[2] * v[2];
   b.at<double>(obs_idx * 2 + 1, 0) = -d1[2] * d2[2];
 }

 cv::Mat f(2, 1, CV_64F);
 cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);

 // Sets the first intrinsics estimate.
 intrinsics->resize(aslam::PinholeCamera::parameterCount());
 (*intrinsics)(PinholeCamera::kFu) = sqrt(fabs(1.0 / f.at<double>(0)));
 (*intrinsics)(PinholeCamera::kFv) = sqrt(fabs(1.0 / f.at<double>(1)));
 (*intrinsics)(PinholeCamera::kCu) = cu;
 (*intrinsics)(PinholeCamera::kCv) = cv;

 return true;
}

}  // namespace calibration
}  // namespace aslam


#ifndef BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_
#define BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_

#include <map>

#include <deal.II/lac/vector.h>

#include "quadrature/quadrature_types.h"
#include "utility/named_type.h"

namespace bart::quadrature::calculators {
/*! \brief Interface for a class that integrates angular flux using a quadrature set.
 *
 * The provided functions calculate the net current at a degree of freedom \f$i\f$:
 * \f[
 * \vec{J}_i = \int \hat{\Omega} \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{m = 0}^M w_m\hat{\Omega}_m\psi_{i,m}\;,
 * \f]
 * the magnitude of the current in direction \f$\hat{n}\f$:
 * \f[
 * j_{\hat{n}} = \int_{\hat{n} \cdot \hat{\Omega} \ge 0} |\hat{n} \cdot \hat{\Omega}| \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{\Omega_m \mid \hat{n} \cdot \Omega \ge 0} w_m|\hat{n} \cdot \hat{\Omega}_m|\psi_{i,m}\;,
 * \f]
 * and the integrated angular flux in direction \f$\hat{n}\f$:
 *
 * \f[
 * \phi_{\hat{n}} = \int_{\hat{n} \cdot \hat{\Omega} \ge 0} \psi(\hat{\Omega})_i d\hat{\Omega} = \sum_{\Omega_m \mid \hat{n} \cdot \Omega \ge 0} w_m\psi_{i,m}\;.
 * \f]
 */
class AngularFluxIntegratorI {
 public:
  virtual ~AngularFluxIntegratorI() = default;
  using Vector = dealii::Vector<double>;
  using VectorPtr = std::shared_ptr<dealii::Vector<double>>;
  using VectorMap = std::map<quadrature::QuadraturePointIndex, VectorPtr>;
  using DegreeOfFreedom = utility::NamedType<int, struct DegreeOfFreedomParam>;

  virtual auto NetCurrent(const VectorMap&) const -> std::vector<Vector> = 0;
  virtual auto NetCurrent(const VectorMap&, const DegreeOfFreedom) const -> Vector = 0;
  virtual auto DirectionalCurrent(const VectorMap&, const Vector normal) const -> std::vector<double> = 0;
  virtual auto DirectionalCurrent(const VectorMap&, const Vector normal, const DegreeOfFreedom) const -> double = 0;
  virtual auto DirectionalFlux(const VectorMap&, const Vector normal) const -> std::vector<double> = 0;
  virtual auto DirectionalFlux(const VectorMap&, const Vector normal, const DegreeOfFreedom) const -> double = 0;
};

} // namespace bart::quadrature::calculators

#endif //BART_SRC_QUADRATURE_CALCULATORS_ANGULAR_FLUX_INTEGRATOR_I_HPP_


#ifndef SPATIAL_DECOMPOSITION_HPP
#define SPATIAL_DECOMPOSITION_HPP

////////////////////////////////////////////////////////////////////////////////
//
// This file is a part of the CxxSDM spatial decomposition
// library. It is released under the MIT License. You should have
// received a copy of the MIT License along with CxxSDM.  If not, see
// http://www.opensource.org/licenses/mit-license.php
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// For details, see the LICENSE file
//
// (C) 2018 Jukka Saarelma
//
////////////////////////////////////////////////////////////////////////////////

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>
#include <vector>
#include <fftw3.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#ifdef _WIN32
  #define DLLEXPORT extern "C" __declspec( dllexport )
#else
  #define DLLEXPORT
#endif

#define EIGEN_USE_LAPACKE

// C interface
extern "C" {
  DLLEXPORT void* initialize(unsigned int fs, unsigned int frame_len,
                   unsigned int resp_len, float* mic_locs,
                   unsigned int num_mics);

  DLLEXPORT void destroy(void* sd_);

  DLLEXPORT void processIRs(void* sd_, float* irs, float* az_out, float* el_out);

  DLLEXPORT void synthFromLocs(float* p, float* az0, float* el0, float* az1, float* el1,
                     unsigned int resp_len, unsigned int ls_num, float* ret);
}

inline Eigen::Vector3f cart2sph(Eigen::Vector3f cart) {
  float hypotXY = hypot(cart.x(), cart.y());
  float r = hypot(hypotXY, cart.z());
  float elev = atan2(cart.z(), hypotXY)/M_PI*180.f;
  float az = atan2(cart.y(), cart.x())/M_PI*180.f;
  return Eigen::Vector3f(az, elev, r);
}

inline Eigen::Vector3f sph2cart(Eigen::Vector3f sph) {
  float z = sph.z()*sinf(sph.y()/180*M_PI);
  float el_cos = sph.z()*cosf(sph.y()/180*M_PI);
  float x = el_cos*cosf(sph.x()/180*M_PI);
  float y = el_cos*sinf(sph.x()/180*M_PI);
  return Eigen::Vector3f(x,y,z);
}

// From http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
template<typename _Matrix_Type_>
inline bool pinv(const _Matrix_Type_ &a, _Matrix_Type_ &result,
                 double epsilon = std::numeric_limits<double>::epsilon()) {
  if(a.rows() < a.cols()) {
    printf("Inverse false rows: %ld cols: %ld \n", a.rows(), a.cols());
    return false;
  }
  Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  double tolerance = epsilon*std::max(a.cols(), a.rows())*
                     svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV()*
           ((svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) ).matrix().asDiagonal()*
           svd.matrixU().adjoint();
  return true;
}

class SpatialDecomposer {
public:
  SpatialDecomposer() {};
  ~SpatialDecomposer() {};

  void initialize(unsigned int fs, unsigned int winlen, unsigned int resp_len,
                  float* mic_locs, unsigned int num_mics);

  void correlate(float* a, float* b, float* out);

  void destroy();

  void processFrame(float* frame);

  void processIRs(float** irs);

private:

  std::vector<unsigned int> comb(int N, int K) {
    std::string bitmask(K, 1); // K leading 1's
    bitmask.resize(N, 0); // N-K trailing 0's
    std::vector<unsigned int> ret;
    // print integers and permute bitmask
    do {
      for (int i = 0; i < N; ++i) { // [0..N-1] integers
          if (bitmask[i]) ret.push_back(i);
      }
    } while (std::prev_permutation(bitmask.begin(), bitmask.end()));
    return ret;
  }

  std::vector<float> hanning(unsigned int len) {
    std::vector<float> ret(len);
    for(unsigned int i = 0; i < len; i++) {
        float multiplier = 0.5f * (1.f - cosf(2.0*M_PI*(float)i/(float)len));
        ret.at(i) = multiplier;
    }
    return ret;
  }

  unsigned int maxIdx(float* data, unsigned int offset, unsigned int max_dist_i);

  //////////////////////////////////////////////////////////////////////////////
  // Interpolate by fitting a gaussian function f = a*exp(-b(x-c)**2) to three
  // values around the maximum value of the cross-correlation vector.
  //
  // Zhang, Lei, and Xiaolin Wu. "On cross correlation based-discrete time delay
  // estimation." Acoustics, Speech, and Signal Processing, 2005. Proceedings.
  // (ICASSP'05). IEEE International Conference on. Vol. 4. IEEE, 2005.
  //////////////////////////////////////////////////////////////////////////////

  float interpolateTau(float* data, unsigned int i);

// Keeping the fields public for now for debugging
public:
  fftwf_plan fft_;
  fftwf_plan ifft_;
  std::vector< fftwf_complex* > f_buffers_;
  std::vector< float* > t_buffers_;
  std::vector< float > han_window_;

  std::vector< fftwf_complex* > corr_buffers_;
  std::vector< float* > real_corr_buffers_;

  std::vector<float> ret_p_;
  std::vector<float> ret_az_;
  std::vector<float> ret_el_;

  unsigned int fs_;
  unsigned int frame_len_;
  unsigned int complex_len_;
  unsigned int transform_len_;
  unsigned int transform_len_dif_;
  unsigned int num_win_;
  unsigned int resp_len_;
  unsigned int num_mics_;
  unsigned int num_mic_pairs_;
  unsigned int max_dist_i_;
  unsigned int offset_;
  Eigen::MatrixXf mic_locs_;
  Eigen::MatrixXf ls_locs_;
  Eigen::MatrixXi mic_pairs_;
  Eigen::MatrixXf V_;
  Eigen::MatrixXf inv_V_;
  Eigen::MatrixXf pair_dists_;
  Eigen::MatrixXf tau_;
  float c_;

};

#endif


// ----------------------------------------------------------------------------
// USAGE EXAMPLES
// ----------------------------------------------------------------------------

//----------------------------------------------------------
// Discrete Conformal Map parameterization
// circle border
// OpenNL solver
// output is a eps map
// input file is mesh.off
//----------------------------------------------------------
// polyhedron_ex_parameterization -t conformal -b circle mesh.off mesh.eps

//----------------------------------------------------------
// Least Squares Conformal Maps parameterization
// two pinned vertices (automatically picked)
// OpenNL solver
// output is a .obj
// input file is mesh.off
//----------------------------------------------------------
// polyhedron_ex_parameterization -t lscm -b 2pts mesh.off mesh.obj

#include <CGAL/Timer.h>
#include <CGAL/parameterize.h>
#include <CGAL/Parameterization_mesh_patch_3.h>
#include <CGAL/Circular_border_parameterizer_3.h>
#include <CGAL/Square_border_parameterizer_3.h>
#include <CGAL/Two_vertices_parameterizer_3.h>
#include <CGAL/Barycentric_mapping_parameterizer_3.h>
#include <CGAL/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Mean_value_coordinates_parameterizer_3.h>
#include <CGAL/LSCM_parameterizer_3.h>
#include <CGAL/Parameterization_mesh_feature_extractor.h>

#include <CGAL/OpenNL/linear_solver.h>

#include "Polyhedron_ex.h"
#include "Mesh_cutter.h"
#include "Parameterization_polyhedron_adaptor_ex.h"

#include <iostream>
#include <string.h>
#include <ctype.h>
#include <fstream>
#include <cassert>

#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
    #include <boost/program_options.hpp>
    namespace po = boost::program_options;
#endif

// ----------------------------------------------------------------------------
// Private types
// ----------------------------------------------------------------------------

typedef Polyhedron_ex                                       Polyhedron;

// Mesh adaptors
typedef Parameterization_polyhedron_adaptor_ex              Parameterization_polyhedron_adaptor;
typedef CGAL::Parameterization_mesh_patch_3<Parameterization_polyhedron_adaptor>
                                                            Mesh_patch_polyhedron;

// Type describing a border or seam as a vertex list
typedef std::list<Parameterization_polyhedron_adaptor::Vertex_handle>
                                                            Seam;

// ----------------------------------------------------------------------------
// Private functions
// ----------------------------------------------------------------------------

// Cut the mesh to make it homeomorphic to a disk
// or extract a region homeomorphic to a disc.
// Return the border of this region (empty on error)
//
// CAUTION:
// This method is provided "as is". It is very buggy and simply part of this example.
// Developers using this package should implement a more robust cut algorithm!
static Seam cut_mesh(Parameterization_polyhedron_adaptor& mesh_adaptor)
{
    // Helper class to compute genus or extract borders
    typedef CGAL::Parameterization_mesh_feature_extractor<Parameterization_polyhedron_adaptor_ex>
                                            Mesh_feature_extractor;
    typedef Mesh_cutter::Backbone           Backbone;

    Seam seam;              // returned list

    // Get refererence to Polyhedron_3 mesh
    Polyhedron& mesh = mesh_adaptor.get_adapted_mesh();

    // Extract mesh borders and compute genus
    Mesh_feature_extractor feature_extractor(mesh_adaptor);
    int nb_borders = feature_extractor.get_nb_borders();
    int genus = feature_extractor.get_genus();

    // If mesh is a topological disk
    if (genus == 0 && nb_borders > 0)
    {
        // Pick the longest border
        seam = feature_extractor.get_longest_border();
    }
    else // if mesh is *not* a topological disk, create a virtual cut
    {
        Backbone seamingBackbone;           // result of cutting
        Backbone::iterator he;

        // Compute a cutting path that makes the mesh a "virtual" topological disk
        mesh.compute_facet_centers();
        Mesh_cutter cutter(mesh);
        if (genus == 0)
        {
            // no border, we need to cut the mesh
            assert (nb_borders == 0);
            cutter.cut(seamingBackbone);    // simple cut
        }
        else // genus > 0 -> cut the mesh
        {
            cutter.cut_genus(seamingBackbone);
        }

        // The Mesh_cutter class is quite buggy
        // => we check that seamingBackbone is valid
        //
        // 1) Check that seamingBackbone is not empty
        if (seamingBackbone.begin() == seamingBackbone.end())
            return seam;                    // return empty list
        //
        // 2) Check that seamingBackbone is a loop and
        //    count occurences of seam halfedges
        mesh.tag_halfedges(0);              // Reset counters
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
        {
            // Get next halfedge iterator (looping)
            Backbone::iterator next_he = he;
            next_he++;
            if (next_he == seamingBackbone.end())
                next_he = seamingBackbone.begin();

            // Check that seamingBackbone is a loop: check that
            // end of current HE == start of next one
            if ((*he)->vertex() != (*next_he)->opposite()->vertex())
                return seam;                // return empty list

            // Increment counter (in "tag" field) of seam halfedges
            (*he)->tag( (*he)->tag()+1 );
        }
        //
        // 3) check that the seamingBackbone is a two-way list
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
        {
            // Counter of halfedge and opposite halfedge must be 1
            if ((*he)->tag() != 1 || (*he)->opposite()->tag() != 1)
                return seam;                // return empty list
        }

        // Convert list of halfedges to a list of vertices
        for (he = seamingBackbone.begin(); he != seamingBackbone.end(); he++)
            seam.push_back((*he)->vertex());
    }

    return seam;
}

// Call appropriate parameterization method based on command line parameters
template<
    class ParameterizationMesh_3,   // 3D surface
    class GeneralSparseLinearAlgebraTraits_d,
                                    // Traits class to solve a general sparse linear system
    class SymmetricSparseLinearAlgebraTraits_d
                                    // Traits class to solve a symmetric sparse linear system
>
typename CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::Error_code
parameterize(ParameterizationMesh_3& mesh,  // Mesh parameterization adaptor
             const std::string& type,              // type of parameterization (see usage)
             const std::string& border)            // type of border parameterization (see usage)
{
    typename CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::Error_code err;

    if ( (type == std::string("floater"))  && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Mean_value_coordinates_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("floater")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Mean_value_coordinates_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("barycentric")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Barycentric_mapping_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_uniform_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("barycentric")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Barycentric_mapping_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_uniform_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("conformal")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_conformal_map_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("conformal")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_conformal_map_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("authalic")) && (border == std::string("circle")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_authalic_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Circular_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("authalic")) && (border == std::string("square")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::Discrete_authalic_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Square_border_arc_length_parameterizer_3<ParameterizationMesh_3>,
                GeneralSparseLinearAlgebraTraits_d
            >());
    }
    else if ( (type == std::string("lscm")) && (border == std::string("2pts")) )
    {
        err = CGAL::parameterize(
            mesh,
            CGAL::LSCM_parameterizer_3<
                ParameterizationMesh_3,
                CGAL::Two_vertices_parameterizer_3<ParameterizationMesh_3>,
                SymmetricSparseLinearAlgebraTraits_d
            >());
    }
    else
    {
        std::cerr << "Error: invalid parameters combination " << type << " + " << border << std::endl;
        err = CGAL::Parameterizer_traits_3<ParameterizationMesh_3>::ERROR_WRONG_PARAMETER;
    }

    return err;
}

// ----------------------------------------------------------------------------
// main()
// ----------------------------------------------------------------------------

#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
int main(int argc, char * argv[])
#else
int main()
#endif
{
    CGAL::Timer total_timer;
    total_timer.start();

    std::cerr << "PARAMETERIZATION" << std::endl;

    //***************************************
    // Read options on the command line
    //***************************************

    std::string type;               // default: Floater param
    std::string border;             // default: circular border param.
    std::string solver;             // default: OpenNL solver
    std::string input;              // required
    std::string output;             // default: out.eps
    try
    {
#if defined(CGAL_USE_BOOST_PROGRAM_OPTIONS) && ! defined(DONT_USE_BOOST_PROGRAM_OPTIONS)
        po::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "prints this help message")
            ("type,t", po::value<std::string>(&type)->default_value("floater"),
            "parameterization method: floater, conformal, barycentric, authalic or lscm")
            ("border,b", po::value<std::string>(&border)->default_value("circle"),
            "border shape: circle, square or 2pts (lscm only)")
            ("solver,s", po::value<std::string>(&solver)->default_value("opennl"),
            "solver: opennl")
            ("input,i", po::value<std::string>(&input)->default_value(""),
            "input mesh (OFF)")
            ("output,o", po::value<std::string>(&output)->default_value("out.eps"),
            "output file (EPS or OBJ)")
            ;

        po::positional_options_description p;
        p.add("input", 1);
        p.add("output", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << "\n";
            return 1;
        }
#else
        std::cerr << "Command-line options require Boost.ProgramOptions" << std::endl;
        std::cerr << "Use hard-coded options" << std::endl;
        border = "square";
        type = "floater";
        solver = "opennl";
        input = "data/rotor.off";
        output = "rotor_floater_square_opennl_parameterized.obj";
#endif
    }
    catch(std::exception& e) {
      std::cerr << "error: " << e.what() << "\n";
      return 1;
    }
    catch(...) {
      std::cerr << "Exception of unknown type!\n";
      throw;
    }

    //***************************************
    // Read the mesh
    //***************************************

    CGAL::Timer task_timer;
    task_timer.start();

    // Read the mesh
    std::ifstream stream(input.c_str());
    Polyhedron mesh;
    stream >> mesh;
    if(!stream || !mesh.is_valid() || mesh.empty())
    {
        std::cerr << "Error: cannot read OFF file " << input << std::endl;
        return EXIT_FAILURE;
    }

    std::cerr << "Read file " << input << ": "
              << task_timer.time() << " seconds "
              << "(" << mesh.size_of_facets() << " facets, "
              << mesh.size_of_vertices() << " vertices)" << std::endl;
    task_timer.reset();

    //***************************************
    // Create mesh adaptor
    //***************************************

    // The Surface_mesh_parameterization package needs an adaptor to handle Polyhedron_ex meshes
    Parameterization_polyhedron_adaptor mesh_adaptor(mesh);

    // The parameterization methods support only meshes that
    // are topological disks => we need to compute a cutting path
    // that makes the mesh a "virtual" topological disk
    //
    // 1) Cut the mesh
    Seam seam = cut_mesh(mesh_adaptor);
    if (seam.empty())
    {
        std::cerr << "Input mesh not supported: the example cutting algorithm is too simple to cut this shape" << std::endl;
        return EXIT_FAILURE;
    }
    //
    // 2) Create adaptor that virtually "cuts" a patch in a Polyhedron_ex mesh
    Mesh_patch_polyhedron   mesh_patch(mesh_adaptor, seam.begin(), seam.end());
    if (!mesh_patch.is_valid())
    {
        std::cerr << "Input mesh not supported: non manifold shape or invalid cutting" << std::endl;
        return EXIT_FAILURE;
    }

    std::cerr << "Mesh cutting: " << task_timer.time() << " seconds." << std::endl;
    task_timer.reset();

    //***************************************
    // switch parameterization
    //***************************************

    std::cerr << "Parameterization..." << std::endl;

    // Defines the error codes
    typedef CGAL::Parameterizer_traits_3<Mesh_patch_polyhedron> Parameterizer;
    Parameterizer::Error_code err;

    if (solver == std::string("opennl"))
    {
        err = parameterize<Mesh_patch_polyhedron,
                           OpenNL::DefaultLinearSolverTraits<double>,
                           OpenNL::SymmetricLinearSolverTraits<double>
                          >(mesh_patch, type, border);
    }
    else
    {
        std::cerr << "Error: invalid solver parameter " << solver << std::endl;
        err = Parameterizer::ERROR_WRONG_PARAMETER;
    }

    // Report errors
    switch(err) {
    case Parameterizer::OK: // Success
        break;
    case Parameterizer::ERROR_EMPTY_MESH: // Input mesh not supported
    case Parameterizer::ERROR_NON_TRIANGULAR_MESH:
    case Parameterizer::ERROR_NO_TOPOLOGICAL_DISC:
    case Parameterizer::ERROR_BORDER_TOO_SHORT:
        std::cerr << "Input mesh not supported: " << Parameterizer::get_error_message(err) << std::endl;
        return EXIT_FAILURE;
        break;
    default: // Error
        std::cerr << "Error: " << Parameterizer::get_error_message(err) << std::endl;
        return EXIT_FAILURE;
        break;
    };

    std::cerr << "Parameterization: " << task_timer.time() << " seconds." << std::endl;
    task_timer.reset();

    //***************************************
    // Output
    //***************************************

    // get output file's extension
    std::string extension = output.substr(output.find_last_of('.'));

    // Save mesh
    if (extension == ".eps" || extension == ".EPS")
    {
        // write Postscript file
        if ( ! mesh.write_file_eps(output.c_str()) )
        {
            std::cerr << "Error: cannot write file " << output << std::endl;
            return EXIT_FAILURE;
        }
    }
    else if (extension == ".obj" || extension == ".OBJ")
    {
        // write Wavefront obj file
        if ( ! mesh.write_file_obj(output.c_str()) )
        {
            std::cerr << "Error: cannot write file " << output << std::endl;
            return EXIT_FAILURE;
        }
    }
    else
    {
        std::cerr << "Error: output format not supported" << output << std::endl;
        err = Parameterizer::ERROR_WRONG_PARAMETER;
        return EXIT_FAILURE;
    }

    std::cerr << "Write file " << output << ": "
              << task_timer.time() << " seconds " << std::endl;

    return EXIT_SUCCESS;
}


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
///
/// Code used to manage 2d boxes

#include <iostream>
#include <vector>
#include <array>
#include <Eigen/Dense>
#include <random>
#include <utilities.hpp>
#include <pcg_random.hpp>
#include <exceptions.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <mutex>
#include <limits>
#include <map>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian);

namespace alma {

/// Those are helper functions that turn
/// 3d points to 2d points
///@param xyz - 3d vector to become 2d vector
///@return 2d vector discarding the z axis
inline Eigen::Vector2d xy_me(Eigen::Vector3d& xyz) {
    return xyz.block(0, 0, 2, 1);
}

/// Those are helper functions that turn
/// 3d points to 2d points
///@param xyz - matrix of 3d points to become 2d vector
///@return 2d vectors discarding the z axis
inline Eigen::MatrixXd xy_me(Eigen::MatrixXd& xyz) {
    return xyz.block(0, 0, 2, xyz.cols());
}
/// It appends a 0 to z axis to obtain 3d vector
///@param xy - 2d vector to become 3d vector
///@return 2d vector adding
inline Eigen::Vector3d upDim(Eigen::Vector2d& xy) {
    Eigen::Vector3d xyz;
    xyz << xy(0), xy(1), 0.;
    return xyz;
}

/// This contains info about sides
struct geom2d_border {
    /// Point limits of border
    Eigen::Vector2d p1;
    Eigen::Vector2d p2;
    // The vector defined
    /// as p2-p1
    Eigen::Vector2d nd;
    /// The normalized perpendicular vector
    /// pointing outside
    Eigen::Vector2d np;
    /// The segment object
    boost::geometry::model::segment<boost::tuple<double, double>> sb;
    /// The segment object little bit larger
    boost::geometry::model::segment<boost::tuple<double, double>> sbl;
    /// Properties of border:
    double Tboxeq = -1;
    int Eborder = 0;

    std::mutex door;

    geom2d_border() = default;

    geom2d_border(const geom2d_border& A)
        : p1(A.p1), p2(A.p2), nd(A.nd), np(A.np), sb(A.sb), sbl(A.sbl),
          Tboxeq(A.Tboxeq), Eborder(A.Eborder), door() {
        /// The mutex is not copyied but generated
        /// as new in the copy
    }

    geom2d_border& operator=(const geom2d_border& A) {
        p1 = A.p1;
        p2 = A.p2;
        nd = A.nd;
        np = A.np;
        sb = A.sb;
        sbl = A.sbl;
        Tboxeq = A.Tboxeq;
        Eborder = A.Eborder;
        return *this;
    }

    /// Return a random point in the border
    ///@param[in] r - random generator
    ///@return    3d vector with random position
    template <class Random> Eigen::Vector2d get_random_point(Random& r) {
        Eigen::Vector2d rp =
            this->p1 + std::uniform_real_distribution(0., 1.)(r) * this->nd;
        while (true) {
            boost::tuple<double, double> rp_(rp(0), rp(1));
            if (boost::geometry::covered_by(rp_, sb))
                break;
            rp =
                this->p1 + std::uniform_real_distribution(0., 1.)(r) * this->nd;
        }
        return rp;
    }

    /// Returns the border length in length units
    ///@return border length in length units
    double get_length() const {
        return (p2 - p1).norm();
    }
};

class geometry_2d {
private:
    /// Some typedefs to make shorter definitions
    typedef boost::tuple<double, double> point;
    typedef boost::geometry::model::polygon<point> polygon;
    typedef boost::geometry::model::segment<point> segment;

    /// Boost convex_hull
    polygon hull;

    /// Central point
    Eigen::Vector2d center;

    /// Area
    double area;
    std::vector<geom2d_border> borders;

    /// Contact map
    /// [other geoms ids,border defining contact region]
    std::map<std::size_t, std::vector<geom2d_border>> contacts;

    /// Geom id
    std::size_t id;

    /// Matrix containing bounding box
    Eigen::MatrixXd bbox;

    /// It returns the bounding box limits
    ///@param[in] p - polygon to search the bounding box from
    ///@returns   the bounding box stored as follows:
    ///           (xmin,ymin)
    ///           (xmax,ymax)
    Eigen::MatrixXd bounding_box(polygon& p);

    /// Get the sides of the figures
    ///@param[in] vertices_ :  Vector containing the vertices
    void get_sides(Eigen::MatrixXd& vertices_);

public:
    /// Public variables about info
    /// Equilibrium temperature
    double Teq = -1.;
    double Treal = -1.;
    /// Is reservoir
    bool reservoir = false;
    /// Material ID
    std::string material;
    /// If periodic
    bool periodic = false;
    /// Translation to apply
    Eigen::Vector2d translation;
    /// Box id to which translate
    std::size_t box2translate;
    /// Angle
    double theta = 0.;
    Eigen::Matrix2d rotmat;

    /// Notice that move semantics is
    /// specifically forbiden
    /// as move Constructor and
    /// move assignment operator
    /// are declared deleted

    /// Default and deleted constructors:
    geometry_2d() = default;
    geometry_2d(const geometry_2d& original) = default;
    geometry_2d(geometry_2d&& source) = delete;
    ~geometry_2d() = default;

    /// Default assignment operators:
    geometry_2d& operator=(const geometry_2d& original) = default;
    geometry_2d& operator=(geometry_2d&& source) = delete;

    /// Constructor
    ///@param[in] vertices_ - Matrix containing 2d points
    ///                       of the triangle vertex
    geometry_2d(Eigen::MatrixXd& vertices_);

    /// Other constructors are left to compiler

    /// It returns true if inside or in border of hull
    ///@param[in] point2check - 2d point to check if inside geometry
    ///@return true if point is inside geometry
    bool inside(Eigen::Vector2d& point2check) const;

    /// It returns the area
    ///@return area in (length unit)**2
    double get_area() const {
        return this->area;
    }

    /// It returns the center
    ///@return center (calculated using mean)
    Eigen::Vector2d get_center() const {
        return this->center;
    }

    /// It returns intersection with polygon sides:
    ///@param[in] r0 - original position
    ///@param[in] v  - velocity vector
    ///@param[in] dt - time step
    ///@return    [time to side collision,
    ///            collision position    ,
    ///            border ids]
    std::tuple<double, Eigen::Vector2d, std::vector<int>>
    get_inter_side(Eigen::Vector2d& r0, Eigen::Vector2d& v, double dt);

    /// Getter for geometry ID
    ///@return geometry id
    std::size_t get_id() {
        return this->id;
    }
    /// Setter for geometry ID
    ///@param[in] vale to set geometry id to
    void set_id(std::size_t id_) {
        this->id = id_;
    }

    /// Builds up contacts from other boxes
    /// Id need to be set up
    ///@param[in] system - geometry_2d of all system
    void calculate_contacts(std::vector<geometry_2d>& system);

    /// Returns borders in general
    ///@return all polygon sides
    std::vector<geom2d_border>& get_borders() {
        return this->borders;
    }

    /// Returns border
    ///@param[in] border_id - the border id
    ///@return specific side of the polygon
    geom2d_border& get_border(std::size_t border_id) {
        return this->borders[border_id];
    }

    /// Modify border Tboxeq variable
    ///@param[in] border_id - the border id
    ///@param[in] _Tboxeq - value to which variable is set
    void set_border_Tboxeq(std::size_t border_id, double _Tboxeq) {
        this->borders[border_id].Tboxeq = _Tboxeq;
    }

    /// Get border Tboxeq variable
    ///@param[in] border_id - the border id
    ///@return id-th side temperature
    double get_border_Tboxeq(std::size_t border_id) {
        return this->borders[border_id].Tboxeq;
    }

    /// Modify border Eborder variable
    ///@param[in] border_id - the border id
    ///@param[in] _Eborder - value to which variable is set
    void set_border_Eborder(std::size_t border_id, double _Eborder) {
        this->borders[border_id].door.lock();
        this->borders[border_id].Eborder = _Eborder;
        this->borders[border_id].door.unlock();
    }

    /// Modify border Eborder variable
    ///@param[in] border_id - the border id
    ///@param[in] _Eborder  - value to add to variable
    void add_border_Eborder(std::size_t border_id, double _Eborder) {
        this->borders[border_id].door.lock();
        this->borders[border_id].Eborder += _Eborder;
        this->borders[border_id].door.unlock();
    }

    /// Get border Eborder variable
    ///@param[in] border_id - the border id
    ///@return id-th boundary energy
    double get_border_Eborder(std::size_t border_id) {
        return this->borders[border_id].Eborder;
    }

    /// Returns the polygon
    ///@return polygon
    polygon& get_poly() {
        return this->hull;
    }

    /// Returns the contacts of the geometry
    ///@return [contact box id,vector(contact region)]
    std::map<std::size_t, std::vector<geom2d_border>>& get_contacts() {
        return this->contacts;
    }

    /// Returns bounding box limits
    ///@return bounding box limits ((xmin,ymin),(xmax,ymax))
    Eigen::MatrixXd get_bbox() {
        return this->bbox;
    }

    /// Get point random point inside the geometry
    ///@param[in] r - random number generator
    ///@return    random coordinates inside geometry
    template <class Random> Eigen::Vector2d get_random_point(Random& r) {
        /// Randomly generate points inside bounding box
        /// then check if inside geometry
        /// Inefficient when the ratio between bbox and
        /// geometry is large
        Eigen::Vector2d rtrial;
        while (true) {
            rtrial(0) =
                this->bbox(0, 0) + std::uniform_real_distribution(0., 1.)(r) *
                                       (this->bbox(1, 0) - this->bbox(0, 0));
            rtrial(1) =
                this->bbox(0, 1) + std::uniform_real_distribution(0., 1.)(r) *
                                       (this->bbox(1, 1) - this->bbox(0, 1));
            point rt(rtrial(0), rtrial(1));
            if (boost::geometry::covered_by(rt, this->get_poly()))
                break;
        }
        return rtrial;
    }

    /// Function for translation:
    ///@param[in] r0   - initial point to translate
    ///@param[in] v    - point direction
    ///@param[in] gs   - vector of geometries
    ///@param[in] rng  - random number generator
    ///@return [new box id,new position after translation]
    template <class Random>
    std::pair<std::size_t, Eigen::Vector2d> translate(
        Eigen::Vector2d& r0,
        Eigen::Vector2d& v,
        std::vector<geometry_2d>& gs,
        Random& rng) {
        /// Check if using it in periodic structure
        if (!gs[this->id].periodic) {
            throw alma::geometry_error("Error: this can only be used in"
                                       "periodic cells");
        }

        /// Get translation vector
        /// and box id to translate
        Eigen::Vector2d T = 1.0005 * gs[this->id].translation;
        auto Tibox = gs[this->id].box2translate;

        if (Tibox >= gs.size() or alma::almost_equal(T.norm(), 0.)) {
            std::cout << this->id << '\t' << Tibox << '\t' << T << std::endl;
            throw alma::geometry_error("Error: Bad id for mapping of"
                                       "periodic cells");
        }

        point p0(r0(0), r0(1));
        point p1(r0(0) + T(0), r0(1) + T(1));
        // Segment:
        segment s(p0, p1);

        geom2d_border sborder;
        for (auto& b : gs[Tibox].get_borders()) {
            if (boost::geometry::intersects(s, b.sbl)) {
                sborder = b;
                break;
            }
        }

        std::vector<point> rinter;
        boost::geometry::intersection(s, sborder.sbl, rinter);

        if (rinter.empty()) {
            std::cout << "r0:\n" << r0 << std::endl;
            std::cout << "v:\n" << v << std::endl;
            std::cout << "T:\n" << T << std::endl;
            std::cout << "rn:\n" << (r0 + T).eval() << std::endl;
            std::cout << "center of newbox:";
            std::cout << gs[Tibox].get_center() << std::endl;
            throw alma::geometry_error("Error in translation, "
                                       "no box found");
        }

        Eigen::Vector2d transpoint;
        Eigen::Vector2d transpoint_T;
        Eigen::Vector2d transpoint_v;
        transpoint(0) = rinter[0].get<0>();
        transpoint(1) = rinter[0].get<1>();
        transpoint_T = transpoint -
		 1.0e-6 * gs[this->id].translation /
		 (gs[this->id].translation).norm();
        transpoint_v = transpoint + 1.0e-6 * v/v.norm();
        /// Getting point
        std::vector<Eigen::Vector2d> rf;
        rinter.clear();

        std::vector<std::size_t> pboxes;
        for (auto& [ibc, border] : gs[Tibox].get_contacts()) {
	    bool here_i = boost::geometry::intersects(s,
                                 border[0].sbl);
	    bool here_T = gs[ibc].inside(transpoint);
            bool here_Tv = gs[ibc].inside(transpoint_v);
            bool here_TT = gs[ibc].inside(transpoint_T);

	    bool here = here_i or here_T or here_Tv or here_TT;

            if (ibc == Tibox or !here or
               gs[ibc].periodic or gs[ibc].reservoir)
                continue;

	    if (here_i) {
                boost::geometry::intersection(s,
                            border[0].sbl,rinter);
                Eigen::Vector2d rf_;
                rf_(0) = rinter[0].get<0>();
                rf_(1) = rinter[0].get<1>();
                rf.push_back(rf_);
                pboxes.push_back(ibc);
            }
            else if (here_T){
		rf.push_back(transpoint);
                pboxes.push_back(ibc);
	    }
	    else if (here_Tv) {
		rf.push_back(transpoint_v);
                pboxes.push_back(ibc);
	    }
            else {
		rf.push_back(transpoint_T);
                pboxes.push_back(ibc);
            }
        }
        /// Choose randomly if we are in corner
        Eigen::Vector2d RF;

        std::size_t newibox;
        if (pboxes.size() > 1) {
            std::vector<std::size_t> pboxes2;

            for (std::size_t ib = 0; ib < pboxes.size(); ib++) {
                Eigen::Vector2d rcheck = rf[ib] + 1.0e-6 * v;
                if (gs[pboxes[ib]].inside(rcheck))
                    pboxes2.push_back(pboxes[ib]);
            }

            auto pos = alma::choose(pboxes2.begin(), pboxes2.end(), rng) -
                       pboxes2.begin();

            RF = rf[pos];
            newibox = pboxes2[pos];
        }
        else if (pboxes.size() == 1) {
            newibox = pboxes[0];
            RF = rf[0];
        }
        else {
            throw alma::geometry_error("Error in Translation");
        }
        return std::make_pair(newibox,RF);
    }
};

/// Helper function to add id to geometries contained in vector
///@param[in] gs vector containing the geometries
inline void assign_geom_ids(std::vector<geometry_2d>& gs) {
    for (std::size_t i = 0; i < gs.size(); i++) {
        gs[i].set_id(i);
    }
}

/// Check if point is in corner shared by 3 polygons or more
/// it also gives the id of those figures
std::pair<bool, std::vector<std::size_t>> in_corner3(
    Eigen::Vector2d& r,
    std::vector<geometry_2d>& gs);

/// Solving intersection in three shared corner
/// We are not making contact with
/// void through single points
template <class Random>
/// It returns intersection with polygon sides:
///@param[in] rf  - position to correct
///@param[in] v   - velocity vector
///@param[in] sys - system geometry
///@param[in] candidates - candidates to where particle can be put
///@param[in] r   - random generator
///@return    [box id,
///            border ids]
std::pair<std::size_t, std::size_t> correct_corner_problem(
    Eigen::Vector2d& rf,
    Eigen::Vector2d& v,
    std::vector<geometry_2d>& sys,
    std::vector<std::size_t>& candidates,
    Random& r) {
    std::map<std::size_t, std::vector<std::size_t>> bids;

    std::vector<std::size_t> finalist;

    boost::tuple<double, double> rc(rf(0), rf(1));

    for (auto candidate : candidates) {
        bids[candidate] = std::vector<std::size_t>(0);
        std::size_t border_id = 0;
        for (auto& b : sys[candidate].get_borders()) {
            /// To rule out already there stuff
            if (boost::geometry::intersects(rc, b.sbl)) {
                if (b.np.dot(v) > 0.)
                    bids[candidate].push_back(border_id);
            }
            border_id++;
        }

        /// In triangles facing exterior it can only
        /// exist single solution
        if (bids[candidate].size() > 1) {
            bids.erase(candidate);
        }
        else {
            finalist.push_back(candidate);
        }
    }

    std::size_t winner = *(alma::choose(finalist.begin(), finalist.end(), r));

    return std::make_pair(winner, bids[winner][0]);
}

/// Returns geometry_2d vector containing all vectors
///@param[in] xmlfname - xml filename
///@return vector containing all geometric data for each box
std::vector<alma::geometry_2d> read_geometry_XML(std::string xmlfname);

/// Calculates the gradient for the box using the contact boxes
///@param[in] sys - system
///@return thermal gradient
Eigen::MatrixXd calculate_gradientT(std::vector<alma::geometry_2d>& sys);

}; // namespace alma


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file scalar_matrix.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>
#include <fl/util/types.hpp>

namespace fl
{

/**
 * \ingroup types
 *
 * Represents a matrix of the size 1x1. This Matrix can be used in the same way
 * as a scalar.
 */
class ScalarMatrix
    : public Eigen::Matrix<Real, 1, 1>
{
public:
    /**
     * \brief Default constructor.
     * Creates ScalarMatrix with no arguments. The initial value is zero by
     * default.
     */
    ScalarMatrix()
    {
        this->data()[0] = Real(0);
    }

    /**
     * \brief Creates a ScalarMatrix with an initial value or converts a scalar
     * into a ScalarMatrix.
     * \param value initial value
     */
    ScalarMatrix(Real value)
    {
        this->data()[0] = value;
    }

    /**
     * \brief Constructor copying the value of the ScalarMatrix \a other
     */
    ScalarMatrix(const ScalarMatrix& other)
    {
        this->data()[0] = other.data()[0];
    }

    /**
     * \brief Constructor copying the value of the expression \a other
     */
    template <typename OtherDerived>
    ScalarMatrix(const Eigen::MatrixBase<OtherDerived> &other)
    {
        this->data()[0] = other(0);
    }

    /**
     * \brief operator Real() implicit typecast conversion of a ScalarMatrix
     * into a scalar
     */
    operator Real() const
    {
        return Real(this->data()[0]);
    }

    /**
     * \brief operator+= adds a scalar value to this matrix and assigns
     */
    void operator+=(Real value)
    {
        this->data()[0] += value;
    }

    /**
     * \brief operator-= subtracts a scalar value to this matrix and assigns
     */
    void operator-=(Real value)
    {
        this->data()[0] -= value;
    }

    /**
     * \brief operator*= multiplies a scalar value with this this matrix and
     * assigns
     */
    void operator*=(Real value)
    {
        this->data()[0] *= value;
    }

    /**
     * \brief operator/= divide a scalar value with this this matrix and
     * assigns
     */
    void operator/=(Real value)
    {
        this->data()[0] /= value;
    }

    /**
     * \brief prefix operator ++ which increments the matrix value by 1.
     */
    ScalarMatrix& operator++()
    {
        return (++this->data()[0], *this);
    }

    /**
     * \brief postfix operator ++ which increments the matrix value by 1.
     */
    ScalarMatrix operator++(int)
    {
        auto r = ScalarMatrix(this->data()[0]++); // RVO
        return r;
    }

    /**
     * \brief prefix operator -- which decrements the matrix value by 1.
     */
    ScalarMatrix& operator--()
    {
        return (--this->data()[0], *this);
    }

    /**
     * \brief postfix operator -- which decrements the matrix value by 1.
     */
    ScalarMatrix operator--(int)
    {
        auto r = ScalarMatrix(this->data()[0]--); // RVO
        return r;
    }
};

}


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

#ifndef RW_MATH_ROTATION3D_HPP
#define RW_MATH_ROTATION3D_HPP

/**
 * @file Rotation3D.hpp
 */
#if !defined(SWIG)
#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>
#include <limits>
#endif

namespace rw { namespace math {

    template< class T > class Rotation3DVector;
    /** @addtogroup math */
    /* @{*/

#if !defined(SWIGJAVA)
    /**
     * @brief A 3x3 rotation matrix \f$ \mathbf{R}\in SO(3) \f$
     *
     * @f$
     *  \mathbf{R}=
     *  \left[
     *  \begin{array}{ccc}
     *  {}^A\hat{X}_B & {}^A\hat{Y}_B & {}^A\hat{Z}_B
     *  \end{array}
     *  \right]
     *  =
     *  \left[
     *  \begin{array}{ccc}
     *  r_{11} & r_{12} & r_{13} \\
     *  r_{21} & r_{22} & r_{23} \\
     *  r_{31} & r_{32} & r_{33}
     *  \end{array}
     *  \right]
     * @f$
     */

#endif
    template< class T = double > class Rotation3D
    {
      public:
        //! Value type.
        typedef T value_type;

        //! @brief The type of the internal Eigen matrix implementation.
        typedef Eigen::Matrix< T, 3, 3 > EigenMatrix3x3;

        /**
           @brief A rotation matrix with uninitialized storage.
         */
        Rotation3D ()
        {
            _m (0, 0) = 1;
            _m (0, 1) = 0;
            _m (0, 2) = 0;
            _m (1, 0) = 0;
            _m (1, 1) = 1;
            _m (1, 2) = 0;
            _m (2, 0) = 0;
            _m (2, 1) = 0;
            _m (2, 2) = 1;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         *
         * @param r11 \f$ r_{11} \f$
         * @param r12 \f$ r_{12} \f$
         * @param r13 \f$ r_{13} \f$
         * @param r21 \f$ r_{21} \f$
         * @param r22 \f$ r_{22} \f$
         * @param r23 \f$ r_{23} \f$
         * @param r31 \f$ r_{31} \f$
         * @param r32 \f$ r_{32} \f$
         * @param r33 \f$ r_{33} \f$
         *
         * @f$
         *  \mathbf{R} =
         *  \left[
         *  \begin{array}{ccc}
         *  r_{11} & r_{12} & r_{13} \\
         *  r_{21} & r_{22} & r_{23} \\
         *  r_{31} & r_{32} & r_{33}
         *  \end{array}
         *  \right]
         * @f$
         */

#endif
        Rotation3D (T r11, T r12, T r13, T r21, T r22, T r23, T r31, T r32, T r33)
        {
            _m (0, 0) = r11;
            _m (0, 1) = r12;
            _m (0, 2) = r13;
            _m (1, 0) = r21;
            _m (1, 1) = r22;
            _m (1, 2) = r23;
            _m (2, 0) = r31;
            _m (2, 1) = r32;
            _m (2, 2) = r33;
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Constructs an initialized 3x3 rotation matrix
         * @f$ \robabx{a}{b}{\mathbf{R}} =
         * \left[
         *  \begin{array}{ccc}
         *   \robabx{a}{b}{\mathbf{i}} & \robabx{a}{b}{\mathbf{j}} & \robabx{a}{b}{\mathbf{k}}
         *  \end{array}
         * \right]
         * @f$
         *
         * @param i @f$ \robabx{a}{b}{\mathbf{i}} @f$
         * @param j @f$ \robabx{a}{b}{\mathbf{j}} @f$
         * @param k @f$ \robabx{a}{b}{\mathbf{k}} @f$
         */

#endif
        Rotation3D (const rw::math::Vector3D< T >& i, const rw::math::Vector3D< T >& j,
                    const rw::math::Vector3D< T >& k)
        {
            _m (0, 0) = i[0];
            _m (0, 1) = j[0];
            _m (0, 2) = k[0];
            _m (1, 0) = i[1];
            _m (1, 1) = j[1];
            _m (1, 2) = k[1];
            _m (2, 0) = i[2];
            _m (2, 1) = j[2];
            _m (2, 2) = k[2];
        }

#if !defined(SWIGJAVA)
        /**
         * @brief Initialize Rotation3D from other rotation types
         * @param rotVec [in] rotation type such as \b EAA, \b RPY, or \b Quaternion
         */
        explicit Rotation3D (const Rotation3DVector< T >& rotVec);

        /**
         * @brief Constructs a 3x3 rotation matrix set to identity
         * @return a 3x3 identity rotation matrix
         *
         * @f$
         * \mathbf{R} =
         * \left[
         * \begin{array}{ccc}
         * 1 & 0 & 0 \\
         * 0 & 1 & 0 \\
         * 0 & 0 & 1
         * \end{array}
         * \right]
         * @f$
         */

#endif
        static const Rotation3D& identity ();

        /**
         * @brief Normalizes the rotation matrix to satisfy SO(3).
         *
         * Makes a normalization of the rotation matrix such that the columns
         * are normalized and othogonal s.t. it belongs to SO(3).
         */
        void normalize ();
#if !defined(SWIG)
        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        inline T& operator() (size_t row, size_t column) { return _m (row, column); }

        /**
         * @brief Returns reference to matrix element
         * @param row [in] row
         * @param column [in] column
         * @return reference to the element
         */
        inline const T& operator() (size_t row, size_t column) const { return _m (row, column); }
#else
        MATRIXOPERATOR (T);
#endif
        /**
         * @brief Returns the i'th row of the rotation matrix
         * @param i [in] Index of the row to return. Only valid indices are 0, 1 and 2.
         */
        const rw::math::Vector3D< T > getRow (size_t i) const
        {
            RW_ASSERT (i < 3);
            return rw::math::Vector3D< T > (_m (i, 0), _m (i, 1), _m (i, 2));
        }

        /**
         * @brief Returns the i'th column of the rotation matrix
         * @param i [in] Index of the column to return. Only valid indices are 0, 1 and 2.
         */
        const rw::math::Vector3D< T > getCol (size_t i) const
        {
            RW_ASSERT (i < 3);
            return rw::math::Vector3D< T > (_m (0, i), _m (1, i), _m (2, i));
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true only if all elements are equal.
         *
         * @param rhs [in] Rotation to compare with
         * @return True if equal.
         */
        bool operator== (const Rotation3D< T >& rhs) const
        {
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if (!(_m (i, j) == rhs (i, j)))
                        return false;
            return true;
        }

        /**
         * @brief Comparison operator.
         *
         * The comparison operator makes a element wise comparison.
         * Returns true if any of the elements are different.
         *
         * @param rhs [in] Rotation to compare with
         * @return True if not equal.
         */
        bool operator!= (const Rotation3D< T >& rhs) const { return !(*this == rhs); }

        /**
         * @brief Compares rotations with a given precision
         *
         * Performs an element wise comparison. Two elements are considered equal if the difference
         * are less than \b precision.
         *
         * @param rot [in] Rotation to compare with
         * @param precision [in] The precision to use for testing
         * @return True if all elements are less than \b precision apart.
         */
        bool equal (const Rotation3D< T >& rot,
                    const T precision = std::numeric_limits< T >::epsilon ()) const
        {
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if (fabs (_m (i, j) - rot (i, j)) > precision)
                        return false;
            return true;
        }

        /**
         * @brief Verify that this rotation is a proper rotation
         *
         * @return True if this rotation is considered a proper rotation
         */
        bool isProperRotation () const;

        /**
         * @brief Verify that this rotation is a proper rotation
         *
         * @return True if this rotation is considered a proper rotation
         */
        bool isProperRotation (T precision) const;

        /**
         * @brief Returns a Eigen 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        const EigenMatrix3x3& e () const { return _m; };

        /**
         * @brief Returns a Eigen 3x3 matrix @f$ \mathbf{M}\in SO(3)
         * @f$ that represents this rotation
         *
         * @return @f$ \mathbf{M}\in SO(3) @f$
         */
        EigenMatrix3x3& e () { return _m; };

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        inline const Rotation3D operator* (const Rotation3D& bRc) const
        {
            return multiply (*this, bRc);
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param rhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        template< class R >
        inline const Rotation3D operator* (const Eigen::MatrixBase< R >& rhs) const
        {
            return Rotation3D< T > (this->e () * rhs);
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param lhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @param rhs [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        template< class R >
        friend inline Rotation3D operator* (const Eigen::MatrixBase< R >& lhs,
                                            const Rotation3D< T >& rhs)
        {
            return Rotation3D< T > (lhs * rhs.e ());
        }

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        inline const rw::math::Vector3D< T > operator* (const rw::math::Vector3D< T >& bVc) const
        {
            return multiply (*this, bVc);
        }

        /**
         * @brief Construct a rotation matrix from a 3x3 Eigen matrix
         * It is the responsibility of the user that 3x3 matrix is indeed a
           rotation matrix.
         */
        template< class R > explicit Rotation3D (const EigenMatrix3x3& r)
        {
            _m (0, 0) = r (0, 0);
            _m (0, 1) = r (0, 1);
            _m (0, 2) = r (0, 2);
            _m (1, 0) = r (1, 0);
            _m (1, 1) = r (1, 1);
            _m (1, 2) = r (1, 2);
            _m (2, 0) = r (2, 0);
            _m (2, 1) = r (2, 1);
            _m (2, 2) = r (2, 2);
        }

        /**
         * @brief Construct a rotation matrix from a 3x3 Eigen matrix
         * It is the responsibility of the user that 3x3 matrix is indeed a
           rotation matrix.
        */
        template< class R > explicit Rotation3D (const Eigen::MatrixBase< R >& m)
        {
            RW_ASSERT (m.cols () == 3);
            RW_ASSERT (m.rows () == 3);
            _m (0, 0) = T (m.row (0) (0));
            _m (0, 1) = T (m.row (0) (1));
            _m (0, 2) = T (m.row (0) (2));
            _m (1, 0) = T (m.row (1) (0));
            _m (1, 1) = T (m.row (1) (1));
            _m (1, 2) = T (m.row (1) (2));
            _m (2, 0) = T (m.row (2) (0));
            _m (2, 1) = T (m.row (2) (1));
            _m (2, 2) = T (m.row (2) (2));
        }

        /**
         * @brief Creates a skew symmetric matrix from a Vector3D. Also
         * known as the cross product matrix of v.
         *
         * @relates Rotation3D
         *
         * @param v [in] vector to create Skew matrix from
         */
        static Rotation3D< T > skew (const rw::math::Vector3D< T >& v)
        {
            return Rotation3D< T > (0, -v (2), v (1), v (2), 0, -v (0), -v (1), v (0), 0);
        }

        // Faster-than-boost matrix multiplications below.

        /**
         *  @brief Write to \b result the product \b a * \b b.
         */
        static void multiply (const Rotation3D< T >& a, const Rotation3D< T >& b,
                              Rotation3D< T >& result);

        /**
         *  @brief Write to \b result the product \b a * \b b.
         */
        static void multiply (const Rotation3D< T >& a, const rw::math::Vector3D< T >& b,
                              rw::math::Vector3D< T >& result);

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{R}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         *
         * @param bRc [in] \f$ \robabx{b}{c}{\mathbf{R}} \f$
         *
         * @return \f$ \robabx{a}{c}{\mathbf{R}} \f$
         */
        static const Rotation3D< T > multiply (const Rotation3D< T >& aRb,
                                               const Rotation3D< T >& bRc);

        /**
         * @brief Calculates \f$ \robabx{a}{c}{\mathbf{v}} =
         * \robabx{a}{b}{\mathbf{R}} \robabx{b}{c}{\mathbf{v}} \f$
         *
         * @param aRb [in] \f$ \robabx{a}{b}{\mathbf{R}} \f$
         * @param bVc [in] \f$ \robabx{b}{c}{\mathbf{v}} \f$
         * @return \f$ \robabx{a}{c}{\mathbf{v}} \f$
         */
        static const rw::math::Vector3D< T > multiply (const Rotation3D< T >& aRb,
                                                       const rw::math::Vector3D< T >& bVc);
#if !defined(SWIGJAVA)
        /**
         * @brief Calculate the inverse.
         * @note This function changes the object that it is invoked on, but this is about x5 faster
         * than rot = inverse( rot )
         * @see rw::math::inverse(const rw::math::Rotation3D< T > &) for the (slower) version that does not change the
         * rotation object itself.
         * @return the inverse rotation.
         */
         #endif
        inline Rotation3D< T >& inverse ()
        {
            T tmpVal  = _m (0, 1);
            _m (0, 1) = _m (1, 0);
            _m (1, 0) = tmpVal;

            tmpVal    = _m (0, 2);
            _m (0, 2) = _m (2, 0);
            _m (2, 0) = tmpVal;

            tmpVal    = _m (1, 2);
            _m (1, 2) = _m (2, 1);
            _m (2, 1) = tmpVal;
            return *this;
        }

        /**
         * @brief Calculate the inverse.
         * @param copy [in] if coopy is false, This function changes the object that it is invoked
         * on, but this is about x5 faster than rot = inverse( rot ). else it changes the object,
         * making it a bit slower.
         * @return the inverse rotation.
         */
        Rotation3D< T > inverse (bool copy);

        /**
         * @brief Calculate the inverse. of a const Rotation3D. For this function copy is always
         * true
         * @param copy [in] always true
         * @return the inverse rotation.
         */
        Rotation3D< T > inverse (bool copy) const;

        T tr () const { return (*this) (0, 0) + (*this) (1, 1) + (*this) (2, 2); }

#if defined(SWIG)
        TOSTRING ();
#endif

      private:
        EigenMatrix3x3 _m;
    };

    /**
     * @brief Casts Rotation3D<T> to Rotation3D<Q>
     *
     * @relates Rotation3D
     *
     * @param rot [in] Rotation3D with type T
     * @return Rotation3D with type Q
     */
    template< class Q, class T > const Rotation3D< Q > cast (const Rotation3D< T >& rot)
    {
        Rotation3D< Q > res;
        for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
                res (i, j) = static_cast< Q > (rot (i, j));
        return res;
    }

#if !defined(SWIGJAVA)
    /**
     * @brief Calculates the inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$ of a rotation matrix
     *
     * @relates Rotation3D
     *
     * @see Rotation3D::inverse() for a faster version that modifies the existing rotation object
     * instead of allocating a new one.
     *
     * @param aRb [in] the rotation matrix @f$ \robabx{a}{b}{\mathbf{R}} @f$
     *
     * @return the matrix inverse @f$ \robabx{b}{a}{\mathbf{R}} =
     * \robabx{a}{b}{\mathbf{R}}^{-1} @f$
     *
     * @f$ \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} =
     * \robabx{a}{b}{\mathbf{R}}^T @f$
     */

#endif
    template< class T > const rw::math::Rotation3D< T > inverse (const rw::math::Rotation3D< T >& aRb)
    {
        return Rotation3D< T > (aRb (0, 0),
                                aRb (1, 0),
                                aRb (2, 0),

                                aRb (0, 1),
                                aRb (1, 1),
                                aRb (2, 1),

                                aRb (0, 2),
                                aRb (1, 2),
                                aRb (2, 2));
    }

    /**
     * @brief Writes rotation matrix to stream
     *
     * @relates Rotation3D
     *
     * @param os [in/out] output stream to use
     * @param r [in] rotation matrix to print
     * @return the updated output stream
     */
    template< class T > std::ostream& operator<< (std::ostream& os, const Rotation3D< T >& r)
    {
        return os << "Rotation3D(" << r (0, 0) << ", " << r (0, 1) << ", " << r (0, 2) << ", "
                  << r (1, 0) << ", " << r (1, 1) << ", " << r (1, 2) << ", " << r (2, 0) << ", "
                  << r (2, 1) << ", " << r (2, 2) << ")";
    }
#if !defined(SWIG)
    // Explicit template specifications.
    extern template class rw::math::Rotation3D< double >;
    extern template class rw::math::Rotation3D< float >;
#else
    SWIG_DECLARE_TEMPLATE (Rotation3Dd, rw::math::Rotation3D< double >);
    SWIG_DECLARE_TEMPLATE (Rotation3Df, rw::math::Rotation3D< float >);
#endif
    using Rotation3Dd = Rotation3D< double >;
    using Rotation3Df = Rotation3D< float >;

    /**@}*/
}}    // namespace rw::math

namespace rw { namespace common {
    class OutputArchive;
    class InputArchive;
    namespace serialization {
        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void write (const rw::math::Rotation3D< double >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::write
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void write (const rw::math::Rotation3D< float >& sobject,
                    rw::common::OutputArchive& oarchive, const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void read (rw::math::Rotation3D< double >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);

        /**
         * @copydoc rw::common::serialization::read
         * @relatedalso rw::math::Rotation3D
         */
        template<>
        void read (rw::math::Rotation3D< float >& sobject, rw::common::InputArchive& iarchive,
                   const std::string& id);
    }    // namespace serialization
}}       // namespace rw::common

namespace boost { namespace serialization {
    /**
     * @brief Boost serialization.
     * @param archive [in] the boost archive to read from or write to.
     * @param R [in/out] the rotation matrix to read/write.
     * @param version [in] class version (currently version 0).
     * @relatedalso rw::math::Rotation3D
     */
    template< class Archive, class T >
    void serialize (Archive& archive, rw::math::Rotation3D< T >& R, const unsigned int version)
    {
        archive& R (0, 0);
        archive& R (0, 1);
        archive& R (0, 2);
        archive& R (1, 0);
        archive& R (1, 1);
        archive& R (1, 2);
        archive& R (2, 0);
        archive& R (2, 1);
        archive& R (2, 2);
    }
}}    // namespace boost::serialization

#endif    // end include guard


// Header for this file
#include "jvector.h"
// C System-Headers
#include <termios.h> /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <fcntl.h>   //fopen(),fclose()
#include <unistd.h>  //read(), write()
#include <stdio.h>

// C++ System headers
#include <vector>      //vector
#include <string>      //string
#include <fstream>     //iss* ofstream
#include <chrono>      // timing functions
#include <cmath>       //sqrt, abs
#include <iostream>    //cout
#include <typeinfo>    //typeid
#include <algorithm>   // transform, find, count, erase
#include <functional>  // plus/minus/multiplies
#include <utility>     //std::make_pair
#include <map>         //std::map
#include <typeinfo>    //typeid
#include <mutex> //protect against concurrent access when using (unordered) parallel for loops
#include <assert.h> //static_assert

// Boost Headers
#include <boost/algorithm/string.hpp>  //split() and is_any_of for parsing .csv files
#include <boost/lexical_cast.hpp>  //lexical cast (unsurprisingly)
#include <dirent.h>

// Miscellaneous Headers
#include <omp.h>  //OpenMP pragmas

namespace jaspl {

template class JVector<int>;
template class JVector<float>;
template class JVector<double>;
template class JVector<char>;

template <class F> JVector<F>::JVector(std::string raw_data) {
    ParseRawData(raw_data);
}

template <class F> JVector<F>::JVector(std::vector<F> vec) {
    underlying_vector = vec;
}

template <class F> JVector<F>::JVector(F* ptr, uint ptr_size) {

    underlying_vector.reserve(underlying_vector.size() + ptr_size);
    std::copy(&ptr[0], &ptr[ptr_size], std::back_inserter(underlying_vector));
}

template <class F> JVector<F>::JVector(uint size) {
    underlying_vector.reserve( size );
}

template <class F> JVector<F>::JVector(uint size, F fill_element) {
    underlying_vector.reserve( size );
    underlying_vector = std::vector<F> ( size , fill_element);
}

template <class F> JVector<F>::JVector() {}

template <class F> JVector<F>::~JVector() {
    underlying_vector.clear();
}

template <class F> bool JVector<F>::check_if_arithmetic(F input) {

    if ( !std::is_arithmetic<F>::value ) {

        std::string err_mesg = "JVector: ";
        err_mesg += "cannot initialize from non-arithmetic type ";
        err_mesg += boost::lexical_cast<std::string>(typeid(input).name());
        throw std::invalid_argument(err_mesg);

        return false;
    } else {
        return true;
    }
}

template <class F> uint JVector<F>::size() {
    return underlying_vector.size();
}

template <class F> uint JVector<F>::num_chars(std::string raw_data, char delim) {
    return std::count(raw_data.begin(), raw_data.end(), delim);
}

template <class F> void JVector<F>::ParseRawData( std::string raw_data ) {

    uint lines = num_chars(raw_data, '\n');

    std::istringstream data_stream(raw_data);

    for (uint i = 0; i < lines; i++) {
        std::string input;
        std::getline(data_stream, input);
        try {
            F val = boost::lexical_cast<F>(input);
            underlying_vector.push_back(val);
        } catch (const boost::bad_lexical_cast& err) {
            std::cerr << err.what() << std::endl;
        }
    }
}

template <class F> double JVector<F>::sum(std::vector<F>& data_list,double exponent) {

    double tot=0;

    for ( uint i = 0 ; i < data_list.size(); i ++) {
        tot += pow( data_list[i], exponent );
    }

    return tot;
}

template <class F> double JVector<F>::mean(std::vector<F>& data_list) {
    //compute mean value of data set
    double sum_x=sum(data_list,1.0);
    double n=data_list.size();
    return sum_x/n;
}

template <class F> double JVector<F>::mean() {
    return mean(underlying_vector);
}

template <class F> F JVector<F>::min() {
    F min_power= *std::min_element(underlying_vector.begin(), underlying_vector.end());
    return min_power;
}

template <class F> F JVector<F>::max() {
    F min_power= *std::max_element(underlying_vector.begin(), underlying_vector.end());
    return min_power;
}

template <class F> double JVector<F>::std_dev(std::vector<F> &data_list) {

    //compute mean value of data set
    double sum_x=sum(data_list,1.0);
    double n=data_list.size();
    double mean = sum_x/n;

    //compute variance taking into account Bessel's correction i.e. n/(n-1)
    double sum_x2=sum(data_list,2.0);
    double sigma_sqr=sum_x2/(n-1.0)-n/(n-1.0)*pow(mean,2.0);

    //return square root of variance
    return sqrt(sigma_sqr);
}

template <class F> double JVector<F>::std_dev() {
    return std_dev(underlying_vector);
}

template <class F> double JVector<F>::norm() {
    return sqrt(sum(underlying_vector,2.0));

}

template <class F> void JVector<F>::Normalize() {
    double norm_factor=sqrt(sum(underlying_vector,2.0));

    for(unsigned int i = 0; i<underlying_vector.size(); i++) {
        underlying_vector.at(i)=underlying_vector.at(i)/norm_factor;
    }
}

}


///////////////////////////////////////////////////////////////////////////////
// weighted_tail_quantile.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_TAIL_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_tail_quantile_impl
    //  Tail quantile estimation based on order statistics of weighted samples
    /**
        @brief Tail quantile estimation based on order statistics of weighted samples (for both left and right tails)

        An estimator \f$\hat{q}\f$ of tail quantiles with level \f$\alpha\f$ based on order statistics
        \f$X_{1:n} \leq X_{2:n} \leq\dots\leq X_{n:n}\f$ of weighted samples are given by \f$X_{\lambda:n}\f$ (left tail)
        and \f$X_{\rho:n}\f$ (right tail), where

            \f[
                \lambda = \inf\left\{ l \left| \frac{1}{\bar{w}_n}\sum_{i=1}^{l} w_i \geq \alpha \right. \right\}
            \f]

        and

            \f[
                \rho = \sup\left\{ r \left| \frac{1}{\bar{w}_n}\sum_{i=r}^{n} w_i \geq (1 - \alpha) \right. \right\},
            \f]

        \f$n\f$ being the number of samples and \f$\bar{w}_n\f$ the sum of all weights.

        @param quantile_probability
    */
    template<typename Sample, typename Weight, typename LeftRight>
    struct weighted_tail_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Weight, std::size_t>::result_type float_type;
        // for boost::result_of
        typedef Sample result_type;

        weighted_tail_quantile_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            float_type threshold = sum_of_weights(args)
                             * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] );

            std::size_t n = 0;
            Weight sum = Weight(0);

            while (sum < threshold)
            {
                if (n < static_cast<std::size_t>(tail_weights(args).size()))
                {
                    sum += *(tail_weights(args).begin() + n);
                    n++;
                }
                else
                {
                    if (std::numeric_limits<result_type>::has_quiet_NaN)
                    {
                        return std::numeric_limits<result_type>::quiet_NaN();
                    }
                    else
                    {
                        std::ostringstream msg;
                        msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                        boost::throw_exception(std::runtime_error(msg.str()));
                        return Sample(0);
                    }
                }
            }

            // Note that the cached samples of the left are sorted in ascending order,
            // whereas the samples of the right tail are sorted in descending order
            return *(boost::begin(tail(args)) + n - 1);
        }
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_tail_quantile<>
//
namespace tag
{
    template<typename LeftRight>
    struct weighted_tail_quantile
      : depends_on<sum_of_weights, tail_weights<LeftRight> >
    {
        /// INTERNAL ONLY
        typedef accumulators::impl::weighted_tail_quantile_impl<mpl::_1, mpl::_2, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_tail_quantile
//
namespace extract
{
    extractor<tag::quantile> const weighted_tail_quantile = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_tail_quantile)
}

using extract::weighted_tail_quantile;

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif