//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/function/scalar/oneplus.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

   template < class A0,
             class unit_tag,
             class style,
             class base_A0 = bd::scalar_of_t<A0>
  >
  struct invtrig_base{};

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::not_simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 a0) BOOST_NOEXCEPT
    {
      A0 sign, x, z;
      x = bs::abs(a0);
      sign = bitofsign(a0);
      if ((x < Constant<A0,0x38d1b717>())) return a0; //1.0e-4
      if ((x >  One<A0>())) return Nan<A0>();
      auto bx_larger_05    = (x > Half<A0>());
      if (bx_larger_05)
      {
        z = Half<A0>()*oneminus(x);
        x =  sqrt(z);
      }
      else
      {
        z = sqr(x);
      }
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = fma(z1, z*x, x);
      if(bx_larger_05)
      {
        z1 = z1+z1;
        z1 = Pio_2<A0>()-z1;
      }
      return bitwise_xor(z1, sign);
    }

    static BOOST_FORCEINLINE A0 acos(const  A0& a0) BOOST_NOEXCEPT
    {
      if (a0 < Mhalf<A0>())
        return Pi<A0>()-asin( sqrt(oneplus(a0)*Half<A0>()))*Two<A0>();
      else if (a0 > Half<A0>())
        return asin( sqrt(oneminus(a0)*Half<A0>()))*Two<A0>();
      return (Pio_2<A0>()-asin(a0));
    }

    static BOOST_FORCEINLINE A0 atan(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = kernel_atan(a0);
      return bitwise_xor(x, bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(A0 a0) BOOST_NOEXCEPT
    {
      if (is_eqz(a0))  return Zero<A0>();
      if (is_inf(a0))  return Pio_2<A0>();
      A0 x = bs::abs(a0);
      A0 y = 0.0;
      A0 more = Zero<A0>();
      if( x > Tan_3pio_8<A0>())
      {
        y = Pio_2<A0>();
        more = Pio_2lo<A0>();
        x = -rec(x);
      }
      else if( x > Tanpio_8<A0>())
      {
        y = Pio_4<A0>();
        more =  Pio_4lo<A0>();
        x = minusone(x)/oneplus(x);
      }
      A0 z = sqr(x);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
    z1 = fma(x, z1*z, x);

      return y+(z1+more);
    }
  };
}
} }
#endif


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "algorithms/public/STFT.hpp"
#include "algorithms/util/AlgorithmUtils.hpp"
#include "algorithms/util/FluidEigenMappings.hpp"
#include "algorithms/GraphPlayUtils.hpp"
#include "data/TensorTypes.hpp"
#include "data/FluidDataSet.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <random>

namespace fluid {
namespace algorithm {

class GraphPlay {

public:
  using  MatrixXd = Eigen::MatrixXd;
  using  VectorXd = Eigen::VectorXd;
  using  DataSet = FluidDataSet<std::string, double, 1>;

  void init(RealVectorView audio, index sampleRate,
            index windowSize, index fftSize, index hopSize, index numBands,
            index distance, double threshold, RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    mWindowSize = windowSize;
    mFFTSize = fftSize;
    mHopSize = hopSize;
    mFrameSize = (mFFTSize / 2) + 1;
    mThreshold = threshold;
    STFT stft = STFT(mWindowSize, mFFTSize, mHopSize);
    mLength = std::floor((audio.size() + mHopSize) / mHopSize);
    mSpectrogram = ComplexMatrix(mLength, mFrameSize);
    stft.process(audio, mSpectrogram);
    RealMatrix magnitude(mLength, mFrameSize);
    stft.magnitude(mSpectrogram, magnitude);
    mDM = mUtils.computeDM(magnitude, numBands, sampleRate, windowSize, fftSize, distance);
    mDM.diagonal().setZero();
    mForbidden = ArrayXXd::Ones(mDM.rows(), mDM.cols());
    mVisited = ArrayXXd::Zero(mDM.rows(), mDM.cols());
    ArrayXd odf = mDM.diagonal(1).array();
    mRP = (mDM.array() < threshold).cast<double>();
    mRP = mRP.array() * mForbidden.array();
    mInitialized = true;
  }

  void processFrame(ComplexVectorView out, double start, double threshold,
    index minLength, index minDist, index forget, RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    if(mThreshold != threshold){
      mRP =  (mDM.array() < threshold).cast<double>();
      mRP = mRP.array() * mForbidden.array();
      mThreshold = threshold;
    }
    mVisited = (mVisited.array() - 1).cwiseMax(0);
    index startFrame = lrint(start * (mSpectrogram.rows() - 1));
    if(startFrame != mStartFrame ){
      mStartFrame = startFrame;
      mPos = mStartFrame;
      index nNeighbors = mRP.col(mPos).sum();
      while(nNeighbors == 0){
        nNeighbors = mRP.col(mPos).sum();
        mPos = (mPos + 1) % mSpectrogram.rows();
      }
      mCount = 0;
    }
    else if (mCount < minLength){
      mPos = (mPos + 1) % mSpectrogram.rows();
      mCount++;
    }
    else{
        index nNeighbors = mRP.col(mPos).sum();
        index nForbidden = mForbidden.col(mPos).sum();
        index prevPos = mPos;
        if(nNeighbors > 0){
          std::vector<index> candidates(nNeighbors);
          index nCandidates = 0;
          for(index i = 0; i < mRP.rows(); i++)
            if(abs(i - mPos) > minDist && mRP(mPos, i) > 0 && mVisited(mPos, i) <= 0){
              candidates[nCandidates++] = i;
            }
            index next = mUtils.randInt(nCandidates);
            mPos = candidates[next];
            mCount = 0;
        }
        if (mPos == prevPos){
          mPos = (mPos + 1) % mSpectrogram.rows();
        }
        mVisited(prevPos, mPos) = forget;
    }
    out = mSpectrogram.row(mPos);
    output(0)  = mPos;
  }

  bool initialized(){
    return mInitialized;
  }

  index num{0};

  index mWindowSize;
  index mHopSize;
  index mFFTSize;

private:
  GraphPlayUtils mUtils;
  index mFrameSize;
  ComplexMatrix mSpectrogram;
  MatrixXd mDM;
  MatrixXd mRP;
  MatrixXd mForbidden;
  MatrixXd mVisited;
  VectorXd mDeg;
  bool mInitialized{false};
  int mPos{0};
  index mLength;
  index mStartFrame{-1};
  index mEndFrame;
  double mThreshold;
  index mCount{0};
};
} // namespace algorithm
} // namespace fluid


/*
 * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: transforms.cpp 23275 2009-08-28 20:27:09Z mariusmuja $
 *
 */

