//
//  ClosedFormSVDSVD2d.hpp
//  IPC
//
//  Created by Minchen Li on 8/31/18.
//  based on https://www.researchgate.net/publication/263580188_Closed_Form_SVD_Solutions_for_2_x_2_Matrices_-_Rev_2
//

#ifndef ClosedFormSVD2d_hpp
#define ClosedFormSVD2d_hpp

#include <Eigen/Eigen>

#include <iostream>

namespace IPC {

template <typename MatrixType>
class AutoFlipSVD : Eigen::JacobiSVD<MatrixType> {
    typedef Eigen::JacobiSVD<MatrixType> Base;

public:
    AutoFlipSVD(void) {}
    AutoFlipSVD(const MatrixType& mtr, unsigned int computationOptions = 0)
        : Base(2, 2, computationOptions)
    {
        if (MatrixType::RowsAtCompileTime == Eigen::Dynamic) {
            assert(mtr.rows() == 2);
        }
        else {
            assert(MatrixType::RowsAtCompileTime == 2);
        }

        if (MatrixType::ColsAtCompileTime == Eigen::Dynamic) {
            assert(mtr.cols() == 2);
        }
        else {
            assert(MatrixType::ColsAtCompileTime == 2);
        }

        Base::m_isInitialized = true;

        compute(mtr);
    }

public:
    AutoFlipSVD& compute(const MatrixType& A)
    {
        bool computeU = (Base::m_computeFullU || Base::m_computeThinU);
        bool computeV = (Base::m_computeFullV || Base::m_computeThinV);

        const double a = A(0, 0);
        const double b = A(1, 0);
        const double c = A(0, 1);
        const double d = A(1, 1);

        const double ad = a * d;
        const double bc = b * c;
        const double _2admbc = 2.0 * (ad - bc);
        const double sqn = A.squaredNorm();

        const double sum = sqn + _2admbc;
        const double dif = sqn - _2admbc;
        if (dif <= 0.0) {
            // avoid dividing by 0 in general formula
            const double aa = (a + d) / 2.0;
            const double bb = (b - c) / 2.0;
            const double lambda = std::sqrt(aa * aa + bb * bb);
            Base::m_singularValues.setConstant(lambda);

            if (computeU) {
                if (lambda == 0.0) {
                    // avoid dividing by 0
                    Base::m_matrixU.setIdentity();
                }
                else {
                    const double cosl = aa / lambda;
                    const double sinl = bb / lambda;
                    Base::m_matrixU << cosl, -sinl, sinl, cosl;
                }
            }

            if (computeV) {
                Base::m_matrixV.setIdentity();
            }
        }
        else if (sum <= 0.0) {
            // avoid dividing by 0 in general formula
            // symmetric matrix with a=-d
            const double aa = (a - d) / 2.0;
            const double bb = (b + c) / 2.0;
            const double lambda = std::sqrt(aa * aa + bb * bb);
            Base::m_singularValues << lambda, -lambda;

            if (computeU || computeV) {
                if (bb == 0.0) {
                    // avoid dividing by 0 and sqrt(<0)
                    if (computeU) {
                        Base::m_matrixU.setIdentity();
                    }

                    if (computeV) {
                        Base::m_matrixV.setIdentity();
                    }
                }
                else {
                    const double a_div_lambda_half = aa / lambda / 2.0;
                    bool neg_b = (bb < 0.0);
                    const double cos2 = 0.5 + a_div_lambda_half;
                    const double cos = ((cos2 <= 0.0) ? 0.0 : std::sqrt(cos2));
                    const double sin2 = 0.5 - a_div_lambda_half;
                    const double sin = ((sin2 <= 0.0) ? 0.0 : (neg_b ? -std::sqrt(sin2) : std::sqrt(sin2)));
                    if (computeU) {
                        Base::m_matrixU << cos, -sin, sin, cos;
                    }

                    if (computeV) {
                        Base::m_matrixV << cos, -sin, sin, cos;
                    }
                }
            }
        }
        else {
            const double sqrt_sum = std::sqrt(sum); // safe
            const double sqrt_dif = std::sqrt(dif); // safe

            Base::m_singularValues[0] = (sqrt_sum + sqrt_dif) / 2.0;
            Base::m_singularValues[1] = ((_2admbc < 0.0) ? (-std::abs(sqrt_sum - sqrt_dif) / 2.0) : (std::abs(sqrt_sum - sqrt_dif) / 2.0));

            if (computeU || computeV) {
                const double a2 = a * a;
                const double b2 = b * b;
                const double c2 = c * c;
                const double d2 = d * d;

                const double denom = sqrt_sum * sqrt_dif * 2.0;

                const double a2md2 = a2 - d2;
                const double b2mc2 = b2 - c2;

                const double ab = a * b;
                const double cd = c * d;
                const bool neg_ab_p_cd = ((ab + cd) < 0);

                if (computeU) {
                    const double a2md2_m_b2mc2_div_ = (a2md2 - b2mc2) / denom; // safe

                    // avoid sqrt(<0)
                    const double cosl2 = 0.5 + a2md2_m_b2mc2_div_;
                    const double cosl = ((cosl2 <= 0.0) ? 0.0 : std::sqrt(cosl2));
                    const double sinl2 = 0.5 - a2md2_m_b2mc2_div_;
                    const double sinl = ((sinl2 <= 0.0) ? 0.0 : (neg_ab_p_cd ? -std::sqrt(sinl2) : std::sqrt(sinl2)));

                    Base::m_matrixU << cosl, -sinl, sinl, cosl;
                }

                if (computeV) {
                    const double ac = a * c;
                    const double bd = b * d;
                    const bool neg_ac_p_bd = ((ac + bd) < 0);
                    const double a2md2_p_b2mc2_div_ = (a2md2 + b2mc2) / denom; // safe

                    // avoid sqrt(<0)
                    const double cosr2 = 0.5 + a2md2_p_b2mc2_div_;
                    const double cosr = ((cosr2 <= 0.0) ? 0.0 : std::sqrt(cosr2));
                    const double sinr2 = 0.5 - a2md2_p_b2mc2_div_;
                    const double sinr = ((sinr2 <= 0.0) ? 0.0 : (neg_ac_p_bd ? -std::sqrt(sinr2) : std::sqrt(sinr2)));

                    const bool s = neg_ab_p_cd ^ neg_ac_p_bd;
                    const bool neg_apsd = ((a + (s ? -d : d)) < 0.0);
                    if (neg_apsd) {
                        Base::m_matrixV << -cosr, sinr, -sinr, -cosr;
                    }
                    else {
                        Base::m_matrixV << cosr, -sinr, sinr, cosr;
                    }
                }
            }
        }

        return *this;
    }
    AutoFlipSVD& compute(const MatrixType& mtr, unsigned int computationOptions)
    {
        if (MatrixType::RowsAtCompileTime == Eigen::Dynamic) {
            assert(mtr.rows() == 2);
        }
        else {
            assert(MatrixType::RowsAtCompileTime == 2);
        }

        if (MatrixType::ColsAtCompileTime == Eigen::Dynamic) {
            assert(mtr.cols() == 2);
        }
        else {
            assert(MatrixType::ColsAtCompileTime == 2);
        }

        allocate(computationOptions);
        Base::m_isInitialized = true;

        compute(mtr);

        return *this;
    }

protected:
    void allocate(unsigned int computationOptions)
    {
        if (Base::m_isAllocated && 2 == Base::m_rows && 2 == Base::m_cols && computationOptions == Base::m_computationOptions) {
            return;
        }

        Base::m_rows = 2;
        Base::m_cols = 2;
        Base::m_isInitialized = false;
        Base::m_isAllocated = true;
        Base::m_computationOptions = computationOptions;
        Base::m_computeFullU = (computationOptions & Eigen::ComputeFullU) != 0;
        Base::m_computeThinU = (computationOptions & Eigen::ComputeThinU) != 0;
        Base::m_computeFullV = (computationOptions & Eigen::ComputeFullV) != 0;
        Base::m_computeThinV = (computationOptions & Eigen::ComputeThinV) != 0;
        eigen_assert(!(Base::m_computeFullU && Base::m_computeThinU) && "JacobiSVD: you can't ask for both full and thin U");
        eigen_assert(!(Base::m_computeFullV && Base::m_computeThinV) && "JacobiSVD: you can't ask for both full and thin V");
        eigen_assert(EIGEN_IMPLIES(Base::m_computeThinU || Base::m_computeThinV,
                         MatrixType::ColsAtCompileTime == Eigen::Dynamic)
            && "JacobiSVD: thin U and V are only available when your matrix has a dynamic number of columns.");

        Base::m_diagSize = 2;
        Base::m_singularValues.resize(2);
        Base::m_matrixU.resize(2, 2);
        Base::m_matrixV.resize(2, 2);
        Base::m_workMatrix.resize(2, 2);
    }

public:
    const typename Eigen::JacobiSVD<MatrixType>::SingularValuesType& singularValues(void) const
    {
        return Base::m_singularValues;
    }
    const MatrixType& matrixU(void) const
    {
        return Base::m_matrixU;
    }
    const MatrixType& matrixV(void) const
    {
        return Base::m_matrixV;
    }
};

} // namespace IPC

#endif /* ClosedFormSVD2d_hpp */


#include "stdafx.h"
#include "KinectDistortionModel.h"
#include <Eigen\Dense>

using namespace DirectX;
using namespace Eigen;

namespace k4u
{
  float ApplyMediaFoundationRadialDistortion(const DirectX::XMFLOAT3& coeffs, float r)
  {
    return 1 +
      coeffs.x * pow(r, 2.f) +
      coeffs.y * pow(r, 4.f) +
      coeffs.z * pow(r, 6.f);
  }

  float ApplyKinectRadialDistortion(const DirectX::XMFLOAT3& coeffsA, const DirectX::XMFLOAT3& coeffsB, float r)
  {
    auto a = ApplyMediaFoundationRadialDistortion(coeffsA, r);
    auto b = ApplyMediaFoundationRadialDistortion(coeffsB, r);
    auto bi = b == 0.f ? 1.f : 1.f / b;
    return a * bi;
  }

  DirectX::XMFLOAT3 ConvertKinectRadialDistortionToMediaFoundation(const k4a_calibration_camera_t& calibration)
  {
    const int sampleSize = 100;
    Matrix<float, sampleSize, 1> y1;
    Matrix<float, sampleSize, 3> x;

    auto& params = calibration.intrinsics.parameters.param;
    XMFLOAT3 coeffsA{ params.k1, params.k2, params.k3 };
    XMFLOAT3 coeffsB{ params.k4, params.k5, params.k6 };

    auto rStep = calibration.metric_radius / (sampleSize - 1);
    auto rCurrent = 0.f;
    for (auto i = 0; i < sampleSize; i++)
    {
      auto r = rCurrent;
      y1(i) = ApplyKinectRadialDistortion(coeffsA, coeffsB, r) - 1;

      auto r2 = r * r;
      auto r4 = r2 * r2;
      auto r6 = r4 * r2;

      x(i, 0) = r2;
      x(i, 1) = r4;
      x(i, 2) = r6;
      rCurrent += rStep;
    }

    auto k = x.fullPivHouseholderQr().solve(y1);
    return { k(0), k(1), k(2) };
  }
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_TANH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TANH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing tanh capabilities

    Returns the hyperbolic tangent: \f$\frac{\sinh(x)}{\cosh(x)}\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = tanh(x);
    @endcode

    is similar to:

    @code
    T r = sinh(x)/cosh(x);
    @endcode

  **/
  const boost::dispatch::functor<tag::tanh_> tanh = {};
} }
#endif

#include <boost/simd/function/scalar/tanh.hpp>
#include <boost/simd/function/simd/tanh.hpp>