/** \author Radu Bogdan Rusu */

#include <point_cloud_mapping/geometry/angles.h>
#include <point_cloud_mapping/geometry/transforms.h>
#include <point_cloud_mapping/geometry/nearest.h>

#include <Eigen/SVD>

namespace cloud_geometry
{
  namespace transforms
  {

  /**
   * See header file
   */
  bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const sensor_msgs::PointCloud& pc_b_,
		  Eigen::Matrix4d &transformation)
  {

	  assert (pc_a_.get_points_size()==pc_b_.get_points_size());

	  sensor_msgs::PointCloud pc_a = pc_a_;
	  sensor_msgs::PointCloud pc_b = pc_b_;

	  // translate both point cloud so that their centroid will be in origin
	  geometry_msgs::Point32 centroid_a;
	  geometry_msgs::Point32 centroid_b;

	  nearest::computeCentroid(pc_a, centroid_a);
	  nearest::computeCentroid(pc_b, centroid_b);

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  pc_a.points[i].x -= centroid_a.x;
		  pc_a.points[i].y -= centroid_a.y;
		  pc_a.points[i].z -= centroid_a.z;
	  }

	  for (size_t i=0;i<pc_b.get_points_size();++i) {
		  pc_b.points[i].x -= centroid_b.x;
		  pc_b.points[i].y -= centroid_b.y;
		  pc_b.points[i].z -= centroid_b.z;
	  }

	  // solve for rotation
	  Eigen::Matrix3d correlation;
	  correlation.setZero();

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
		  correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
		  correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;

		  correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
		  correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
		  correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;

		  correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
		  correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
		  correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
	  }

	  Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);

	  Eigen::Matrix3d Ut = svd.matrixU().transpose();
	  Eigen::Matrix3d V = svd.matrixV();
	  Eigen::Matrix3d X = V*Ut;

	  double det = X.determinant();
	  if (det<0) {
		  V.col(2) = -V.col(2);
		  X = V*Ut;
	  }

	  transformation.setZero();
	  transformation.topLeftCorner<3,3>() = X;
	  transformation(3,3) = 1;

	  sensor_msgs::PointCloud pc_rotated_a;
	  transformPoints(pc_a_.points, pc_rotated_a.points, transformation);

	  geometry_msgs::Point32 centroid_rotated_a;
	  nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);

	  transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
	  transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
	  transformation(2,3) = centroid_b.z - centroid_rotated_a.z;

	  return true;

  }

  /**
   * See header file
   */
	bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const std::vector<int>& indices_a,
										  const sensor_msgs::PointCloud& pc_b_, const std::vector<int>& indices_b,
										  Eigen::Matrix4d &transformation)
	{

	  assert (indices_a.size()==indices_b.size());

	  sensor_msgs::PointCloud pc_a;
	  sensor_msgs::PointCloud pc_b;

	  getPointCloud(pc_a_, indices_a, pc_a);
	  getPointCloud(pc_b_, indices_b, pc_b);

	  // translate both point cloud so that their centroid will be in origin
	  geometry_msgs::Point32 centroid_a;
	  geometry_msgs::Point32 centroid_b;

	  nearest::computeCentroid(pc_a, centroid_a);
	  nearest::computeCentroid(pc_b, centroid_b);

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  pc_a.points[i].x -= centroid_a.x;
		  pc_a.points[i].y -= centroid_a.y;
		  pc_a.points[i].z -= centroid_a.z;
	  }

	  for (size_t i=0;i<pc_b.get_points_size();++i) {
		  pc_b.points[i].x -= centroid_b.x;
		  pc_b.points[i].y -= centroid_b.y;
		  pc_b.points[i].z -= centroid_b.z;
	  }

	  // solve for rotation
	  Eigen::Matrix3d correlation;
	  correlation.setZero();

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
		  correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
		  correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;

		  correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
		  correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
		  correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;

		  correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
		  correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
		  correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
	  }

	  Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);

	  Eigen::Matrix3d Ut = svd.matrixU().transpose();
	  Eigen::Matrix3d V = svd.matrixV();
	  Eigen::Matrix3d X = V*Ut;

	  double det = X.determinant();
	  if (det<0) {
		  V.col(2) = -V.col(2);
		  X = V*Ut;
	  }

	  transformation.setZero();
	  transformation.topLeftCorner<3,3>() = X;
	  transformation(3,3) = 1;

	  sensor_msgs::PointCloud pc_rotated_a;
	  getPointCloud(pc_a_, indices_a, pc_a);
	  transformPoints(pc_a.points, pc_rotated_a.points, transformation);

	  geometry_msgs::Point32 centroid_rotated_a;
	  nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);

	  transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
	  transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
	  transformation(2,3) = centroid_b.z - centroid_rotated_a.z;