#endif


#include "partitioning_1d.h"
#include <boost/assert.hpp>

/*
 * get the first and last elements within a partition
 * assuming 0 <= part_id < partinfo._num_partitions
*/
Partition1D get_partition(const int part_id, const PartitioningInfo& partinfo) {
	    Partition1D part;
		BOOST_ASSERT_MSG(part_id>=0, "negative partition id specified");
		BOOST_ASSERT_MSG(part_id < partinfo._num_partitions, "requested partition with higher id than existing partitions");

		// compute first element
		int overhead = part_id - partinfo._num_partitions_full_load;
		if (overhead >0) {
			    part._first_id= part_id * partinfo._size_full_load - overhead;
		}
		else {
			    part._first_id= part_id *partinfo._size_full_load;
		}

		// compute last element
		if (part_id < partinfo._num_partitions_full_load) {
			    part._last_id = part._first_id + partinfo._size_full_load-1; // -1 as last element is inclusive in the range
		}
		else {
			    part._last_id = part._first_id + partinfo._size_full_load-2; // -2 as last element is inclusive and the partition is of reduced (-1) size
		}

		return part;
}

PartitioningInfo split_work(const int num_partitions, const int ids_to_process) {
	return {
		._num_partitions = num_partitions,
		._num_partitions_full_load = ids_to_process - ((ids_to_process/num_partitions) * num_partitions),
		._size_full_load = (ids_to_process/num_partitions) + 1
	};
}

int get_workload(const int part_id, const PartitioningInfo& partinfo) {
	if (part_id < partinfo._num_partitions_full_load) {
		return partinfo._size_full_load;
	}
	else {
		return partinfo._size_full_load-1;
	}
}


#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <random>
#include <iostream>
#include <vector>
#include <set>

#include <igl/cotmatrix.h>
#include <igl/boundary_loop.h>

#include "ElasticShellMaterial.h"
#include "../MeshConnectivity.h"
#include "../GeometryDerivatives.h"

double ElasticShellMaterial::cotan_v0(const Eigen::Vector3d v0, const Eigen::Vector3d v1, const Eigen::Vector3d v2)
{
    double e0 = (v2 - v1).norm();
    double e1 = (v2 - v0).norm();
    double e2 = (v0 - v1).norm();
    double angle0 = acos((e1 * e1 + e2 * e2 - e0 * e0) / (2 * e1 * e2));
    double cot = 1.0 / tan(angle0);

    return cot;
}


#include "utils.h"
#include "soft_clustering.h"
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>

using namespace std;

Hyperparam::Hyperparam(AdaCluster& mdl_)
    : copt::BoundedProblem<double>(mdl_.get_n_dims()),
      mdl(mdl_),
      k(mdl_.get_k()),
      n_samples(mdl_.get_n_samples()),
      n_dims(mdl_.get_n_dims()),
      kappa(mdl_.get_kappa()),
      log_pi(mdl_.get_log_pi()),
      data(mdl_.get_data()),
      log_asg(mdl_.get_log_asg()),
      mu(mdl_.get_mu()) {
  buffer.resize(k);
}

double Hyperparam::value(const copt::Vector<double>& x) {
  // x = alpha
  double obj = 0.0;
  for (size_t i = 0; i < n_samples; ++i) {
    for (size_t c = 0; c < k; ++c) {
      buffer[c] = log_pi[c];
      for (unsigned long int j = 0; j < n_dims; ++j){
        if (!isnan(data[i][j])){
          double dist = mdl.distance(data[i][j], mu[c][j], x[j], j) / kappa[j];
          double var = mdl.variance(data[i][j], x[j], j);
          double upd = dist + 0.5 * (log(2 * M_PI) + log(kappa[j]) + log(var));
          // if (isnan(buffer[c])){
          //   cout << "buffer nan is detected ";
          //   cout << m_lowerBound[j] << " " << m_upperBound[j] << " ";
          //   cout << data[i][j] << " " << mu[c][j] << " " << x[j] << " ";
          //   cout << mdl.distance(data[i][j], mu[c][j], x[j], j) / kappa[j] << " ";
          //   cout << mdl.variance(data[i][j], x[j], j) << endl;
          //   break;
          // }
          if (!isnan(upd))
            buffer[c] -= upd;
        }
      }
    }
    obj += logsumexp(buffer);
  }
  return -obj;
}

void Hyperparam::gradient(const copt::Vector<double>& x,
                          copt::Vector<double>& grad) {
  bool terminate = 0;
  double gr = 0.0;
  for (unsigned long int j = 0; j < n_dims; ++j) {
    terminate = 0;
    gr = 0.0;
    for (size_t i = 0; i < n_samples; ++i){
      for (size_t c = 0; c < k; ++c)
        if (!isnan(data[i][j])){
          double diff_dist = mdl.diff_distance(data[i][j], mu[c][j], x[j], j) / kappa[j];
          double diff_var = mdl.diff_variance(data[i][j], x[j], j);
          double var = mdl.variance(data[i][j], x[j], j);
          double pr_asg = exp(log_asg[i][c]);
          double upd = 0.0;
          if (diff_var == 0.0)
            upd = pr_asg * diff_dist;
          else
            upd = pr_asg * (diff_dist + 0.5 * diff_var / var);
          gr += upd;
          // if (isnan(gr)){
          //   cout << "grad nan is detected";
          //   cout << m_lowerBound[j] << " " << m_upperBound[j] << " ";
          //   cout << data[i][j] << " " << mu[c][j] << " " << x[j] << " ";
          //   cout << mdl.diff_distance(data[i][j], mu[c][j], x[j], j) / kappa[j] << " ";
          //   cout << mdl.diff_variance(data[i][j], x[j], j) << " ";
          //   cout << mdl.variance(data[i][j], x[j], j) << endl;
          //   break;
          // }
          if (isnan(upd)){
            terminate = 1;
            break;
          }
        }
      if (terminate)
        break;
    }
    if (terminate)
      grad[j] = 0;
    else
      grad[j] = gr;
  }
}

AdaCluster::AdaCluster(const vector<vector<double>>& data_,
                       const vector<unsigned long int>& label_,
                       unsigned long int max_round_, unsigned long int k_)
    : data(data_), label(label_) {
  max_round = max_round_;
  k = k_;
  nmis.resize(max_round);
  fill(nmis.begin(), nmis.end(), 0);
  logliks.resize(max_round);
  fill(logliks.begin(), logliks.end(), -numeric_limits<double>::max());
  n_samples = data.size();
  n_dims = data[0].size();
  log_pi.resize(k);
  fill(log_pi.begin(), log_pi.end(), -log(k));
  log_asg_sum.resize(k);
  max_log_pi.resize(k);
  kappa.resize(n_dims);
  kappa_a.resize(n_dims);
  kappa_b.resize(n_dims);
  mu.resize(k);
  mu_a.resize(k);
  mu_b.resize(k);
  for (size_t c = 0; c < k; ++c){
    mu[c].resize(n_dims);
    mu_a[c].resize(n_dims);
    mu_b[c].resize(n_dims);
  }
  log_asg.resize(n_samples);
  asg.resize(n_samples);
  for (size_t i = 0; i < n_samples; ++i) log_asg[i].resize(k);
  attr_discrete.resize(n_dims);
  attr_positive.resize(n_dims);
  attr_nonnegative.resize(n_dims);

  alpha.resize(n_dims);
  lb.resize(n_dims);
  ub.resize(n_dims);
  for (unsigned long int j = 0; j < n_dims; ++j) {
    attr_discrete[j] = is_discrete(data, j);
    attr_positive[j] = is_positive(data, j);
    attr_nonnegative[j] = attr_positive[j];
    if (!attr_positive[j]) attr_nonnegative[j] = is_nonnegative(data, j);
  }
  for (unsigned long int j = 0; j < n_dims; ++j) {
    if (attr_discrete[j]) {
      if (attr_nonnegative[j]) {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      } else {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      }
    } else {
      if (attr_positive[j]) {
        alpha[j] = 0;
        // lb[j] = -numeric_limits<double>::max();
        lb[j] = -10;
        ub[j] = 2;
      } else if (attr_nonnegative[j]) {
        alpha[j] = 0.5;
        lb[j] = 0;
        ub[j] = 1;
      } else {
        alpha[j] = 1;
        lb[j] = 0;
        // ub[j] = numeric_limits<double>::max();
        ub[j] = 10;
      }
    }
  }
}

double AdaCluster::distance(const double x,
                            const double y,
                            const double alpha,
                            const unsigned long int dim) {
  // return nnc_distance(x, y, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_distance(x, y, alpha);
    else
      return rc_distance(x, y, alpha);
  } else{
    if (attr_nonnegative[dim])
      return nnc_distance(x, y, alpha);
    else
      return rc_distance(x, y, alpha);
  }
}

double AdaCluster::variance(const double x,
                            const double alpha,
                            const unsigned long int dim){
  // return nnc_variance(x, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_variance(x, alpha);
    else
      return rc_variance(x, alpha);
  } else {
    if (attr_nonnegative[dim])
      return nnc_variance(x, alpha);
    else
      return rc_variance(x, alpha);
  }
}

double AdaCluster::diff_distance(const double x,
                                 const double y,
                                 const double alpha,
                                 const unsigned long int dim) {
  // return nnc_diff_distance(x, y, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_diff_distance(x, y, alpha);
    else
      return rc_diff_distance(x, y, alpha);
  } else {
    if (attr_nonnegative[dim])
      return nnc_diff_distance(x, y, alpha);
    else
      return rc_diff_distance(x, y, alpha);
  }
}

double AdaCluster::diff_variance(const double x,
                                 const double alpha,
                                 const unsigned long int dim) {
  // return nnc_diff_variance(x, alpha);
  if (attr_discrete[dim]){
    if (attr_nonnegative[dim])
      return nnd_diff_variance(x, alpha);
    else
      return rc_diff_variance(x, alpha);
  } else {
    if (attr_nonnegative[dim])
      return nnc_diff_variance(x, alpha);
    else
      return rc_diff_variance(x, alpha);
  }
}

unsigned long int AdaCluster::get_k() { return k; }

unsigned long int AdaCluster::get_max_round() { return max_round; }

unsigned long int AdaCluster::get_n_dims() { return n_dims; }

unsigned long int AdaCluster::get_n_samples() { return n_samples; }

vector<double>& AdaCluster::get_kappa() { return kappa; }

vector<double>& AdaCluster::get_log_pi() { return log_pi; }

vector<double>& AdaCluster::get_logliks() { return logliks; }

vector<double>& AdaCluster::get_nmis() { return nmis; }

copt::Vector<double>& AdaCluster::get_alpha() { return alpha; }

copt::Vector<double>& AdaCluster::get_lb() { return lb; }

copt::Vector<double>& AdaCluster::get_ub() { return ub; }

const vector<vector<double>>& AdaCluster::get_data() { return data; }

vector<vector<double>>& AdaCluster::get_log_asg() { return log_asg; }

vector<vector<double>>& AdaCluster::get_mu() { return mu; }

void AdaCluster::initialize_random() {
  unsigned long int ri;
  for (size_t c = 0; c < k; ++c) {
    ri = rand() % n_samples;
    for (unsigned long int j = 0; j < n_dims; ++j){
      mu[c][j] = data[ri][j];
      mu_a[c][j] = mu[c][j];
      mu_b[c][j] = 1.0;
    }
  }
  for (unsigned long int j = 0; j < n_dims; ++j){
    kappa[j] = 1.0;
    kappa_a[j] = 1.0;
    kappa_b[j] = 1e-9;
  }
}

void AdaCluster::initialize_k_plus_plus() {
  boost::mt19937 gen;
  unsigned long int ri, k_eff;
  ri = rand() % n_samples;
  for (unsigned long int j = 0; j < n_dims; ++j) mu[0][j] = data[ri][j];
  k_eff = 1;
  vector<double> probs(n_samples, 0);
  double dist, best_dist;
  do {
    for (size_t i = 0; i < n_samples; ++i) {
      best_dist = numeric_limits<double>::max();
      for (size_t c = 0; c < k_eff; ++c) {
        dist = 0;
        for (unsigned long int j = 0; j < n_dims; ++j){
          // dist += distance(data[i][j], mu[c][j], alpha[j], j);
          dist += pow(data[i][j] - mu[c][j], 2);
        }
        if (dist < best_dist) best_dist = dist;
      }
      probs[i] = pow(best_dist, 2);
    }
    boost::random::discrete_distribution<> dist(probs.begin(), probs.end());
    ri = dist(gen);
    for (unsigned long int j = 0; j < n_dims; ++j) mu[k_eff][j] = data[ri][j];
    k_eff += 1;
  } while (k_eff != k);

  for (unsigned long int j = 0; j < n_dims; ++j)
    if (attr_nonnegative[j])
      for (size_t c = 0; c < k_eff; ++c)
        if (mu[c][j] == 0)
          mu[c][j] = 1e-9;
  for (unsigned long int j = 0; j < n_dims; ++j){
    kappa[j] = 1.0;
    kappa_a[j] = 1.0;
    kappa_b[j] = 1.0;
  }
  for (size_t c = 0; c < k_eff; ++c)
    for (unsigned long int j = 0; j < n_dims; ++j){
      mu_a[c][j] = mu[c][j];
      mu_b[c][j] = 1.0;
    }
}

void AdaCluster::fit() {
  bool updated = 0;
  double loglik = 0;
  double nmi = 0;
  double dist = 0;
  double best_dist = 0;
  unsigned long int best_asg = -1;
  Hyperparam prblm(*this);
  prblm.setLowerBound(lb);
  prblm.setUpperBound(ub);
  for (size_t r = 0; r < max_round; r++) {
    updated = 0;
    loglik = 0.0;
    fill(max_log_pi.begin(), max_log_pi.end(), -numeric_limits<double>::max());
    for (size_t i = 0; i < n_samples; ++i) {
      for (size_t c = 0; c < k; ++c) {
        log_asg[i][c] = log_pi[c];
        for (unsigned long int j = 0; j < n_dims; ++j)
          if (!isnan(data[i][j])){
            double dist = distance(data[i][j], mu[c][j], alpha[j], j) / kappa[j];
            double var = variance(data[i][j], alpha[j], j);
            log_asg[i][c] -= dist + 0.5 * (log(2 * M_PI) + log(kappa[j]) + log(var));
          }
      }
      double norm = logsumexp(log_asg[i]);
      for (size_t c = 0; c < k; ++c) log_asg[i][c] -= norm;
      loglik += norm;
      for (size_t c = 0; c < k; ++c)
        if (log_asg[i][c] > max_log_pi[c]) max_log_pi[c] = log_asg[i][c];
    }
    for (size_t c = 0; c < k; ++c) {
      log_asg_sum[c] = 0.0;
      for (size_t i = 0; i < n_samples; ++i)
        log_asg_sum[c] += exp(log_asg[i][c] - max_log_pi[c]);
      log_asg_sum[c] = log(log_asg_sum[c]) + max_log_pi[c];
      log_pi[c] = log_asg_sum[c] - log(n_samples);
    }

    for (size_t c = 0; c < k; c++)
      for (unsigned long int j = 0; j < n_dims; ++j) {
        double tmp = 0.0;
        for (size_t i = 0; i < n_samples; ++i)
          if (!isnan(data[i][j]))
            tmp += exp(log_asg[i][c])*data[i][j];
        mu[c][j] = (kappa[j]*mu_a[c][j]*mu_b[c][j] + tmp
          )/(kappa[j]*mu_b[c][j] + exp(log_asg_sum[c]));
        // vector<double> pos(n_samples, 0);
        // vector<double> neg(n_samples, 0);
        // double zero_count = 0;
        // for (size_t i = 0; i < n_samples; ++i){
        //   if (data[i][j] > 0)
        //     pos[i] = log_asg[i][c] + log(data[i][j]);
        //   else if (data[i][j] < 0)
        //     neg[i] = log_asg[i][c] + log(-data[i][j]);
        //   else
        //     zero_count += 1;
        // }
        // mu[c][j] = (kappa[j]*mu_a[c][j] + exp(logsumexp(pos)) - exp(logsumexp(neg))
        //             )/(kappa[j]*mu_b[c][j] + exp(log_asg_sum[c]));
      }

    double pr_asg = 0.0;
    double nom = 0.0;
    double denom = 0.0;
    for (unsigned long int j = 0; j < n_dims; ++j) {
      nom = 0.0;
      denom = 0.0;
      for (size_t i = 0; i < n_samples; ++i)
        for (size_t c = 0; c < k; c++) {
          pr_asg = exp(log_asg[i][c]);
          if (!isnan(data[i][j])) {
            nom += pr_asg * distance(data[i][j], mu[c][j], alpha[j], j);
            denom += pr_asg;
          }
        }
      kappa[j] =  (kappa_b[j] + nom) / (kappa_a[j] + 0.5*denom);
    }

    copt::Vector<double> alpha_prev(alpha);
    // cout << alpha.transpose() << endl;
    copt::LbfgsbSolver<Hyperparam> solver;
    solver.minimize(prblm, alpha);
    // cout << alpha.transpose() << endl;

    for (unsigned long int j = 0; j < n_dims; ++j)
      if (isnan(alpha[j]))
        alpha[j] = alpha_prev[j];

    for (size_t i = 0; i < n_samples; ++i) {
      best_dist = log_asg[i][0];
      best_asg = 0;
      for (size_t c = 1; c < k; ++c)
        if (log_asg[i][c] > best_dist) {
          best_dist = log_asg[i][c];
          best_asg = c;
        }
      if (asg[i] != best_asg) {
        asg[i] = best_asg;
        updated = 1;
      }
    }
    nmi = calc_nmi(label, asg);
    nmis[r] = nmi;
    cout << "round=" << r << " nmi=" << nmi << " loglik=" << loglik << endl;
    if (!updated) break;
  }
  cout << "Alpha=";
  for (unsigned long int j = 0; j < n_dims; ++j)
      cout << alpha[j] << " ";
  cout << endl;
  cout << "Kappa=";
  for (unsigned long int j = 0; j < n_dims; ++j)
      cout << kappa[j] << " ";
  cout << endl;
  for (size_t c = 0; c < k; ++c){
    cout << "mu[" << c << "]=";
    for (unsigned long int j = 0; j < n_dims; ++j)
        cout << mu[c][j] << " ";
    cout << endl;
  }
}

// GMM::GMM(const vector<vector<double>>& data_,
//          const vector<unsigned long int>& label_, unsigned long int max_round_,
//          unsigned long int k_)
//     : AdaCluster(data_, label_, max_round_, k_) {}

// double GMM::log_base_measure(unsigned long int sample, unsigned long int dim) {
//   return -0.5 * log(M_PI) - 0.5 * log(kappa[dim]);
// }

// double GMM::distance(const double x, double y, unsigned long int dim) {
//   return pow(x - y, 2) / 2.0;
// }

// void GMM::update_hyperparams() {}

// BSC::BSC(const vector<vector<double>>& data_,
//          const vector<unsigned long int>& label_, unsigned long int max_round_,
//          unsigned long int k_)
//     : AdaCluster(data_, label_, max_round_, k_) {
//   beta.resize(n_dims);
//   for (unsigned long int j = 0; j < n_dims; ++j) beta[j] = 2.0;
//   sum_log_data.resize(n_dims);
//   fill(sum_log_data.begin(), sum_log_data.end(), 0);
//   for (unsigned long int j = 0; j < n_dims; ++j)
//     for (size_t i = 0; i < n_samples; ++i) sum_log_data[j] += log_data[i][j];
// }

// vector<double>& BSC::get_beta() { return beta; }

// double BSC::log_base_measure(unsigned long int sample, unsigned long int dim) {
//   return -0.5 * log(M_PI) - 0.5 * log(kappa[dim]) -
//          0.5 * (2.0 - beta[dim]) * log_data[sample][dim];
// }

// double BSC::distance(const double x, double y, unsigned long int dim) {
//   return beta_div(x, y, beta[dim]);
// }

// void BSC::update_hyperparams() {
//   vector<double> x(n_dims);
//   vector<double> lb(n_dims);
//   vector<double> ub(n_dims);
//   for (unsigned long int j = 0; j < n_dims; ++j) {
//     lb[j] = -numeric_limits<double>::max();
//     ub[j] = numeric_limits<double>::max();
//     // lb[j] = -5.0;
//     // ub[j] = 5.0;
//   }
//   Saddle saddle(data, log_data, log_asg, log_pi, mu, kappa);
//   saddle.setLowerBound(lb);
//   saddle.setUpperBound(ub);
//   copt::LbfgsbSolver<double> solver;
//   int max_trial = 1;
//   double estimate = 0.0;
//   bool success = 1;
//   for (int trial = 0; trial < max_trial; trial++) {
//     for (unsigned long int j = 0; j < n_dims; ++j) x[j] = beta[j];
//     solver.minimize(saddle, x);
//     success = 1;
//     for (unsigned long int j = 0; j < n_dims; ++j)
//       if (std::isnan(x[j]) || !std::isfinite(x[j])) {
//         success = 0;
//         break;
//       }
//     if (success) break;
//   }
//   for (unsigned long int j = 0; j < n_dims; ++j) {
//     beta[j] = x[j];
//     // cout << " beta[" << j << "]=" << beta[j];
//   }
//   // cout << endl;
// }


#include <cstdlib>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main (const int argc, const char** argv) {
    if (argc > 2) {
        using namespace boost::numeric::ublas;

        matrix<double> m(atoi(argv[1]), atoi(argv[2])); // build
        for (unsigned i = 0; i < m.size1(); i++)
            for (unsigned j = 0; j < m.size2(); j++)
                m(i, j) = 1.0 + i + j; // fill
        std::cout << m << std::endl; // print
        return EXIT_SUCCESS;
    }

    return EXIT_FAILURE;
}


#include "Nodal.h"
#include "Functions.h"
#include "quadrules/GaussJacobi.h"
#include "quadrules/IntervalQuadratureRule.h"
#include "util/Combinatorics.h"

#include <Eigen/LU>

#include <algorithm>
#include <cassert>
#include <iterator>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace tndm {

std::vector<double> LegendreGaussLobattoPoints(unsigned n, unsigned a, unsigned b) {
    assert(n >= 2);

    auto gjPoints = GaussJacobi(n - 2, a + 1, b + 1).points();
    std::vector<double> glPoints;
    glPoints.reserve(n);
    glPoints.push_back(-1.0);
    std::copy(gjPoints.rbegin(), gjPoints.rend(), std::back_inserter(glPoints));
    glPoints.push_back(1.0);
    return glPoints;
}

template <std::size_t D>
MatrixXd Vandermonde(unsigned degree, std::vector<std::array<double, D>> const& points) {
    assert(binom(degree + D, D) == points.size());

    MatrixXd vandermonde(points.size(), binom(degree + D, D));

    for (std::size_t i = 0; i < points.size(); ++i) {
        std::size_t bf = 0;
        for (auto j : AllIntegerSums<D>(degree)) {
            vandermonde(i, bf++) = DubinerP(j, points[i]);
        }
    }

    return vandermonde;
}