//	  transformation.setIdentity();
//	  transformation(0,3) = centroid_b.x - centroid_a.x;
//	  transformation(1,3) = centroid_b.y - centroid_a.y;
//	  transformation(2,3) = centroid_b.z - centroid_a.z;

	  return true;

  }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Obtain a 4x4 rigid transformation matrix (with translation)
      * \param plane_a the normalized coefficients of the first plane
      * \param plane_b the normalized coefficients of the second plane
      * \param tx the desired translation on x-axis
      * \param ty the desired translation on y-axis
      * \param tz the desired translation on z-axis
      * \param transformation the resultant transformation matrix
      */
    void
      getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const std::vector<double> &plane_b,
                                     float tx, float ty, float tz, Eigen::Matrix4d &transformation)
    {
      double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
      // Compute the rotation axis R = Nplane x (0, 0, 1)
      geometry_msgs::Point32 r_axis;
      r_axis.x = plane_a[1]*plane_b[2] - plane_a[2]*plane_b[1];
      r_axis.y = plane_a[2]*plane_b[0] - plane_a[0]*plane_b[2];
      r_axis.z = plane_a[0]*plane_b[1] - plane_a[1]*plane_b[0];

      if (r_axis.z < 0)
        angle = -angle;

      // Build a normalized quaternion
      double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
      double x = r_axis.x * s;
      double y = r_axis.y * s;
      double z = r_axis.z * s;
      double w = cos (0.5 * angle);

      // Convert the quaternion to a 3x3 matrix
      double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
      double wx = w * x; double wy = w * y; double wz = w * z;
      double xy = x * y; double xz = x * z; double yz = y * z;

      transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
      transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
      transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
      transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Obtain a 4x4 rigid transformation matrix (with translation)
      * \param plane_a the normalized coefficients of the first plane
      * \param plane_b the normalized coefficients of the second plane
      * \param tx the desired translation on x-axis
      * \param ty the desired translation on y-axis
      * \param tz the desired translation on z-axis
      * \param transformation the resultant transformation matrix
      */
    void
      getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b,
                                     float tx, float ty, float tz, Eigen::Matrix4d &transformation)
    {
      double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
      // Compute the rotation axis R = Nplane x (0, 0, 1)
      geometry_msgs::Point32 r_axis;
      r_axis.x = plane_a[1]*plane_b.z - plane_a[2]*plane_b.y;
      r_axis.y = plane_a[2]*plane_b.x - plane_a[0]*plane_b.z;
      r_axis.z = plane_a[0]*plane_b.y - plane_a[1]*plane_b.x;

      if (r_axis.z < 0)
        angle = -angle;

      // Build a normalized quaternion
      double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
      double x = r_axis.x * s;
      double y = r_axis.y * s;
      double z = r_axis.z * s;
      double w = cos (0.5 * angle);

      // Convert the quaternion to a 3x3 matrix
      double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
      double wx = w * x; double wy = w * y; double wz = w * z;
      double xy = x * y; double xz = x * z; double yz = y * z;

      transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
      transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
      transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
      transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Convert an axis-angle representation to a 3x3 rotation matrix
      * \note The formula is given by: A = I * cos (th) + ( 1 - cos (th) ) * axis * axis' - E * sin (th), where
      * E = [0 -axis.z axis.y; axis.z 0 -axis.x; -axis.y axis.x 0]
      * \param axis the axis
      * \param angle the angle
      * \param rotation the resultant rotation
      */
    void
      convertAxisAngleToRotationMatrix (const geometry_msgs::Point32 &axis, double angle, Eigen::Matrix3d &rotation)
    {
      double cos_a = cos (angle);
      double sin_a = sin (angle);
      double cos_a_m = 1.0 - cos_a;

      double a_xy = axis.x * axis.y * cos_a_m;
      double a_xz = axis.x * axis.z * cos_a_m;
      double a_yz = axis.y * axis.z * cos_a_m;

      double s_x = sin_a * axis.x;
      double s_y = sin_a * axis.y;
      double s_z = sin_a * axis.z;

      rotation (0, 0) = cos_a + axis.x * axis.x * cos_a_m;
      rotation (0, 1) = a_xy - s_z;
      rotation (0, 2) = a_xz + s_y;
      rotation (1, 0) = a_xy + s_z;
      rotation (1, 1) = cos_a + axis.y * axis.y * cos_a_m;
      rotation (1, 2) = a_yz - s_x;
      rotation (2, 0) = a_xz - s_y;
      rotation (2, 1) = a_yz + s_x;
      rotation (2, 2) = cos_a + axis.z * axis.z * cos_a_m;
    }

  }
}


//
// Created by Сергей Кривонос on 01.09.17.
//
#include "Fraction.h"
#include "Integer.h"
#include "Sum.h"
#include "Product.h"

#include <boost/lexical_cast.hpp>

//
// namespace std{
//     template<>
//     struct is_signed<boost::multiprecision::cpp_int> {
//         bool value = true;
//     };
// }
//

namespace omnn{
namespace math {
	Fraction::Fraction(const Integer& n)
		: base(n, 1)
	{
	}

	Fraction::Fraction(boost::rational<a_int>&& r)
	: base(r.numerator(), r.denominator())
	{
	}

    Fraction::Fraction(const boost::multiprecision::cpp_dec_float_100& f)
    {
        auto s = boost::lexical_cast<std::string>(f);
        if (s=="nan") {
            throw s;
        }
        auto doti = s.find_first_of('.');
        if (doti == -1)
        {
            numerator() = Integer(boost::multiprecision::cpp_int(f));
            denominator() = 1;
        }
        else
        {
            auto afterDotI = doti + 1;
            auto fsz = s.length() - afterDotI;
            denominator() = 10_v ^ fsz;
            numerator() = Integer(boost::multiprecision::cpp_int(s.c_str()+afterDotI));
            if (s[0]=='-') {
                numerator() = -numerator();
            }
            s.erase(doti);
            numerator() += Integer(boost::multiprecision::cpp_int(s)) * denominator();
        }
    }

	Valuable Fraction::operator -() const
    {
        return Fraction(-getNumerator(), getDenominator());
    }

    bool Fraction::operator ==(const Valuable& v) const
    {
        auto eq = v.IsFraction() && Hash()==v.Hash();
        if(eq){
            auto& ch = v.as<Fraction>();
            eq= _1.Hash() == ch._1.Hash()
                && _2.Hash() == ch._2.Hash()
                && _1 == ch._1
                && _2 == ch._2;
        } else if (v.IsExponentiation()) {
            eq = v.operator==(*this);
        }
        return eq;
    }

    void Fraction::solve(const Variable& va, solutions_t& s) const {
        numerator().solve(va, s);
        solutions_t exclude;
        denominator().solve(va, exclude);
        for(auto& so:exclude)
            s.erase(so);
    }

    Valuable::solutions_t Fraction::Distinct() const {
        Valuable::solutions_t branches;
        for (auto&& n : numerator().Distinct()) {
            for (auto&& d : denominator().Distinct()) {
                branches.emplace(n / d);
            }
        }
        return branches;
    }