template <std::size_t D>
LebesgueFunction<D>::LebesgueFunction(unsigned degree,
                                      std::vector<std::array<double, D>> const& nodes)
    : degree(degree), phi(nodes.size()), L(nodes.size()) {
    auto vandermonde = Vandermonde(degree, nodes);
    vInvT = vandermonde.transpose().inverse();
}

template <std::size_t D> double LebesgueFunction<D>::operator()(std::array<double, D> const& xi) {
    std::size_t bf = 0;
    for (auto j : AllIntegerSums<D>(degree)) {
        phi(bf++) = DubinerP(j, xi);
    }
    assert(bf == vInvT.cols());
    L = vInvT * phi;
    return L.lpNorm<1>();
}

template MatrixXd Vandermonde<1u>(unsigned, std::vector<std::array<double, 1u>> const&);
template MatrixXd Vandermonde<2u>(unsigned, std::vector<std::array<double, 2u>> const&);
template MatrixXd Vandermonde<3u>(unsigned, std::vector<std::array<double, 3u>> const&);

template class LebesgueFunction<1u>;
template class LebesgueFunction<2u>;
template class LebesgueFunction<3u>;

} // namespace tndm


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * 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.                                            *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>

namespace br
{

/*!
 * \ingroup distances
 * \brief L2 distance computed using eigen.
 * \author Josh Klontz \cite jklontz
 */
class L2Distance : public UntrainableDistance
{
    Q_OBJECT

    float compare(const cv::Mat &a, const cv::Mat &b) const
    {
        const int size = a.rows * a.cols;
        Eigen::Map<Eigen::VectorXf> aMap((float*)a.data, size);
        Eigen::Map<Eigen::VectorXf> bMap((float*)b.data, size);
        return (aMap-bMap).squaredNorm();
    }
};

BR_REGISTER(Distance, L2Distance)

} // namespace br

#include "distance/L2.moc"


#include "sbs/common/primitive.h"

#include <Eigen/Geometry>

namespace sbs {
namespace common {

line_segment_t::line_segment_t(point_t const& p, point_t const& q) : p(p), q(q) {}

triangle_t::triangle_t(point_t const& a, point_t const& b, point_t const& c) : p_{a, b, c} {}

normal_t triangle_t::normal() const
{
    Eigen::Vector3d const ab = b() - a();
    Eigen::Vector3d const ac = c() - a();
    return ab.cross(ac).normalized();
}

double triangle_t::area() const
{
    return 0.5 * (b() - a()).cross(c() - a()).norm();
}

std::array<line_segment_t, 3u> triangle_t::edges() const
{
    return std::array<line_segment_t, 3u>{
        line_segment_t{p1(), p2()},
        line_segment_t{p2(), p3()},
        line_segment_t{p3(), p1()}};
}

std::array<point_t, 3u> const& triangle_t::nodes() const
{
    return p_;
}

std::array<point_t, 3u>& triangle_t::nodes()
{
    return p_;
}

point_t const& triangle_t::p1() const
{
    return p_[0];
}

point_t const& triangle_t::p2() const
{
    return p_[1];
}

point_t const& triangle_t::p3() const
{
    return p_[2];
}

point_t& triangle_t::p1()
{
    return p_[0];
}

point_t& triangle_t::p2()
{
    return p_[1];
}

point_t& triangle_t::p3()
{
    return p_[2];
}

point_t const& triangle_t::a() const
{
    return p1();
}

point_t const& triangle_t::b() const
{
    return p2();
}

point_t const& triangle_t::c() const
{
    return p3();
}

point_t& triangle_t::a()
{
    return p1();
}

point_t& triangle_t::b()
{
    return p2();
}

point_t& triangle_t::c()
{
    return p3();
}

ray_t::ray_t(point_t const& p, direction_t const& v) : p(p), v(v) {}

sphere_t::sphere_t(point_t const& center, double radius) : center(center), radius(radius) {}

sphere_t sphere_t::from(tetrahedron_t const& t)
{
    common::point_t const approx_barycenter =
        0.25 * t.p1() + 0.25 * t.p2() + 0.25 * t.p3() + 0.25 * t.p4();
    auto const d1     = (approx_barycenter - t.p1()).norm();
    auto const d2     = (approx_barycenter - t.p2()).norm();
    auto const d3     = (approx_barycenter - t.p3()).norm();
    auto const d4     = (approx_barycenter - t.p4()).norm();
    auto const radius = std::max({d1, d2, d3, d4});
    sphere_t const sphere{approx_barycenter, radius};
    return sphere;
}

sphere_t sphere_t::from(triangle_t const& t)
{
    common::point_t const approx_barycenter = 0.33 * t.p1() + 0.33 * t.p2() + 0.34 * t.p3();
    auto const d1                           = (approx_barycenter - t.p1()).norm();
    auto const d2                           = (approx_barycenter - t.p2()).norm();
    auto const d3                           = (approx_barycenter - t.p3()).norm();
    auto const radius                       = std::max({d1, d2, d3});
    sphere_t const sphere{approx_barycenter, radius};
    return sphere;
}

bool operator==(line_segment_t const& l1, line_segment_t const& l2)
{
    return l1.p.isApprox(l2.p) && l1.q.isApprox(l2.q);
}

bool operator!=(line_segment_t const& l1, line_segment_t const& l2)
{
    return !(l1 == l2);
}

line_segment_t operator+(line_segment_t const& l, vector3d_t const& t)
{
    return line_segment_t(l.p + t, l.q + t);
}

line_segment_t operator+(vector3d_t const& t, line_segment_t const& l)
{
    return l + t;
}

line_segment_t operator*(Eigen::Matrix3d const& R, line_segment_t const& l)
{
    return line_segment_t(R * l.p, R * l.q);
}

std::tuple<double, double, double>
barycentric_coordinates(point_t const& A, point_t const& B, point_t const& C, point_t const& p)
{
    Eigen::Vector3d const v0 = B - A;
    Eigen::Vector3d const v1 = C - A;
    Eigen::Vector3d const v2 = p - A;

    Eigen::Vector3d const AB = B - A;
    Eigen::Vector3d const AC = C - A;
    Eigen::Vector3d const AP = p - A;

    double const d00   = AB.dot(AB);
    double const d01   = AB.dot(AC);
    double const d11   = AC.dot(AC);
    double const d20   = AP.dot(AB);
    double const d21   = AP.dot(AC);
    double const denom = d00 * d11 - d01 * d01;
    double const v     = (d11 * d20 - d01 * d21) / denom;
    double const w     = (d00 * d21 - d01 * d20) / denom;
    double const u     = 1.0 - v - w;

    return std::make_tuple(u, v, w);
}

bool intersects(tetrahedron_t const& t1, tetrahedron_t const& t2)
{
    std::array<common::normal_t, 44u> separating_axis{};
    auto const& t1_triangles = t1.faces();
    auto const& t2_triangles = t2.faces();

    auto const face_axis_transform = [](common::triangle_t const& f) {
        return f.normal();
    };

    auto separating_axis_it = std::transform(
        t1_triangles.begin(),
        t1_triangles.end(),
        separating_axis.begin(),
        face_axis_transform);
    separating_axis_it = std::transform(
        t2_triangles.begin(),
        t2_triangles.end(),
        separating_axis_it,
        face_axis_transform);

    auto const t1_edges = t1.edges();
    auto const t2_edges = t2.edges();

    for (auto const& edge : t1_edges)
    {
        auto const edge_edge_separating_axis_transform_op =
            [&edge](common::line_segment_t const& e) {
                Eigen::Vector3d const d1 = edge.q - edge.p;
                Eigen::Vector3d d2       = e.q - e.p;

                auto axis = d1.cross(d2);

                double constexpr eps = std::numeric_limits<double>::epsilon();
                // Check if edges are parallel up to numerical precision eps
                if (axis.isZero(eps))
                {
                    d2   = e.p - edge.p;
                    axis = d1.cross(d2);
                }
                // Check if edges are on the same line up to numerical precision eps
                if (axis.isZero(eps))
                {
                    // Cancel this axis as a potential separating axis. When projecting
                    // nodes onto this axis, everything will be zeroed out, and thus no
                    // separating interval can be found. The separating axis test for
                    // this axis will fail.
                    axis.setZero();
                }

                return axis;
            };
        separating_axis_it = std::transform(
            t2_edges.begin(),
            t2_edges.end(),
            separating_axis_it,
            edge_edge_separating_axis_transform_op);
    }

    auto const is_separating_axis = [&t1, &t2](common::normal_t const& axis) {
        if (axis.isZero())
            return false;

        auto const project = [axis](common::point_t const& p) {
            return p.dot(axis);
        };

        std::array<double, 4u> const projection1{
            project(t1.p1()),
            project(t1.p2()),
            project(t1.p3()),
            project(t1.p4())};

        std::array<double, 4u> const projection2{
            project(t2.p1()),
            project(t2.p2()),
            project(t2.p3()),
            project(t2.p4())};

        auto const [min_it1, max_it1] = std::minmax_element(projection1.begin(), projection1.end());
        auto const [min_it2, max_it2] = std::minmax_element(projection2.begin(), projection2.end());

        bool const has_separating_interval = (*max_it1 < *min_it2) || (*max_it2 < *min_it1);
        return has_separating_interval;
    };

    return std::none_of(separating_axis.begin(), separating_axis.end(), is_separating_axis);
}

bool intersects(triangle_t const& triangle, tetrahedron_t const& tetrahedron)
{
    std::array<common::normal_t, 23u> separating_axis{};
    auto const& tet_triangles = tetrahedron.faces();

    auto const face_axis_transform = [](common::triangle_t const& f) {
        return f.normal();
    };

    separating_axis.front() = triangle.normal();

    auto separating_axis_it = separating_axis.begin() + 1u;
    separating_axis_it      = std::transform(
        tet_triangles.begin(),
        tet_triangles.end(),
        separating_axis_it,
        face_axis_transform);

    auto const t1_edges = triangle.edges();
    auto const t2_edges = tetrahedron.edges();

    for (auto const& edge : t1_edges)
    {
        auto const edge_edge_separating_axis_transform_op =
            [&edge](common::line_segment_t const& e) {
                Eigen::Vector3d const d1 = edge.q - edge.p;
                Eigen::Vector3d d2       = e.q - e.p;

                auto axis = d1.cross(d2);

                double constexpr eps = std::numeric_limits<double>::epsilon();
                // Check if edges are parallel up to numerical precision eps
                if (axis.isZero(eps))
                {
                    d2   = e.p - edge.p;
                    axis = d1.cross(d2);
                }
                // Check if edges are on the same line up to numerical precision eps
                if (axis.isZero(eps))
                {
                    // Cancel this axis as a potential separating axis. When projecting
                    // nodes onto this axis, everything will be zeroed out, and thus no
                    // separating interval can be found. The separating axis test for
                    // this axis will fail.
                    axis.setZero();
                }

                return axis;
            };
        separating_axis_it = std::transform(
            t2_edges.begin(),
            t2_edges.end(),
            separating_axis_it,
            edge_edge_separating_axis_transform_op);
    }

    auto const is_separating_axis = [&triangle, &tetrahedron](common::normal_t const& axis) {
        if (axis.isZero())
            return false;

        auto const project = [axis](common::point_t const& p) {
            return p.dot(axis);
        };

        std::array<double, 3u> const projection1{
            project(triangle.p1()),
            project(triangle.p2()),
            project(triangle.p3())};

        std::array<double, 4u> const projection2{
            project(tetrahedron.p1()),
            project(tetrahedron.p2()),
            project(tetrahedron.p3()),
            project(tetrahedron.p4())};

        auto const [min_it1, max_it1] = std::minmax_element(projection1.begin(), projection1.end());
        auto const [min_it2, max_it2] = std::minmax_element(projection2.begin(), projection2.end());

        bool const has_separating_interval = (*max_it1 < *min_it2) || (*max_it2 < *min_it1);
        return has_separating_interval;
    };

    return std::none_of(separating_axis.begin(), separating_axis.end(), is_separating_axis);
}

bool intersects(point_t const& point, tetrahedron_t const& tetrahedron)
{
    auto const project = [](common::point_t const& p, common::triangle_t const& triangle) {
        auto const n = triangle.normal();
        auto const d = p - triangle.p1();
        return n.dot(d);
    };
    std::array<common::triangle_t, 4u> const faces = tetrahedron.faces();
    std::array<double, 4u> const projections{
        project(point, faces[0]),
        project(point, faces[1]),
        project(point, faces[2]),
        project(point, faces[3])};

    return std::none_of(projections.begin(), projections.end(), [](double const s) {
        return s > 0.;
    });
}

std::optional<point_t> intersect(line_segment_t const& segment, triangle_t const& triangle)
{
    Eigen::Vector3d const ab = triangle.b() - triangle.a();
    Eigen::Vector3d const ac = triangle.c() - triangle.a();
    Eigen::Vector3d const qp = segment.p - segment.q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = segment.p - triangle.a();
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};
    if (t > d)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a() + v * triangle.b() + w * triangle.c();
    return intersection;
}

std::optional<point_t> intersect(ray_t const& ray, triangle_t const& triangle)
{
    Eigen::Vector3d const& p = ray.p;
    Eigen::Vector3d const& q = ray.p + 1. * ray.v;
    Eigen::Vector3d const ab = triangle.b() - triangle.a();
    Eigen::Vector3d const ac = triangle.c() - triangle.a();
    Eigen::Vector3d const qp = p - q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = p - triangle.a();
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    // t *= ood;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a() + v * triangle.b() + w * triangle.c();
    return intersection;
}

std::optional<point_t> intersect_twoway(line_segment_t const& segment, triangle_t const& triangle)
{
    auto const intersection = intersect(segment, triangle);
    if (intersection.has_value())
        return intersection;

    line_segment_t const flipped_segment{segment.q, segment.p};
    return intersect(flipped_segment, triangle);
}

std::optional<point_t> intersect_twoway(ray_t const& ray, triangle_t const& triangle)
{
    auto const intersection = intersect(ray, triangle);
    if (intersection.has_value())
        return intersection;

    triangle_t const flipped_triangle{triangle.a(), triangle.c(), triangle.b()};
    return intersect(ray, flipped_triangle);
}

std::optional<point_t> intersect(line_segment_t const& segment, plane_t const& plane)
{
    double const d           = plane.p.dot(plane.n);
    Eigen::Vector3d const pq = segment.q - segment.p;
    double const t           = (d - plane.n.dot(segment.p)) / (plane.n.dot(pq));

    if (t >= 0.0 && t <= 1.0)
    {
        Eigen::Vector3d const intersection = segment.p + t * pq;
        return intersection;
    }
    return {};
}

/**
 * @brief
 * Implementation of closest point on triangle to a point P from Christer Ericson's Real-Time
 * Collision Detection
 * @param p Point off triangle
 * @param t Triangle on which we wish to find the closest point to p
 * @return The closest point q on triangle t to point p
 */
point_t closest_point(point_t const& p, triangle_t const& t)
{
    auto const& a = t.a();
    auto const& b = t.b();
    auto const& c = t.c();

    // Check if P in vertex region outside A
    common::vector3d_t const ab = b - a;
    common::vector3d_t const ac = c - a;
    common::vector3d_t const ap = p - a;
    double const d1             = ab.dot(ap);
    double const d2             = ac.dot(ap);
    if (d1 <= 0.0 && d2 <= 0.0)
        return a; // barycentric coordinates (1,0,0)

    // Check if P in vertex region outside B
    common::vector3d_t const bp = p - b;
    double const d3             = ab.dot(bp);
    double const d4             = ac.dot(bp);
    if (d3 >= 0.0 && d4 <= d3)
        return b; // barycentric coordinates (0,1,0)

    // Check if P in edge region of AB, if so return projection of P onto AB
    double const vc = d1 * d4 - d3 * d2;
    if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0)
    {
        double const v = d1 / (d1 - d3);
        return a + v * ab; // barycentric coordinates (1-v, v,0)
    }

    // Check if P in vertex region outside C
    common::vector3d_t const cp = p - c;
    double const d5             = ab.dot(cp);
    double const d6             = ac.dot(cp);
    if (d6 >= 0.0 && d5 <= d6)
        return c; // barycentric coordinates (0,0,1)

    // Check if P in edge region of AC, if so return projection of P onto AC
    double const vb = d5 * d2 - d1 * d6;
    if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0)
    {
        double const w = d2 / (d2 - d6);
        return a + w * ac; // barycentric coordinates (1-w, 0, w)
    }

    // Check if P in edge region of BC, if so return projection of P onto BC
    double const va = d3 * d6 - d5 * d4;
    if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0)
    {
        double const w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        return b + w * (c - b); // barycentric coordinates (0,1-w, w)
    }

    // P inside face region. Compute Q through its barycentric coordinates (u, v, w)
    double const denom = 1.0 / (va + vb + vc);
    double const v     = vb * denom;
    double const w     = vc * denom;
    return a + ab * v + ac * w; //=u*a+v*b+w*c,u=va* denom=1.0f−v−w
}

tetrahedron_t::tetrahedron_t(
    point_t const& p1,
    point_t const& p2,
    point_t const& p3,
    point_t const& p4)
    : p_{p1, p2, p3, p4}
{
}

double tetrahedron_t::unsigned_volume() const
{
    return std::abs(signed_volume());
}

double tetrahedron_t::signed_volume() const
{
    vector3d_t const p21 = p2() - p1();
    vector3d_t const p31 = p3() - p1();
    vector3d_t const p41 = p4() - p1();
    return p21.cross(p31).dot(p41);
}

std::array<triangle_t, 4u> tetrahedron_t::faces() const
{
    return std::array<triangle_t, 4u>{
        triangle_t{p1(), p2(), p4()},
        triangle_t{p2(), p3(), p4()},
        triangle_t{p3(), p1(), p4()},
        triangle_t{p1(), p3(), p2()}};
}

std::array<line_segment_t, 6u> tetrahedron_t::edges() const
{
    return std::array<line_segment_t, 6u>{
        line_segment_t{p1(), p2()},
        line_segment_t{p2(), p3()},
        line_segment_t{p3(), p1()},
        line_segment_t{p1(), p4()},
        line_segment_t{p2(), p4()},
        line_segment_t{p3(), p4()},
    };
}

std::array<point_t, 4u> const& tetrahedron_t::nodes() const
{
    return p_;
}

std::array<point_t, 4u>& tetrahedron_t::nodes()
{
    return p_;
}

point_t const& tetrahedron_t::p1() const
{
    return p_[0];
}

point_t const& tetrahedron_t::p2() const
{
    return p_[1];
}

point_t const& tetrahedron_t::p3() const
{
    return p_[2];
}

point_t const& tetrahedron_t::p4() const
{
    return p_[3];
}

point_t& tetrahedron_t::p1()
{
    return p_[0];
}

point_t& tetrahedron_t::p2()
{
    return p_[1];
}

point_t& tetrahedron_t::p3()
{
    return p_[2];
}

point_t& tetrahedron_t::p4()
{
    return p_[3];
}

aabb_t::aabb_t(point_t const& min, point_t const& max) : min(min), max(max) {}

bool aabb_t::contains(point_t const& p) const
{
    return (p.x() >= min.x() && p.x() <= max.x()) && (p.y() >= min.y() && p.y() <= max.y()) &&
           (p.z() >= min.z() && p.z() <= max.z());
}

aabb_t aabb_t::from(tetrahedron_t const& t)
{
    constexpr double inf = std::numeric_limits<double>::infinity();
    point_t min{inf, inf, inf}, max{-inf, -inf, -inf};
    for (auto const& p : t.nodes())
    {
        if (p.x() > max.x())
            max.x() = p.x();
        if (p.y() > max.y())
            max.y() = p.y();
        if (p.z() > max.z())
            max.z() = p.z();

        if (p.x() < min.x())
            min.x() = p.x();
        if (p.y() < min.y())
            min.y() = p.y();
        if (p.z() < min.z())
            min.z() = p.z();
    }
    return aabb_t{min, max};
}

aabb_t aabb_t::from(triangle_t const& t)
{
    constexpr double inf = std::numeric_limits<double>::infinity();
    point_t min{inf, inf, inf}, max{-inf, -inf, -inf};
    for (auto const& p : t.nodes())
    {
        if (p.x() > max.x())
            max.x() = p.x();
        if (p.y() > max.y())
            max.y() = p.y();
        if (p.z() > max.z())
            max.z() = p.z();

        if (p.x() < min.x())
            min.x() = p.x();
        if (p.y() < min.y())
            min.y() = p.y();
        if (p.z() < min.z())
            min.z() = p.z();
    }
    return aabb_t{min, max};
}

plane_t::plane_t(point_t const& p, normal_t const& n) : p(p), n(n) {}

plane_t::plane_t(triangle_t const& t) : p(t.a()), n()
{
    n = (t.b() - t.a()).cross(t.c() - t.a()).normalized();
}

double plane_t::signed_distance(point_t const& q) const
{
    return (q - p).dot(n);
}

} // namespace common
} // namespace sbs


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to 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_MATH_ISNORMAL_HPP
#define BOOST_MATH_ISNORMAL_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

template <typename T>
inline constexpr bool isnormal(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return x == T(0) ? false :
               boost::math::ccmath::isinf(x) ? false :
               boost::math::ccmath::isnan(x) ? false :
               boost::math::ccmath::abs(x) < (std::numeric_limits<T>::min)() ? false : true;
    }
    else
    {
        using std::isnormal;

        if constexpr (!std::is_integral_v<T>)
        {
            return isnormal(x);
        }
        else
        {
            return isnormal(static_cast<double>(x));
        }
    }
}
}