    void Fraction::optimize()
    {
//        if (!optimizations) {
//            hash = numerator().Hash() ^ denominator().Hash();
//            return;
//        }
    reoptimize_the_fraction:
        numerator().optimize();
        denominator().optimize();

        while(denominator().IsFraction())
        {
            auto& fdn = denominator().as<Fraction>();
            numerator() *= fdn.denominator();
            denominator() = std::move(fdn.numerator());
        }

//        if (numerator().IsSum()) {
//            Become(numerator()/denominator());
//            return;
//        }

        if (denominator().IsInt())
        {
            if (denominator().ca() == 1)
            {
                Become(std::move(numerator()));
                return;
            }
            else if (denominator() < 0) {
                numerator() = -numerator();
                denominator() = -denominator();
            }
        }

        if (numerator().IsExponentiation()) {
            auto& e = numerator().as<Exponentiation>();
            auto& exp = e.getExponentiation();
            if (exp.IsInt() && exp < 0) {
                denominator() *= e.getBase() ^ (-exp);
                numerator() = 1;
            } else if (exp.IsFraction()) {
                auto& f = exp.as<Fraction>();
                auto in = e.getBase() / (denominator() ^ f.Reciprocal());
                if (in.IsInt())
                {
                    e.setBase(std::move(in));
                    Become(std::move(e));
                    return;
                }
            }
        }

        // integers
        if (numerator().IsInt())
        {
            auto& n = numerator().ca();
            auto& denom = denominator();
            auto dni = denom.IsInt();
            if (n == 0)
            {
                if (dni && denom == 0_v)
                    throw "NaN";
                Become(std::move(numerator()));
                return;
            }
            if (dni)
            {
                auto& dn = denominator().ca();
                if (n < 0 && dn < 0) {
                    numerator() = -numerator();
                    denominator() = -denominator();
                    goto reoptimize_the_fraction;
                }
                if (dn == 1)
                {
                    Become(std::move(numerator()));
                    return;
                }
                if (dn == -1)
                {
                    Become(-numerator());
                    return;
                }
                auto d = boost::gcd(n, dn);
                if (d != 1) {
                    numerator() /= Integer(d);
                    denominator() /= Integer(d);
                    optimize();
                    return;
                }
            }
        }
        else
        {
            std::vector<Variable> coVa;

            if (numerator().IsProduct()) {
                //if (denominator().IsProduct()) {
                    Become(numerator() / denominator());
                    return;
                //}
                //else
                //{
                //    auto n = Product::cast(numerator());
                    //if (n->Has(denominator()))
					//{
     //                   Become(*n / denominator());
     //                   return;
     //               }
     //           }
            }
            else if (denominator().IsProduct())
            {
                auto& dn = denominator().as<Product>();
                if (dn.Has(numerator())) {
                    denominator() /= numerator();
                    numerator() = 1_v;
                    goto reoptimize_the_fraction;
                }
                else if (numerator().IsInt() || numerator().IsSimpleFraction()) {
                    for (auto& m : dn)
                    {
                        if (m.IsVa()) {
                            numerator() *= m ^ -1;
                        }
                        else if (m.IsExponentiation()) {
                            auto& e = m.as<Exponentiation>();
                            numerator() *= e.getBase() ^ -e.getExponentiation();
                        }
                        else
                            numerator() /= m;
                    }
                    Become(std::move(numerator()));
                    return;
                }
            }
            else if (denominator().FindVa() && !denominator().IsSum())
            {
                Become(Product{ std::move(numerator()), Exponentiation( std::move(denominator()), -1)});
                return;
            }
            else // no products
            {
                // sum
//                auto s = Sum::cast(numerator());
//                if (s) {
//                    auto sum(std::move(*s));
//                    sum /= denominator();
//                    Become(std::move(sum));
//                    return;
//                }
            }
        }

        if (IsFraction()) {
            if(IsSimple())
                hash = numerator().Hash() ^ denominator().Hash();
            else if (!denominator().IsSum())
                Become(numerator()*(denominator()^-1));
        }
    }

    bool Fraction::MultiplyIfSimplifiable(const Valuable& v)
    {
        auto is = IsSimpleFraction() && v.IsSimple();
        if(is){
            *this *= v;
        } else if (!v.IsFraction()) {
            auto s = v.IsMultiplicationSimplifiable(*this);
            is = s.first;
            if (is) {
                Become(std::move(s.second));
            }
        } else {
            IMPLEMENT
        }
        return is;
    }

    std::pair<bool,Valuable> Fraction::IsMultiplicationSimplifiable(const Valuable& v) const
    {
        std::pair<bool,Valuable> is;
        is.first = IsSimpleFraction() && v.IsSimple();
        if(is.first){
            is.second = *this * v;
        } else if (v.IsConstant()) {
        } else if (!v.IsFraction()) {
            is = v.IsMultiplicationSimplifiable(*this);
        } else {
            IMPLEMENT
        }
        return is;
    }

bool Fraction::SumIfSimplifiable(const Valuable& v)
{
    auto is = IsSimpleFraction() && v.IsSimple();
    if(is){
        *this += v;
    } else if (!v.IsFraction()) {
        auto s = v.IsSummationSimplifiable(*this);
        is = s.first;
        if (is) {
            Become(std::move(s.second));
        }
    } else {
        IMPLEMENT
    }
    return is;
}

std::pair<bool,Valuable> Fraction::IsSummationSimplifiable(const Valuable& v) const
{
    std::pair<bool,Valuable> is;
    auto simple = IsSimpleFraction();
    is.first = simple && v.IsSimple();
    if(is.first){
        is.second = *this + v;
    } else if (v.IsVa()) {
        is.first = !simple && HasVa(v.as<Variable>());
        if (is.first) {
            LOG_AND_IMPLEMENT("Optimize summation of " << *this << " with " << v);
        }
    } else if (!v.IsFraction()) {
        is = v.IsSummationSimplifiable(*this);
    } else {
        IMPLEMENT
    }
    return is;
}

    Valuable& Fraction::operator +=(const Valuable& v)
    {
        if (v.IsFraction()){
            auto& f = v.as<Fraction>();
            if(denominator() == f.denominator()) {
                numerator() += f.numerator();
            } else {
                numerator() = numerator() * f.denominator() + f.numerator() * denominator();
                denominator() *= f.denominator();
            }
        }
        else if(v.IsInt() && IsSimple()) {
            setNumerator(numerator() + denominator() * v.as<Integer>());
        } else {
            return Become(Sum {*this, v});
        }

        optimize();
        return *this;
    }

    Valuable& Fraction::operator *=(const Valuable& v)
    {
        if (v.IsFraction())
        {
            auto& f = v.as<Fraction>();
            numerator() *= f.numerator();
            denominator() *= f.denominator();
        }
        else if (v.IsInt())
        {
            numerator() *= v;
        }
        else
        {
            return Become(v * *this);
        }

        optimize();
        return *this;
    }

    Valuable& Fraction::operator /=(const Valuable& v)
    {
        if (v.IsFraction())
        {
            auto& f = v.as<Fraction>();
            numerator() *= f.denominator();
            denominator() *= f.numerator();
        }
        else if (v.IsProduct())
        {
            for(auto& _ : v.as<Product>())
                *this /= _;
        }
        else
        {
            denominator() *= v;
        }
        optimize();
        return *this;
    }

    Valuable& Fraction::operator %=(const Valuable& v)
    {
        Integer d(*this / v);
		return *this -= d * v;
    }