#endif // BOOST_MATH_ISNORMAL_HPP


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HBEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HBEV_HPP

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a banded Hermitian matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * hbev() computes the eigenvalues and optionally the associated
     * eigenvectors of a banded Hermitian matrix A. A matrix is Hermitian
     * when herm( A ) == A. When A is real, a Hermitian matrix is also
     * called symmetric.
     *
     * The eigen decomposition is A = U S * herm(U)  where  U  is a
     * unitary matrix and S is a diagonal matrix. The eigenvalues of A
     * are on the main diagonal of S. The eigenvalues are real.
     *
     * Workspace is organized following the arguments in the calling sequence.
     *  optimal_workspace() : for optimizing use of blas 3 kernels
     *  minimal_workspace() : minimum size of workarrays, but does not allow for optimization
     *                        of blas 3 kernels
     *  workspace( work ) for real matrices where work is a real array with
     *                    vector_size( work ) >= 3*matrix_size1( a ) - 2
     *  workspace( work, rwork ) for complex matrices where work is a complex
     *                           array with vector_size( work ) >= matrix_size1( a )
     *                           and rwork is a real array with
     *                           vector_size( rwork ) >= 3 * matrix_size1( a ) - 2.
     */

    /*
     * If uplo=='L' only the lower triangular part is stored.
     * If uplo=='U' only the upper triangular part is stored.
     *
     * The matrix is assumed to be stored in LAPACK band format, i.e.
     * matrices are stored columnwise, in a compressed format so that when e.g. uplo=='U'
     * the (i,j) element with j>=i is in position  (i-j) + j * (KD+1) + KD  where KD is the
     * half bandwidth of the matrix. For a triadiagonal matrix, KD=1, for a diagonal matrix
     * KD=0.
     * When uplo=='L', the (i,j) element with j>=i is in position  (i-j) + j * (KD+1).
     *
     * The matrix A is thus a rectangular matrix with KD+1 rows and N columns.
     */

    namespace detail {
      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 float* ab, int const ldab, float* w, float* z, int const ldz,
                 float* work, int& info)
      {
	      //for (int i=0; i<n*kd; ++i) std::cout << *(ab+i) << " " ;
	      //std::cout << "\n" ;
        LAPACK_SSBEV (&jobz, &uplo, &n, &kd, ab, &ldab, w, z, &ldz,
                      work, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 double* ab, int const ldab, double* w, double* z, int const ldz,
                 double* work, int& info)
      {
        LAPACK_DSBEV (&jobz, &uplo, &n, &kd, ab, &ldab, w, z, &ldz,
                      work, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 traits::complex_f* ab, int const ldab, float* w,
                 traits::complex_f* z, int const ldz,
                 traits::complex_f* work, float* rwork, int& info)
      {
        LAPACK_CHBEV (&jobz, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
                      w, traits::complex_ptr(z), &ldz,
                      traits::complex_ptr(work), rwork, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 traits::complex_d* ab, int const ldab, double* w,
                 traits::complex_d* z, int const ldz,
                 traits::complex_d* work, double* rwork, int& info)
      {
        LAPACK_ZHBEV (&jobz, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
                      w, traits::complex_ptr(z), &ldz,
                      traits::complex_ptr(work), rwork, &info);
      }
    }

    namespace detail {
       template <int N>
       struct Hbev{};

       /// Handling of workspace in the case of one workarray.
       template <>
       struct Hbev< 1 > {
          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, minimal_workspace , int& info ) const {
             traits::detail::array<T> work( 3*n-2 );
             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   info );
          }

          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, optimal_workspace , int& info ) const {
             traits::detail::array<T> work( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   info );
          }

          template <typename T, typename R, typename W>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, detail::workspace1<W> work,
                           int& info ) const {
             assert( traits::vector_size( work.w_ ) >= 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work.w_ ),
                   info );
          }
       }; // Hbev< 1 >

       /// Handling of workspace in the case of two workarrays.
       template <>
       struct Hbev< 2 > {
          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, minimal_workspace , int& info ) const {
             traits::detail::array<T> work( n );
             traits::detail::array<R> rwork( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   traits::vector_storage( rwork ),
                   info );
          }

          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, optimal_workspace , int& info ) const {
             traits::detail::array<T> work( n );
             traits::detail::array<R> rwork( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   traits::vector_storage( rwork ),
                   info );
          }

          template <typename T, typename R, typename W, typename RW>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, detail::workspace2<W,RW> work,
                           int& info ) const {
             assert( traits::vector_size( work.wr_ ) >= 3*n-2 );
             assert( traits::vector_size( work.w_ ) >= n );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work.w_ ),
                   traits::vector_storage( work.wr_ ),
                   info );
          }
       }; // Hbev< 2 >

       /// Compute eigendecomposition of the banded Hermitian matrix ab.
       /// if jobz=='N' only the eigenvalues are computed.
       /// if jobz=='V' compute the eigenvalues a and the eigenvectors.
       ///
       /// Workspace is organized following the arguments in the calling sequence.
       ///  optimal_workspace() : for optimizing use of blas 3 kernels
       ///  minimal_workspace() : minimum size of workarrays, but does not allow for optimization
       ///                       of blas 3 kernels
       ///  workspace( work ) for real matrices where work is a real array with
       ///                    vector_size( work ) >= 3*matrix_size1( a )-2
       ///  workspace( work, rwork ) for complex matrices where work is a complex
       ///                           array with vector_size( work ) >= matrix_size1( a )
       ///                           and rwork is a real array with
       ///                           vector_size( rwork ) >= 3*matrix_size1( a )-2.
       template <typename AB, typename Z, typename W, typename Work>
       int hbev( char const jobz, AB& ab, W& w, Z& z, Work work ) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<AB>::matrix_structure,
           traits::hermitian_t
         >::value));
#endif

         typedef typename AB::value_type                            value_type ;

         int const n = traits::matrix_size2 (ab);
         assert (n == traits::matrix_size1 (z));
         assert (n == traits::vector_size (w));
         assert ( jobz=='N' || jobz=='V' );

         int info ;
         detail::Hbev< n_workspace_args<value_type>::value >() (jobz,
                       traits::matrix_uplo_tag( ab ), n,
                       traits::matrix_upper_bandwidth(ab),
                       traits::matrix_storage (ab),
                       traits::leading_dimension (ab),
                       traits::vector_storage (w),
                       traits::matrix_storage (z),
                       traits::leading_dimension (z),
                       work, info);
	 return info ;
       } // hbev()

       } // namespace detail

       /// Compute eigendecomposition without eigenvectors
       template <typename AB, typename W, typename Work>
       inline
       int hbev (AB& ab, W& w, Work work) {
          return detail::hbev( 'N', ab, w, ab, work );
       } // hbev()

       /// Compute eigendecomposition with eigenvectors
       template <typename AB, typename W, typename Z, typename Work>
       inline
       int hbev (AB& ab, W& w, Z& z, Work work) {
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<Z>::matrix_structure,
           traits::general_t
         >::value));
         int const n = traits::matrix_size2 (ab);
          assert (n == traits::matrix_size1 (z));
          assert (n == traits::matrix_size2 (z));
          return detail::hbev( 'V', ab, w, z, work );
       } // hbev()

  }

}}}

#endif


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <optional>
#include <sstream>
#include <stack>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

using Mat = Eigen::MatrixXi;

struct Fold {
  enum dir { column, row };

  dir d;
  int coord;
};

Mat linesToMat(std::vector<std::string> &lines) {
  std::vector<std::pair<int, int>> coord;
  int max_x = 0;
  int max_y = 0;
  for (auto &l : lines) {
    if (l.empty()) {
      break;
    }
    for (char &c : l) {
      if (c == ',') {
        c = ' ';
      }
    }

    int x;
    int y;
    std::stringstream ss(l);
    ss >> x;
    ss >> y;

    max_x = std::max(x, max_x);
    max_y = std::max(y, max_y);
    coord.push_back({x, y});
  }

  Mat out(max_y + 1, max_x + 1);
  out.setZero();

  for (auto &[c, r] : coord) {
    out(r, c) = 1;
  }

  return out;
}

std::vector<Fold> linesToFolds(std::vector<std::string> const &lines) {
  std::vector<Fold> folds;
  for (auto const &l : lines) {
    Fold f;
    auto pos = l.find_first_of('=');
    std::string snum(l.data() + pos + 1, l.data() + l.size());
    f.coord = std::stoi(snum);

    auto pos2 = l.find_last_of(' ');
    std::string sdir(l.data() + pos2 + 1, l.data() + pos);

    if (sdir == "x") {
      f.d = Fold::column;
    } else if (sdir == "y") {
      f.d = Fold::row;
    } else {
      fmt::print("Error in fold parsing\n");
      std::abort();
    }

    folds.push_back(f);
  }
  return folds;
}

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::string> out;
  while (std::getline(file, line)) {
    out.push_back(line);
  }

  auto mat = linesToMat(out);
  auto blank = std::find_if(out.begin(), out.end(),
                            [](std::string const &l) { return l.empty(); });
  out.erase(out.begin(), blank + 1);
  return std::make_pair(mat, linesToFolds(out));
}

int main(int _, char **argv) {
  auto [M, folds] = parseFile(argv[1]);
  fmt::print("M({}, {})\n", M.rows(), M.cols());
  for (auto const &f : folds) {
    fmt::print("{}: {}\n", f.d, f.coord);
  }

  Mat Mc = M;
  auto sum = 0;
  for (auto const &f : folds) {
    auto coord = f.coord;
    if (f.d == Fold::column) {
      auto r = coord + 1;
      auto l = coord - 1;
      if (coord >= ((Mc.cols() - 1) / 2)) { // Right side has fewer
        while (r < Mc.cols() && l >= 0) {
          Mc.col(l--) += Mc.col(r++);
        }
        Mc = Mc.leftCols(coord).eval();
      } else { // left side has fewer
        while (r < Mc.cols() && l >= 0) {
          Mc.col(r++) += Mc.col(l--);
        }
        Mc = Mc.rightCols(coord).eval();
      }
    } else {
      auto t = coord - 1;
      auto b = coord + 1;
      if (coord >= ((Mc.rows() - 1) / 2)) { // bottom side has fewer
        while (b < Mc.rows() && t >= 0) {
          Mc.row(t--) += Mc.row(b++);
        }
        Mc = Mc.topRows(coord).eval();
      } else { // top side has fewer
        while (b < Mc.rows() && b >= 0) {
          Mc.row(b++) += Mc.row(t--);
        }
        Mc = Mc.bottomRows(coord).eval();
      }
    }

    for (auto i = 0; i < Mc.size(); ++i) {
      if (Mc.data()[i] > 1) {
        Mc.data()[i] = 1;
      }
    }

    if(sum == 0){
      sum = Mc.sum();
    }
  }

  fmt::print("Part 1: {}\n", sum);
  fmt::print("Part 2\n");
  for(auto r = 0; r < Mc.rows(); ++r){
    for(auto c = 0; c < Mc.cols(); ++c){
      if(Mc(r,c) == 1){
        std::cout << "#";
      } else {
        std::cout << " ";
      }
    }
    std::cout << "\n";
  }
}


#pragma once

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <queue>
#include <sstream>
#include <stack>
#include <string>
#include <vector>