    Valuable& Fraction::operator^=(const Valuable& v)
    {
        if(v.IsFraction()){
            auto& vf = v.as<Fraction>();
            auto& vfdn = vf.denominator();
            if (vfdn == 2_v)
                ;
            else if (vfdn.bit(0_v)!=1_v) {
                IMPLEMENT
            }
        }
        numerator() ^= v;
        denominator() ^= v;
        optimized = {};
        optimize();
        return *this;
//        if(v.IsInt())
//        {
//            auto i = v.ca();
//            if (i != 0_v) {
//                if (i > 1_v) {
//                    auto a = *this;
//                    for (auto n = i; n > 1_v; --n) {
//                        *this *= a;
//                    }
//                    optimize();
//                    return *this;
//                }
//            }
//            else { // zero
//                if (numerator() == 0_v)
//                    throw "NaN"; // feel free to handle this properly
//                else
//                    return Become(1_v);
//            }
//        }
//        else if(IsSimple())
//        {
//            auto f = Fraction::cast(v);
//            if (f->IsSimple())
//            {
//                auto n = f->numerator();
//                auto dn = f->denominator();
//
//                if (n != 1_v)
//                    *this ^= n;
//                Valuable nroot;
//                Valuable left =0, right = *this;
//
//                for (;;)
//                {
//                    nroot = left +(right - left) / 2_v;
//                    auto result = nroot ^ dn;
//                    if (result == *this)
//                        return Become(std::move(nroot));
//                    else if (*this < result)
//                        right = nroot;
//                    else
//                        left = nroot;
//                }
//            }
//        }
//
//        return Become(Exponentiation(*this, v));
    }

    Valuable& Fraction::d(const Variable& x)
    {
        if (IsSimpleFraction()) {
            Become(0_v);
        } else {
            IMPLEMENT
        }
        return *this;
    }

    bool Fraction::operator <(const Valuable& v) const
    {
        if (v.IsFraction())
        {
            auto& f = v.as<Fraction>();
            return numerator() * f.denominator() < f.numerator() * denominator();
        }
        else if (v.IsInt())
        {
            if(denominator()<0)
                return -numerator() < v * -denominator();
            else
                return numerator() < v * denominator();
        }
        else
            return base::operator <(v);
    }

    Fraction::operator double() const
    {
        return static_cast<double>(numerator()) / static_cast<double>(denominator());
    }

    Valuable& Fraction::sq(){
        numerator().sq();
        denominator().sq();
        optimize();
        return *this;
    }

    Valuable Fraction::Sqrt() const
    {
        return numerator().Sqrt() / denominator().Sqrt();
    }

    std::ostream& Fraction::print_sign(std::ostream& out) const
    {
        return out << '/';
    }

    const Valuable::vars_cont_t& Fraction::getCommonVars() const
    {
        vars = numerator().getCommonVars();
        for(auto& r : denominator().getCommonVars())
        {
            vars[r.first] -= r.second;
        }
        return vars;
    }

    Valuable Fraction::InCommonWith(const Valuable& v) const
    {
        auto c = 1_v;
        if (v.IsFraction()) {
            auto& f = v.as<Fraction>();
            c *= getNumerator().InCommonWith(f.getNumerator());
            c /= getDenominator().InCommonWith(f.getDenominator());
        }
        else
        {
            c = getNumerator().InCommonWith(v);
        }
        return c;
    }

    bool Fraction::IsComesBefore(const Valuable& v) const
    {
//        auto va1 = FindVa();
//        auto va2 = v.FindVa();
//        return !va1 && !va2
//            ? (IsSimple() && (v.IsInt() || (v.IsFraction() && Fraction::cast(v)->IsSimple())) ? operator<(v) : str().length() < v.str().length())
//                : (va1 && va2
//                   ? str().length() < v.str().length()
//                   : va1!=nullptr );
        auto mve = base::getMaxVaExp();
        auto vmve = v.getMaxVaExp();
        auto is = mve > vmve;
        if (mve != vmve)
        {}
        else if (v.IsFraction())
        {
            if (IsSimple() && v.IsSimpleFraction())
                is = *this < v;
            else
            {
                auto& f = v.as<Fraction>();
                is = numerator().IsComesBefore(f.numerator()) || denominator().IsComesBefore(f.denominator());
            }
//            auto e = cast(v);
//            bool numerator()IsVa = numerator().IsVa();
//            bool vbaseIsVa = e->numerator().IsVa();
//            if (numerator()IsVa && vbaseIsVa)
//                is = denominator() == e->denominator() ? numerator().IsComesBefore(e->numerator()) : denominator() > e->denominator();
//            else if(numerator()IsVa)
//                is = false;
//            else if(vbaseIsVa)
//                is = true;
//            else if(numerator() == e->numerator())
//                is = denominator().IsComesBefore(e->denominator());
//            else if(denominator() == e->denominator())
//                is = numerator().IsComesBefore(e->numerator());
//            else
//            {
//                auto expComesBefore = denominator().IsComesBefore(e->denominator());
//                auto ebaseComesBefore = numerator().IsComesBefore(e->numerator());
//                is = ebaseComesBefore || expComesBefore;//expComesBefore==ebaseComesBefore || str().length() > e->str().length();
//            }
        }
        else if(v.IsProduct())
            is = Product{*this}.IsComesBefore(v);
        else if(v.IsSum())
            is = Sum{*this}.IsComesBefore(v);
        else if(v.IsVa())
            is = FindVa();
        else if(v.IsInt())
            is = true;
        else
            IMPLEMENT

        return is;
    }

    Fraction::operator unsigned char() const
    {
        return static_cast<unsigned char>(static_cast<Integer>(*this));
    }

    Fraction::operator a_int() const
    {
        if (!IsSimple()) {
            IMPLEMENT
        }
        return static_cast<a_int>(numerator())/static_cast<a_int>(denominator());
    }

    Fraction::operator boost::multiprecision::cpp_dec_float_100() const
    {
        if (IsSimple())
        {
            boost::multiprecision::cpp_dec_float_100 f(numerator().ca());
            f /= boost::multiprecision::cpp_dec_float_100(denominator().ca());
            // TODO : check validity
            return f;
        }
        else
            IMPLEMENT;
    }

    Valuable Fraction::operator()(const Variable& v, const Valuable& augmentation) const
    {
        return (augmentation * denominator() - numerator())(v);
    }

    omnn::math::Fraction Fraction::Reciprocal() const
    {
        return Fraction(denominator(), numerator());
    }

    bool Fraction::IsSimple() const
    {
        return numerator().IsInt()
            && denominator().IsInt()
        ;
    }
}}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

//------------------------------------------------------------------------------
// Pose.h
//
// Stores the camera pose as a 3d position and rotation.
// it is the transform that will bring things into a camera relative space, not
// the transform that would position a proxy object into the orientation of the
// camera
//------------------------------------------------------------------------------

#include "Pose.h"

#include "Utils\cv.h"

#include <Eigen\Dense>
#include <Eigen\Geometry>
#include <opencv2\core\eigen.hpp>
#include <opencv2\calib3d\calib3d.hpp>

namespace mage
{
    Pose::Pose()
    {
        // initialization
        m_viewMatrix = cv::Matx34f::eye();
        m_inverseViewMatrix = cv::Matx44f::eye();
    }

    Pose::Pose(const cv::Matx44f& worldMatrix)
    {
        m_inverseViewMatrix = worldMatrix;
        cv::Matx44f temp = Invert(worldMatrix);
        m_viewMatrix = temp.get_minor<3, 4>(0, 0);
    }

    Pose::Pose(const cv::Matx31f& viewSpacePosition, const cv::Matx33f& viewSpaceRotationMatrix)
    {
        SetViewSpacePositionAndRotation(viewSpacePosition, viewSpaceRotationMatrix);
    }

    void Pose::SetViewMatrix(const cv::Matx34f& viewMat)
    {
            m_viewMatrix(0, 0) = viewMat(0, 0);
            m_viewMatrix(0, 1) = viewMat(0, 1);
            m_viewMatrix(0, 2) = viewMat(0, 2);
            m_viewMatrix(0, 3) = viewMat(0, 3);

            m_viewMatrix(1, 0) = viewMat(1, 0);
            m_viewMatrix(1, 1) = viewMat(1, 1);
            m_viewMatrix(1, 2) = viewMat(1, 2);
            m_viewMatrix(1, 3) = viewMat(1, 3);

            m_viewMatrix(2, 0) = viewMat(2, 0);
            m_viewMatrix(2, 1) = viewMat(2, 1);
            m_viewMatrix(2, 2) = viewMat(2, 2);
            m_viewMatrix(2, 3) = viewMat(2, 3);

            ComputeInverseViewMatrix();
    }

    void Pose::SetViewSpacePositionAndRotation(const cv::Matx31f& positionMatrix, const cv::Matx33f& viewSpaceRotationMatrix)
    {
        m_viewMatrix(0, 3) = positionMatrix(0,0);
        m_viewMatrix(1, 3) = positionMatrix(1,0);
        m_viewMatrix(2, 3) = positionMatrix(2,0);

        m_viewMatrix(0, 0) = viewSpaceRotationMatrix(0, 0);
        m_viewMatrix(0, 1) = viewSpaceRotationMatrix(0, 1);
        m_viewMatrix(0, 2) = viewSpaceRotationMatrix(0, 2);

        m_viewMatrix(1, 0) = viewSpaceRotationMatrix(1, 0);
        m_viewMatrix(1, 1) = viewSpaceRotationMatrix(1, 1);
        m_viewMatrix(1, 2) = viewSpaceRotationMatrix(1, 2);

        m_viewMatrix(2, 0) = viewSpaceRotationMatrix(2, 0);
        m_viewMatrix(2, 1) = viewSpaceRotationMatrix(2, 1);
        m_viewMatrix(2, 2) = viewSpaceRotationMatrix(2, 2);

        ComputeInverseViewMatrix();
    }

    cv::Matx31f Pose::GetViewSpacePosition() const
    {
        return m_viewMatrix.col(3);
    }

    cv::Matx31f Pose::GetViewSpaceRodriguesRotation() const
    {
        cv::Matx31f rotation;
        cv::Rodrigues(GetRotationMatrix(), rotation);
        return rotation;
    }

    float Pose::GetRoll() const
    {
        // Todo: perf. converting from mat to quat back to mat to calculate roll.
        auto quat = ToQuat(Rotation(m_inverseViewMatrix));

        float yaw;
        float pitch;
        float roll;

        ToEuler(quat, yaw, pitch, roll);

        return roll;
    }

    cv::Point3f Pose::GetWorldSpacePosition() const
    {
        return{ m_inverseViewMatrix(0,3), m_inverseViewMatrix(1,3), m_inverseViewMatrix(2,3) };
    }

    cv::Vec3f Pose::GetWorldSpaceForward() const
    {
        return{ m_inverseViewMatrix(0,2), m_inverseViewMatrix(1,2), m_inverseViewMatrix(2,2) };
    }

    cv::Vec3f Pose::GetWorldSpaceRight() const
    {
        return{ m_inverseViewMatrix(0,0), m_inverseViewMatrix(1,0), m_inverseViewMatrix(2,0) };
    }

    // right handed, column major
    cv::Matx33f Pose::GetRotationMatrix() const
    {
        return m_viewMatrix.get_minor<3, 3>(0, 0);
    }

    // use eigen to handle the rotation matrix <=> quaternion conversion
    Quaternion Pose::GetRotationQuaternion() const
    {
        Eigen::Matrix3f eigenMat;
        cv::cv2eigen(GetRotationMatrix(), eigenMat);

        return Eigen::Quaternionf{ eigenMat }.normalized();
    }

    //right handed, column major
    const cv::Matx34f& Pose::GetViewMatrix() const
    {
        return m_viewMatrix;
    }

    cv::Matx44f Pose::GetViewMatrix4x4() const
    {
        return {
            m_viewMatrix(0, 0), m_viewMatrix(0, 1), m_viewMatrix(0, 2), m_viewMatrix(0, 3),
            m_viewMatrix(1, 0), m_viewMatrix(1, 1), m_viewMatrix(1, 2), m_viewMatrix(1, 3),
            m_viewMatrix(2, 0), m_viewMatrix(2, 1), m_viewMatrix(2, 2), m_viewMatrix(2, 3),
                             0,                  0,                  0,                  1
        };
    }

    //right handed, column major
    const cv::Matx44f& Pose::GetInverseViewMatrix() const
    {
        return m_inverseViewMatrix;
    }

    cv::Matx34f Pose::GetRelativeViewMatrix(const Pose& toFrame) const
    {
        return toFrame.GetViewMatrix() * m_inverseViewMatrix;
    }

    void Pose::ComputeInverseViewMatrix()
    {
        m_inverseViewMatrix = Invert(m_viewMatrix);
    }
}


#include <Eigen/Dense>
#include <iostream>
#include <iomanip>
#include <vector>
#include <igl/remove_unreferenced.h>
#include "SecondFundamentalForm/MidedgeAverageFormulation.h"
#include "Stitch.h"
#include "MeshConnectivity.h"
#include <igl/cotmatrix.h>
#include <igl/boundary_facets.h>
#include <igl/boundary_loop.h>