namespace calculator {
using std::string;
using std::vector;

static int _calc_expr_without_paretheses(const string& input) {
  vector<string> parts;
  boost::split(parts, input, [](char c) { return c == ' '; });
  parts.erase(std::remove_if(parts.begin(), parts.end(),
                             [](const string& part) { return part.empty(); }),
              parts.end());
  vector<string> phase1;
  int i = 0;
  while (i < parts.size()) {
    const auto& op = parts[i];
    if (op == "*" || op == "/") {
      const auto& lhs = phase1.back();
      const auto& rhs = parts[i + 1];
      i += 2;
      const int lval = atoi(lhs.c_str());
      const int rval = atoi(rhs.c_str());
      const int val = op == "*" ? lval * rval : lval / rval;
      phase1.pop_back();
      phase1.push_back(std::to_string(val));
    } else {
      phase1.push_back(op);
      i += 1;
    }
  }
  int result = atoi(phase1[0].c_str());
  for (int i = 1; i < phase1.size() - 1; i++) {
    auto& op = phase1[i];
    int rhs = atoi(phase1[i + 1].c_str());
    if (op == "+") {
      result += rhs;
    } else {
      result -= rhs;
    }
  }
  return result;
}

int calc(const string input) {
  const string source(input);
  const int N = source.size();
  int pos = 0;
  vector<string> exprs;
  string expr;
  for (char ch : source) {
    if (ch == '(') {
      exprs.push_back(expr);
      expr.clear();
    } else if (ch == ')') {
      int result = _calc_expr_without_paretheses(expr);
      expr = exprs.back();
      exprs.pop_back();
      expr += " " + std::to_string(result) + " ";
    } else {
      expr += ch;
    }
  }
  int result = _calc_expr_without_paretheses(expr);
  return result;
}

struct Token {
  // // 字母 D 表示数值类型
  char kind;
  double value;
  Token() : kind('D'), value(0) {}
};

static inline bool is_operator(char ch) {
  return ch == '+' || ch == '-' || ch == '*' || ch == '/';
}

static vector<Token> tokenize(string input) {
  std::stringstream cin(input);
  vector<Token> tokens;
  bool leading = true;
  while (cin) {
    Token token;
    char ch = cin.peek();
    if (isdigit(ch)) {
      cin >> token.value;
      leading = false;
    } else if (leading && (ch == '+' || ch == '-')) {
      cin >> token.value;
      leading = false;
    } else {
      cin >> ch;
      if (isspace(ch)) {  // skip
        continue;
      }
      token.kind = ch;
      leading = (ch == '(' || is_operator(ch));
    }
    tokens.push_back(token);
  }
  return tokens;
}

static inline int op_priority(char ch) {
  switch (ch) {
    case '*':
    case '/':
      return 30;
    case '+':
    case '-':
      return 20;
    default:
      return 0;
  }
}
static bool is_integer(double num) {
  return isfinite(num) && floor(num) == num;
}

static string fmt_double(double d) {
  if (is_integer(d)) {
    long num = static_cast<long>(d);
    return std::to_string(num);
  } else {
    string str = std::to_string(d);
    str.erase(str.find_last_not_of('0') + 1, string::npos);
    return str;
  }
}

string infix_to_postfix(string input) {
  std::vector<string> output;
  std::stack<char> operators;

  vector<Token> tokens = tokenize(input);

  for (Token& token : tokens) {
    const char ch = token.kind;
    if (is_operator(ch)) {
      const int op_pri = op_priority(ch);
      while (!operators.empty()) {
        char prev_op = operators.top();
        const int pre_op_pri = op_priority(prev_op);
        if (pre_op_pri >= op_pri) {
          output.push_back(string(1, prev_op));
          operators.pop();
        } else {
          break;
        }
      }
      operators.push(ch);
    } else if (token.kind == 'D') {
      output.push_back(fmt_double(token.value));
    } else if (ch == '(') {
      operators.push(ch);
    } else if (ch == ')') {
      char prev_op = operators.top();
      while (prev_op != '(') {
        output.push_back(string(1, prev_op));
        operators.pop();
        prev_op = operators.top();
      }
      operators.pop();
    } else {
      // skip
    }
  }

  while (!operators.empty()) {
    char ch = operators.top();
    output.push_back(string(1, ch));
    operators.pop();
  }

  return boost::join(output, " ");
}

double calc_v2(string input) {
  return 0;
}
}  // namespace calculator


#ifndef __DIRICHLET_H__
#define __DIRICHLET_H__

#include <armadillo>

#include <cmath>
#include <numeric>
#include <vector>

#include <besiq/config.h>
#ifndef HAVE_TR1_RANDOM
#include <random>
typedef std::mt19937 prg_type;
#else
#include <tr1/random>
typedef std::tr1::mt19937 prg_type;
#endif

/**
 * Computes the dirichlet multinomial probability of a vector
 * x with prior parameter alpha.
 *
 * @param x The observations.
 * @param alpha The prior parameters of the dirichlet density.
 *
 * @return The posterior probability of x.
 */
double dirmult(const arma::vec &x, const arma::vec &alpha);

/**
 * Computes the dirichlet multinomial log probability of a vector
 * x with prior parameter alpha.
 *
 * @param x The observations.
 * @param alpha The prior parameters of the dirichlet density.
 *
 * @return The log posterior probability of x.
 */
double ldirmult(const arma::vec &x, const arma::vec &alpha);

/**
 * Computes the log of the binomial coefficient (n choose k).
 *
 * @param n Number of elements to draw from.
 * @param k Number of elements to draw.
 *
 * @return The log of the binomial coefficient.
 */
double lbinomial(double n, double k);

/**
 * This class is responsible for generating samples from a
 * dirichlet distribution. It does so by using the fact that
 * gamma(a_i, b) / sum_i gamma( a_i, b ) is dirichlet distributed
 * with parameter a_i.
 */
class dir_generator
{
public:
    /**
     * Constructor.
     *
     * Initializes the random generator with the given seed.
     *
     * @param seed The seed given to the random generator.
     */
    dir_generator(unsigned long seed);

    /**
     * Generates a random sample from the dirichlet distribution
     * with the given parameters.
     *
     * @param x The parameters of the dirichlet density.
     *
     * @return A sample from the dirichlet density.
     */
    arma::vec sample(const arma::vec &alpha);

private:
    /**
     * Mersenne twister random generator.
     */
    prg_type m_generator;
};

#endif /* End of __DIRICHLET_H__ */


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * 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.
 *
 *
 * factor_graph_solve.cpp
 *
 *  Created on: Mar 23, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/factor_graph_solve.hpp"
//#include "mrob/CustomCholesky.hpp"

#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseQR>

using namespace mrob;
using namespace std;
using namespace Eigen;

FGraphSolve::FGraphSolve(matrixMethod method, optimMethod optim):
	FGraph(), matrixMethod_(method), optimMethod_(optim), N_(0), M_(0),
	lambda_(1e-6), solutionTolerance_(1e-2)
{

}

FGraphSolve::~FGraphSolve() = default;

void FGraphSolve::solve(optimMethod method, uint_t maxIters)
{
    /**
     * 2800 2D nodes on M3500
     * Time profile :13.902 % build Adjacency matrix,
     *               34.344 % build Information,
     *               48.0506 % build Cholesky,
     *               2.3075 % solve forward and back substitution,
     *               1.3959 % update values,
     *
     */
    optimMethod_ = method; // updates the optimization method
    time_profiles_.reset();

    // Optimization
    switch(optimMethod_)
    {
      case GN:
        this->optimize_gauss_newton();// false => lambda = 0
        this->update_nodes();
        break;
      case LM:
        this->optimize_levenberg_marquardt(maxIters);
        break;
      default:
        assert(0 && "FGraphSolve:: optimization method unknown");
    }

    if (0)
        time_profiles_.print();
}

void FGraphSolve::build_problem(bool useLambda)
{

    // 1) Adjacency matrix A, it has to
    //    linearize and calculate the Jacobians and required matrices
    time_profiles_.start();
    this->build_adjacency();
    time_profiles_.stop("Adjacency");

    // 1.2) builds specifically the information
    switch(matrixMethod_)
    {
      case ADJ:
        time_profiles_.start();
        this->build_info_adjacency();
        time_profiles_.stop("Info Adjacency");
        break;
      case SCHUR:
      default:
        assert(0 && "FGraphSolve: method not implemented");
    }

    // Structure for LM and dampening GN-based methods
    if (useLambda)
    {
        diagL_ = L_.diagonal();
    }
}

void FGraphSolve::optimize_gauss_newton(bool useLambda)
{
    // requires a Column-storage matrix
    SimplicialLDLT<SMatCol,Lower, AMDOrdering<SMatCol::StorageIndex>> cholesky;

    this->build_problem(useLambda);

    // compute cholesky solution
    time_profiles_.start();
    if (useLambda)
    {
        for (uint_t n = 0 ; n < N_; ++n)
            L_.coeffRef(n,n) = lambda_ + diagL_(n);//Circunference =>  diagL_(n) + lambda_
            //L_.coeffRef(n,n) = (1.0 + lambda_)*diagL_(n);//Elipsoid, for circunference =>  diagL_(n) + lambda.
    }
    cholesky.compute(L_);
    time_profiles_.stop("Gauss Newton create Cholesky");
    time_profiles_.start();
    dx_ = cholesky.solve(b_);
    time_profiles_.stop("Gauss Newton solve Cholesky");

}

uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
{
    //SimplicialLDLT<SMatCol,Lower, AMDOrdering<SMatCol::StorageIndex>> cholesky;

    // LM trust region as described in Bertsekas (p.105)

    // 0) parameter initialization
    lambda_ = 1e-5;
    // sigma reference to the fidelity of the model at the proposed solution \in [0,1]
    matData_t sigma1(0.25), sigma2(0.8);// 0 < sigma1 < sigma2 < 1
    matData_t beta1(2.0), beta2(0.25); // lambda updates multiplier values, beta1 > 1 > beta2 >0
    //matData_t lambdaMax, lambdaMin; // XXX lower bound unnecessary

    matData_t currentChi2, deltaChi2, modelFidelity;
    uint_t iter = 0;

    do{
        iter++;
        // 1) solve subproblem and current error
        this->optimize_gauss_newton(true);// Test if solved anything? no nans
        currentChi2 = this->chi2(false);// TODO residuals don't need to be calculated again (see optimizer.cpp)
        this->synchronize_nodes_auxiliary_state();// book-keeps states to undo updates
        this->update_nodes();

        // 1.2) Check for convergence, needs update and re-evaluaiton of errors
        deltaChi2 = currentChi2 - this->chi2(true);
        std::cout << "\nFGraphSolve::optimize_levenberg_marquardt: iteration "
                  << iter << " lambda = " << lambda_ << ", error " << currentChi2
                  << ", and delta = " << deltaChi2
                  << std::endl;
        if (deltaChi2 < 0)
        {
            // proposed dx did not improve, repeat 1) and reduce area of optimization = increase lambda
            lambda_ *= beta1;
            this->synchronize_nodes_state();
            continue;
        }

        // 1.3) check for convergence
        if (deltaChi2 < solutionTolerance_)
            return iter;

        // 2) Fidelity of the quadratized model vs non-linear chi2 evaluation.
        // f = chi2(x_k) - chi2(x_k + dx)
        //     chi2(x_k) - m_k(dx)
        // where m_k is the quadratized model = ||r||^2 - dx'*J' r + 0.5 dx'(J'J + lambda*D2)dx
        modelFidelity = deltaChi2 / (dx_.dot(b_) - 0.5*dx_.dot(L_* dx_));
        std::cout << "model fidelity = " << modelFidelity << " and m_k = " << dx_.dot(b_) << std::endl;

        //3) update lambda
        if (modelFidelity < sigma1)
            lambda_ *= beta1;
        if (modelFidelity > sigma2)
            lambda_ *= beta2;

    } while (iter < maxIters);

    // output
    std::cout << "FGraphSolve::optimize_levenberg_marquardt: failed to converge after "
              << iter << " iterations and error " << currentChi2
              << ", and delta = " << deltaChi2
              << std::endl;
    return 0; //

}