void stitchMeshes(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, Eigen::MatrixXd& stitchedV, Eigen::MatrixXi& stitchedF, std::vector<Eigen::Vector3i> &bnd_edges, Eigen::VectorXi& newIndex)
{
    std::map<int, int> parentvert;
    int nverts = V.rows();
    std::vector<bool> seam(nverts,false);
    std::vector<std::vector<int> > bls;

    igl::boundary_loop(F, bls);

    std::vector<int> boundVerts;

    for(auto &b : bls)
        for(int i = 0; i < b.size(); i++)
        {
            boundVerts.push_back(b[i]);
        }

    for(int i = 0; i < boundVerts.size(); i++)
    {
        for(int j = 0; j < boundVerts.size(); j++)
        {
            if(j == i)
                continue;
            int v1 = boundVerts[i];
            int v2 = boundVerts[j];
            if((V.row(v1) - V.row(v2)).norm() < 1e-5)
            {
                seam[v1] = true;
                seam[v2] = true;
                auto it = parentvert.find(v1);

                if(it == parentvert.end())
                    parentvert[v2] = v1;
                else
                    parentvert[v2] = it->second;
            }
        }
    }

    int nfaces = F.rows();
    Eigen::MatrixXi mappedF = F;

    for (int i = 0; i < nfaces; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            auto it = parentvert.find(mappedF(i, j));
            if (it != parentvert.end())
                mappedF(i,j) = it->second;
        }
    }

    Eigen::VectorXi I;
    Eigen::VectorXi J;
    igl::remove_unreferenced(V, mappedF, stitchedV, stitchedF, I, J);

    std::cout << "number of verts over all patches :  " << V.rows() << " number of verts after stitching : " << stitchedV.rows() << std::endl;
    std::cout << "number of faces over all patches :  " << F.rows() << " number of faces after stitching : " << stitchedF.rows() << std::endl;

    nverts = stitchedV.rows();

    newIndex = I;
    for (int i = 0; i < newIndex.size(); i++)
    {
        if (newIndex(i) == -1)
        {
            auto it = parentvert.find(i);
            newIndex(i) = I(it->second);
        }
    }

    //compute boundary

    Eigen::MatrixXi bndV;
    Eigen::MatrixXi bndF;
    Eigen::MatrixXi OppBndV;
    igl::boundary_facets(F,bndV, bndF, OppBndV);
    bnd_edges.clear();
    for (int i = 0; i < bndV.rows(); i++)
    {
        if (!(seam[bndV(i,0)] && seam[bndV(i,1)])) // both vertices are not on the seam then it's still a boundary
            bnd_edges.push_back(Eigen::Vector3i(bndV(i,0), bndV(i,1), F(bndF(i),OppBndV(i))));
    }

}

void stitchMeshesWithTol(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F, Eigen::MatrixXd &stitchedV, Eigen::MatrixXi &stitchedF, double tol)
{
    std::map<int, int> parentvert;
    int nverts = V.rows();
    std::vector<bool> seam(nverts,false);
    std::vector<std::vector<int> > bls;

    igl::boundary_loop(F, bls);

    std::vector<int> boundVerts;

    for(auto &b : bls)
        for(int i = 0; i < b.size(); i++)
        {
            boundVerts.push_back(b[i]);
        }

    for(int i = 0; i < boundVerts.size(); i++)
    {
        for(int j = 0; j < boundVerts.size(); j++)
        {
            if(j == i)
                continue;
            int v1 = boundVerts[i];
            int v2 = boundVerts[j];
            if((V.row(v1) - V.row(v2)).norm() < 1e-5)
            {
                seam[v1] = true;
                seam[v2] = true;
                auto it = parentvert.find(v1);

                if(it == parentvert.end())
                    parentvert[v2] = v1;
                else
                    parentvert[v2] = it->second;
            }
        }
    }

    int nfaces = F.rows();
    Eigen::MatrixXi mappedF = F;

    for (int i = 0; i < nfaces; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            auto it = parentvert.find(mappedF(i, j));
            if (it != parentvert.end())
                mappedF(i,j) = it->second;
        }
    }

    Eigen::VectorXi I;
    Eigen::VectorXi J;
    igl::remove_unreferenced(V, mappedF, stitchedV, stitchedF, I, J);

    std::cout << "number of verts over all patches :  " << V.rows() << " number of verts after stitching : " << stitchedV.rows() << std::endl;
    std::cout << "number of faces over all patches :  " << F.rows() << " number of faces after stitching : " << stitchedF.rows() << std::endl;
}

void testStitchMeshes()
{
   /* Eigen::MatrixXd V1(8,3);
    Eigen::MatrixXi F1(4,3);
    Eigen::MatrixXd nV1;
    Eigen::MatrixXi nF1;
    V1.setZero();
    V1.row(0) << 0, 0, 0;
    V1.row(1) << 1, 0, 0;
    V1.row(2) << 0, 1, 0;
    V1.row(3) << 1, 1, 0;
    Eigen::RowVector3d shift;
    shift << -1, 0 ,0;
    for (int j = 0; j < 4; j++)
        V1.row(j+4) = V1.row(j) + shift;
    F1 << 0, 1, 3, 0, 3, 2, 4, 5, 7, 4, 7, 6;
    SimulationSetup setup1;
    MidedgeAverageFormulation sff;
    MeshConnectivity mesh1(F1);
    setup1.buildRestFundamentalForms(mesh1, V1, sff);
    stitchMeshes(V1,F1,nV1,nF1,setup1);

    Eigen::MatrixXd nV2;
    Eigen::MatrixXi nF2;
    SimulationSetup setup2;
    MeshConnectivity mesh2(nF1);
    MidedgeAverageFormulation sff2;
    setup2.buildRestFundamentalForms(mesh2, nV1, sff2);
    stitchMeshes(nV1,nF1,nV2,nF2,setup2);

    std::cout << "laplacian difference : " << (setup1.laplacian - setup2.laplacian).norm() << std::endl;
    std::cout << "laplace position : " << (setup1.laplacian * nV1).norm() << std::endl;*/

}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#include "UnfilteredIMU.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Utils/cv.h"
#include <boost/optional.hpp>

#include <arcana/containers/sorted_vector.h>
#include <arcana/analysis/object_trace.h>
#include <arcana/analysis/data_point.h>

namespace E = Eigen;

namespace mage
{
    using seconds = std::chrono::duration<double>;

    struct alignas(16) UnfilteredIMU::Impl
    {
        E::Matrix4d m_bodyIMUToBodyCamera = E::Matrix4d::Identity();
        E::Matrix3d m_bodyIMUToBodyCameraRotationOnly = E::Matrix3d::Identity();
        E::Matrix4d m_bodyCameraToBodyIMU = E::Matrix4d::Identity();

        mage::SensorSample::Timestamp m_gyroTime{};
        mage::SensorSample::Timestamp m_accelTime{};

        mira::sorted_vector<mage::SensorSample, mage::SensorSample::Compare> m_samples;

        mage::SensorSample::Timestamp m_previousTs{};
        mage::Pose m_previousPose;

        Eigen::Vector3d Gravity = Eigen::Vector3d::Zero();

        Eigen::Vector3d Acceleration = Eigen::Vector3d::Zero();
        Eigen::Vector3d Velocity = Eigen::Vector3d::Zero();
        Eigen::Vector3d Position = Eigen::Vector3d::Zero();

        E::AngleAxisd AngularVelocity = E::AngleAxisd::Identity();
        Eigen::Quaterniond Orientation = Eigen::Quaterniond::Identity();

        void ProcessGyroUntil(mage::SensorSample::Timestamp ts)
        {
            if (m_gyroTime == mage::SensorSample::Timestamp{})
                m_gyroTime = ts;

            seconds dt = ts - m_gyroTime;

            E::AngleAxisd av = AngularVelocity;
            av.angle() *= dt.count();

            Orientation = Orientation * av;
            Orientation.normalize();

            m_gyroTime = ts;
        }

        void SetAngularVelocity(Eigen::Map<const Eigen::Vector3f> value)
        {
            Eigen::Vector3f angularVelocityInCameraSpace{
                value[1],
                value[0],
                -value[2]
            };
            AngularVelocity = E::AngleAxisd(mira::deg2rad<double>(angularVelocityInCameraSpace.x()), E::Vector3d::UnitX())
                * E::AngleAxisd(mira::deg2rad<double>(angularVelocityInCameraSpace.y()), E::Vector3d::UnitY())
                * E::AngleAxisd(mira::deg2rad<double>(angularVelocityInCameraSpace.z()), E::Vector3d::UnitZ());
        }

        void ProcessAccelerationUntil(mage::SensorSample::Timestamp ts)
        {
            if (m_accelTime == mage::SensorSample::Timestamp{})
                m_accelTime = ts;

            seconds dt = ts - m_accelTime;

            E::Vector3d vi = Velocity;

            Velocity += dt.count() * (Acceleration - Gravity);
            Position += 0.5 * (Velocity + vi) * dt.count();

            FIRE_OBJECT_TRACE("IMU Linear Velocity.UnfilteredIMU", this, (mira::make_data_point<float>(
                ts,
                (float)Velocity.norm()
            )));

            m_accelTime = ts;
        }

        void SetLinearAcceleration(Eigen::Map<const Eigen::Vector3f> value)
        {
            Eigen::Vector3f sensorAccelInCameraSpace{
                value[1],
                value[0],
                -value[2]
            };

            Acceleration = Orientation * sensorAccelInCameraSpace.cast<double>();

            Gravity = 0.9 * Gravity + 0.1 * Acceleration;
        }

        void ProcessPose(mage::SensorSample::Timestamp ts, const mage::Pose& pose)
        {
            assert(ts >= m_previousTs);

            PredictUpTo(ts);

            auto transform = Decompose(pose.GetInverseViewMatrix());

            Orientation = transform.second.cast<double>();
            Position = ToMap(transform.first).cast<double>();

            if (m_previousTs == mage::SensorSample::Timestamp{})
            {
                Velocity = E::Vector3d::Zero();
                AngularVelocity = E::AngleAxisd::Identity();
            }
            else
            {
                seconds dt = ts - m_previousTs;
                auto previous = Decompose(m_previousPose.GetInverseViewMatrix());

                Velocity = (Position - ToMap(previous.first).cast<double>()) / dt.count();

                AngularVelocity = (transform.second * previous.second.inverse()).cast<double>();
                AngularVelocity.angle() /= dt.count();
            }

            m_previousPose = pose;
            m_previousTs = ts;
        }

        void PredictUpTo(mage::SensorSample::Timestamp timestamp)
        {
            auto from = m_samples.begin();
            auto to = std::find_if(from, m_samples.end(), [timestamp](const mage::SensorSample& sample)
            {
                return sample.GetTimestamp() > timestamp;
            });

            for (auto itr = from; itr != to; itr++)
            {
                if (itr->GetType() == mage::SensorSample::SampleType::Gyrometer)
                {
                    ProcessGyroUntil(itr->GetTimestamp());
                    SetAngularVelocity({ itr->GetData().data(), 3 });
                }
                else if (itr->GetType() == mage::SensorSample::SampleType::Accelerometer)
                {
                    ProcessAccelerationUntil(itr->GetTimestamp());
                    SetLinearAcceleration({ itr->GetData().data(), 3 });
                }
            }

            m_samples.erase(from, to);

            ProcessGyroUntil(timestamp);
            ProcessAccelerationUntil(timestamp);
        }
    };

    UnfilteredIMU::UnfilteredIMU(const device::IMUCharacterization& imuCharacterization)
        : m_impl{ std::allocate_shared<Impl, Eigen::aligned_allocator<Impl>>({}) }
    {
        m_impl->m_bodyIMUToBodyCamera = E::Map<const E::Matrix4f>(imuCharacterization.BodyIMUToBodyCamera.data(), 4, 4).cast<double>();
        m_impl->m_bodyIMUToBodyCameraRotationOnly = m_impl->m_bodyIMUToBodyCamera.block<3, 3>(0, 0);
        m_impl->m_bodyCameraToBodyIMU = E::Map<const E::Matrix4f>(imuCharacterization.BodyCameraToBodyIMU.data(), 4, 4).cast<double>();
    }

    UnfilteredIMU::~UnfilteredIMU() = default;

    void UnfilteredIMU::AddSample(const mage::SensorSample& sample)
    {
        if (sample.GetType() == mage::SensorSample::SampleType::Gyrometer ||
            sample.GetType() == mage::SensorSample::SampleType::Accelerometer)
        {
            m_impl->m_samples.insert(sample);
        }
    }

    mage::Pose UnfilteredIMU::GetPose() const
    {
        Eigen::Vector3f position{ m_impl->Position.cast<float>() };
        return mage::Pose(FromQuatAndTrans(m_impl->Orientation.cast<float>(), { position.x(), position.y(), position.z() }));
    }

    void UnfilteredIMU::PredictUpTo(mage::SensorSample::Timestamp timestamp)
    {
        m_impl->PredictUpTo(timestamp);
    }

    void UnfilteredIMU::AddPose(const mage::Pose& pose, mage::SensorSample::Timestamp timestamp)
    {
        m_impl->ProcessPose(timestamp, pose);
    }
}