void FGraphSolve::build_adjacency()
{
    // 0) resize properly matrices (if needed)
    r_.resize(obsDim_,1);//dense vector TODO is it better to reserve and push_back??
    A_.resize(obsDim_, stateDim_);//Sparse matrix clear data
    W_.resize(obsDim_, obsDim_);//TODO should we reinitialize this all the time? an incremental should be fairly easy

    // 1) create the vector's structures
    std::deque<std::shared_ptr<Factor> >* factors;
    std::deque<std::shared_ptr<Node> >* nodes;
    // TODO: optimizing subgraph is not an option now, but we maintain generality
    factors = &factors_;
    nodes = &nodes_;

    // 2) vector structure to bookkeep the starting Nodes indices inside A

    // 2.2) Node indexes bookeept
    std::vector<uint_t> indNodesMatrix;
    indNodesMatrix.reserve(nodes->size());

    N_ = 0;
    for (id_t i = 0; i < nodes->size(); ++i)
    {
        // calculate the indices to access
        uint_t dim = (*nodes)[i]->get_dim();
        indNodesMatrix.push_back(N_);
        N_ += dim;

    }
    assert(N_ == stateDim_ && "FGraphSolve::buildAdjacency: State Dimensions are not coincident\n");

    // 3) Evaluate every factor given the current state and bookeeping of Factor indices
    std::vector<uint_t> reservationA;
    reservationA.reserve( obsDim_ );
    std::vector<uint_t> reservationW;
    reservationW.reserve( obsDim_ );
    std::vector<uint_t> indFactorsMatrix;
    indFactorsMatrix.reserve(factors->size());
    M_ = 0;
    for (uint_t i = 0; i < factors->size(); ++i)
    {
        auto f = (*factors)[i];
        f->evaluate_residuals();
        f->evaluate_jacobians();
        f->evaluate_chi2();

        // calculate dimensions for reservation and bookeping vector
        uint_t dim = f->get_dim();
        uint_t allDim = f->get_all_nodes_dim();
        for (uint_t j = 0; j < dim; ++j)
        {
            reservationA.push_back(allDim);
            reservationW.push_back(dim-j);
        }
        indFactorsMatrix.push_back(M_);
        M_ += dim;
    }
    assert(M_ == obsDim_ && "FGraphSolve::buildAdjacency: Observation dimensions are not coincident\n");
    A_.reserve(reservationA); //Exact allocation for elements.
    W_.reserve(reservationW); //same

    // XXX This could be subject to parallelization, maybe on two steps: eval + build
    for (uint_t i = 0; i < factors->size(); ++i)
    {
        auto f = (*factors)[i];

        // 4) Get the calculated residual
        r_.block(indFactorsMatrix[i], 0, f->get_dim(), 1) <<  f->get_residual();

        // 5) build Adjacency matrix as a composition of rows
        // 5.1) Get the number of nodes involved. It is a vector of nodes
        auto neighNodes = f->get_neighbour_nodes();
        // Iterates over the Jacobian row
        for (uint_t l=0; l < f->get_dim() ; ++l)
        {
            uint_t totalK = 0;
            // Iterates over the number of neighbour Nodes (ordered by construction)
            for (uint_t j=0; j < neighNodes->size(); ++j)
            {
                uint_t indNode = (*neighNodes)[j]->get_id();
                uint_t dimNode = (*neighNodes)[j]->get_dim();
                for(uint_t k = 0; k < dimNode; ++k)
                {
                    // order according to the permutation vector
                    uint_t iRow = indFactorsMatrix[i] + l;
                    uint_t iCol = indNodesMatrix[indNode] + k;
                    // This is an ordered insertion
                    A_.insert(iRow,iCol) = f->get_jacobian()(l, k + totalK);
                }
                totalK += dimNode;
            }
        }

        // 5) Get information matrix for every factor
        for (uint_t l = 0; l < f->get_dim(); ++l)
        {
            // only iterates over the upper triangular part
            for (uint_t k = l; k < f->get_dim(); ++k)
            {
                uint_t iRow = indFactorsMatrix[i] + l;
                uint_t iCol = indFactorsMatrix[i] + k;
                W_.insert(iRow,iCol) = f->get_information_matrix()(l,k);
                // If QR, then we need the following, but we dont suppoort QR anyway
                //W_.insert(iRow,iCol) = f->get_trans_sqrt_information_matrix()(l,k);
            }
        }
    } //end factors loop

}

void FGraphSolve::build_info_adjacency()
{
    /**
     * L_ dx = b_ corresponds to the normal equation A'*W*A dx = A'*W*r
     * only store the lower part of the information matrix (symmetric)
     *
     * XXX: In terms of speed, using the selfadjointview does not improve,
     * Eigen stores a temporary object and then copy only the upper part.
     *
     */
    L_ = (A_.transpose() * W_.selfadjointView<Eigen::Upper>() * A_);
    b_ = A_.transpose() * W_.selfadjointView<Eigen::Upper>() * r_;
}

matData_t FGraphSolve::chi2(bool evaluateResidualsFlag)
{
    matData_t totalChi2 = 0.0;
    for (uint_t i = 0; i < factors_.size(); ++i)
    {
        auto f = factors_[i];
        if (evaluateResidualsFlag)
        {
            f->evaluate_residuals();
            f->evaluate_chi2();
        }
        totalChi2 += f->get_chi2();
    }
    return totalChi2;
}

void FGraphSolve::update_nodes()
{
    int acc_start = 0;
    for (uint_t i = 0; i < nodes_.size(); i++)
    {
        // node update is the negative of dx just calculated.
        // x = x - alpha * H^(-1) * Grad = x - dx
        // Depending on the optimization, it is already taking care of the step alpha, so we assume alpha = 1
        auto node_update = -dx_.block(acc_start, 0, nodes_[i]->get_dim(), 1);
        nodes_[i]->update(node_update);

        acc_start += nodes_[i]->get_dim();
    }
}

void FGraphSolve::synchronize_nodes_auxiliary_state()
{
    for (auto n : nodes_)
        n->set_auxiliary_state(n->get_state());
}

void FGraphSolve::synchronize_nodes_state()
{
    for (auto n : nodes_)
        n->set_state(n->get_auxiliary_state());
}

// method to output (to python) or other programs the current state of the system.
std::vector<MatX> FGraphSolve::get_estimated_state()
{
    vector<MatX> results;
    results.reserve(nodes_.size());

    for (uint_t i = 0; i < nodes_.size(); i++)
    {
        //nodes_[i]->print();
        MatX updated_pos = nodes_[i]->get_state();
        results.emplace_back(updated_pos);
    }

    return results;
}

MatX1 FGraphSolve::get_chi2_array()
{
    MatX1 results(factors_.size());

    for (uint_t i = 0; i < factors_.size(); ++i)
    {
        auto f = factors_[i];
        results(i) = f->get_chi2();
    }

    return results;
}


#include "quadric_error_metric.h"
#include <Eigen/Geometry>
#include <iostream>

namespace {
	const double eps = 1e-7;
	const double uv_weight = 1e6;
}
void quadric_error_metric(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	std::vector< Eigen::MatrixXd >& Q)
{
	using namespace std;
	using namespace Eigen;

	// array of 4x4 matrices
	Q.resize( V.rows() );
	for( int i=0; i<Q.size(); i++ )
		Q[i] = Matrix4d::Zero();

	const auto & face_from_three_points = [](const Vector3d& v1, const Vector3d& v2, const Vector3d& v3)
	{
		Vector3d n = (v2-v1).cross(v3-v1);
		n.normalize();
		double d = -n.dot(v1);

		Vector4d res;
		res << n(0), n(1), n(2), d;

		return res;
	};

	// the metric at each vertex equals to the sum of metric of its attached faces
	for( int i=0; i<F.rows(); i++ ) {
		Vector3d v1 = V.row( F(i,0) );
		Vector3d v2 = V.row( F(i,1) );
		Vector3d v3 = V.row( F(i,2) );
		Vector4d p = face_from_three_points(v1, v2, v3);
		Matrix4d metric = p*p.transpose();

		Q[ F(i,0) ] += metric;
		Q[ F(i,1) ] += metric;
		Q[ F(i,2) ] += metric;
	}

	// the cost v.T*Q*v should equal to zero
	for( int i=0; i<V.rows(); i++ ) {
		// cout << fabs(v * Q[i] * v.transpose()) << endl;
		assert( fabs(V.row(i).homogeneous() * Q[i] * V.row(i).homogeneous().transpose()) <= eps );
	}
}

void qslim_5d(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	const Eigen::MatrixXd& TC,
	const Eigen::MatrixXi& FT,
	std::vector< Eigen::MatrixXd >& Q)
{
	using namespace std;
	using namespace Eigen;

	assert( F.rows() == FT.rows() );
	const int nF = F.rows();
	// array of 6x6 matrices
	Q.resize( V.rows() );
	for( int i=0; i<Q.size(); i++ ) {
		MatrixXd metric(6,6);
		metric.setZero();
		Q[i] = metric;
	}

	for(int i=0; i<nF; i++) {
		VectorXd p1(5),p2(5),p3(5);
		p1.head(3) = V.row( F(i,0) );
		p2.head(3) = V.row( F(i,1) );
		p3.head(3) = V.row( F(i,2) );
		p1.tail(2) = TC.row( FT(i,0) );
		p2.tail(2) = TC.row( FT(i,1) );
		p3.tail(2) = TC.row( FT(i,2) );
		// Paper Section 5.1
		VectorXd e1 = (p2-p1)/(p2-p1).norm();
		VectorXd e2 = p3-p1-(e1.dot(p3-p1))*e1;
		e2 /= e2.norm();
		const double eps = 1e-7;
		assert( fabs(e1.norm() - 1) <= eps );
		assert( fabs(e2.norm() - 1) <= eps );

		MatrixXd A(5,5);
		A.setIdentity();
		A = A - e1*e1.transpose() - e2*e2.transpose();
		VectorXd b = p1.dot(e1)*e1 + p1.dot(e2)*e2 - p1;
		double c = p1.dot(p1) - p1.dot(e1)*p1.dot(e1) - p1.dot(e2)*p1.dot(e2);

		// Paper Section 3.4
		MatrixXd metric(6,6);
		metric.block(0,0,5,5) = A;
		metric.block(0,5,5,1) = b;
		metric.block(5,0,1,5) = b.transpose();
		metric(5,5) = c;

		// add metric to each vertex
		Q[ F(i,0) ] += metric;
		Q[ F(i,1) ] += metric;
		Q[ F(i,2) ] += metric;
	}
}

void half_edge_qslim_5d(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	const Eigen::MatrixXd& TC,
	const Eigen::MatrixXi& FT,
	MapV5d & hash_Q)
{
	using namespace std;
	using namespace Eigen;

	// initialize 5d vertex map, key is (vi,ti), value is zero metric
	assert( F.rows() == FT.rows() );
	const int nF = F.rows();
	for(int i=0; i<nF; i++) {

		/// A. compute metric for each face
		VectorXd p1(5),p2(5),p3(5);
		p1.head(3) = V.row( F(i,0) );
		p2.head(3) = V.row( F(i,1) );
		p3.head(3) = V.row( F(i,2) );
		p1.tail(2) = TC.row( FT(i,0) );
		p2.tail(2) = TC.row( FT(i,1) );
		p3.tail(2) = TC.row( FT(i,2) );
		// Paper Section 5.1
		VectorXd e1 = (p2-p1)/(p2-p1).norm();
		VectorXd e2 = p3-p1-(e1.dot(p3-p1))*e1;
		e2 /= e2.norm();
		const double eps = 1e-7;
		assert( fabs(e1.norm() - 1) <= eps );
		assert( fabs(e2.norm() - 1) <= eps );

		MatrixXd A(5,5);
		A.setIdentity();
		A = A - e1*e1.transpose() - e2*e2.transpose();
		VectorXd b = p1.dot(e1)*e1 + p1.dot(e2)*e2 - p1;
		double c = p1.dot(p1) - p1.dot(e1)*p1.dot(e1) - p1.dot(e2)*p1.dot(e2);

		// Paper Section 3.4
		MatrixXd metric(6,6);
		metric.block(0,0,5,5) = A;
		metric.block(0,5,5,1) = b;
		metric.block(5,0,1,5) = b.transpose();
		metric(5,5) = c;

		/// B. assign the face metric to each 5d vertex, if it hasn't appeared, initialize
		/// it with the metric, otherwise, add the metric to its original metric.
		for(int j=0; j<3; j++) {
			int vi = F(i,j);
			int ti = FT(i,j);
			if( hash_Q[vi].count(ti) == 0 ) {
				hash_Q[vi][ti] = metric;
			}
			else {
				hash_Q[vi][ti] += metric;
			}
		}
	}

}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_ACOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOSH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing acosh capabilities

    Returns the hyperbolic cosine argument: \f$\log(x+\sqrt{x^2-1})\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acosh(x);
    @endcode

    is similar to:

    @code
    T r = log(x+sqrt{minusone(sqr(x))});
    @endcode

    @see log, sqrt, cosh, minusone

  **/
  const boost::dispatch::functor<tag::acosh_> acosh = {};
} }
#endif

#include <boost/simd/function/scalar/acosh.hpp>
#include <boost/simd/function/simd/acosh.hpp>

#endif