#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

namespace py = pybind11;

// Example autodiff cost function from ceres tutorial
struct ExampleFunctor {
  template<typename T>
  bool operator()(const T *const x, T *residual) const {
    residual[0] = T(10.0) - x[0];
    return true;
  }

  static ceres::CostFunction *Create() {
    return new ceres::AutoDiffCostFunction<ExampleFunctor,
                                           1,
                                           1>(new ExampleFunctor);
  }
};

// cSE3 conversion functions: to/from pointer/vector
template<typename T>
void cSE3convert(const T* data, Sophus::SE3<T>& X) {
  X = Sophus::SE3<T>(Eigen::Quaternion<T>(data+3),
                     Eigen::Map<const Eigen::Matrix<T,3,1>>(data));
}
template<typename T>
void cSE3convert(const Eigen::Matrix<T,7,1> &Xvec, Sophus::SE3<T>& X) {
  X = Sophus::SE3<T>(Eigen::Quaternion<T>(Xvec.data()+3),
                     Eigen::Map<const Eigen::Matrix<T,3,1>>(Xvec.data()));
}
template<typename T>
void cSE3convert(const Sophus::SE3<T>& X, Eigen::Matrix<T,7,1> &Xvec) {
  Xvec.template block<3,1>(0,0) = Eigen::Matrix<T,3,1>(X.translation().data());
  Xvec.template block<4,1>(3,0) = Eigen::Matrix<T,4,1>(X.so3().data());
}

// AutoDiff cost function (factor) for the difference between a measured 3D
// relative transform, Xij = (tij_, qij_), and the relative transform between two
// estimated poses, Xi_hat and Xj_hat. Weighted by measurement covariance, Qij_.
class cSE3Functor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured relative pose and inverted covariance matrix
  cSE3Functor(Eigen::Matrix<double,7,1> &Xij_vec, Eigen::Matrix<double,6,6> &Qij)
  {
    Sophus::SE3<double> Xij;
    cSE3convert(Xij_vec, Xij);
    Xij_inv_ = Xij.inverse();
    Qij_inv_ = Qij.inverse();
  }

  // templated residual definition for both doubles and jets
  // basically a weighted implementation of boxminus using Eigen templated types
  template<typename T>
  bool operator()(const T* _Xi_hat, const T* _Xj_hat, T* _res) const
  {
    // assign memory to usable objects
    Sophus::SE3<T> Xi_hat, Xj_hat;
    cSE3convert(_Xi_hat, Xi_hat);
    cSE3convert(_Xj_hat, Xj_hat);
    Eigen::Map<Eigen::Matrix<T,6,1>> r(_res);

    // compute current estimated relative pose
    const Sophus::SE3<T> Xij_hat = Xi_hat.inverse() * Xj_hat;

    // compute residual via boxminus (i.e., the logarithmic map of the error pose)
    // weight with inverse covariance
    r = Qij_inv_.cast<T>() * (Xij_inv_.cast<T>() * Xij_hat).log();

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Matrix<double,7,1> &Xij, Eigen::Matrix<double,6,6> &Qij) {
    return new ceres::AutoDiffCostFunction<cSE3Functor,
                                           6,
                                           7,
                                           7>(new cSE3Functor(Xij, Qij));
  }

private:
  Sophus::SE3<double> Xij_inv_;
  Eigen::Matrix<double,6,6> Qij_inv_;
};

// AutoDiff cost function (factor) for the difference between a range measurement
// rij, and the relative range between two estimated poses, Xi_hat and Xj_hat.
// Weighted by measurement variance, qij_.
class RangeFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured range and inverted variance
  RangeFunctor(double &rij, double &qij)
  {
    rij_ = rij;
    qij_inv_ = 1.0 / qij;
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _Xi_hat, const T* _Xj_hat, T* _res) const
  {
    // assign memory to usable objects
    Eigen::Matrix<T,3,1> ti_hat(_Xi_hat), tj_hat(_Xj_hat);

    // range measurement error, scaled by inverse variance
    *_res = static_cast<T>(qij_inv_) * (static_cast<T>(rij_) - (tj_hat - ti_hat).norm());

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(double &rij, double &qij) {
    return new ceres::AutoDiffCostFunction<RangeFunctor,
                                           1,
                                           7,
                                           7>(new RangeFunctor(rij, qij));
  }

private:
  double rij_;
  double qij_inv_;
};

// AutoDiff cost function (factor) for the difference between an altitude
// measurement hi, and the altitude of an estimated pose, Xi_hat.
// Weighted by measurement variance, qij_.
class AltFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured range and inverted variance
  AltFunctor(double &hi, double &qi)
  {
    hi_ = hi;
    qi_inv_ = 1.0 / qi;
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _Xi_hat, T* _res) const
  {
    // assign memory to usable objects
    T hi_hat = *(_Xi_hat + 2);

    // altitude measurement error, scaled by inverse variance
    *_res = static_cast<T>(qi_inv_) * (static_cast<T>(hi_) - hi_hat);

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(double &hi, double &qi) {
    return new ceres::AutoDiffCostFunction<AltFunctor,
                                           1,
                                           7>(new AltFunctor(hi, qi));
  }

private:
  double hi_;
  double qi_inv_;
};

// AutoDiff cost function (factor) for the difference between a measured 3D
// relative transform, Xij_ = (tij_, qij_), and the relative transform between two
// estimated poses, Xi_hat and Xj_hat. Weighted by measurement covariance, Qij_.
class Transform3DFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured relative pose and covariance matrix
  Transform3DFunctor(Eigen::Matrix<double,7,1> &X, Eigen::Matrix<double,6,6> &Q)
  {
    tij_ = X.block<3,1>(0,0);
    qij_ = Eigen::Quaterniond(X.block<4,1>(3,0));
    // residual weighted by inverse covariance
    Wij_ = Q.inverse();
  }

  // templated residual definition for both doubles and jets
  // basically a weighted implementation of boxminus using Eigen templated types
  template<typename T>
  bool operator()(const T* Xi_hat, const T* Xj_hat, T* res) const
  {
    // assign memory to usable objects
    Eigen::Map<const Eigen::Matrix<T,3,1>> ti_hat(Xi_hat);
    Eigen::Quaternion<T> qi_hat(Xi_hat+3);
    Eigen::Map<const Eigen::Matrix<T,3,1>> tj_hat(Xj_hat);
    Eigen::Quaternion<T> qj_hat(Xj_hat+3);
    Eigen::Map<Eigen::Matrix<T,6,1>> r(res);

    // compute trivial vector subtraction component
    Eigen::Matrix<T,3,1> t_res = tij_ - (tj_hat - ti_hat);

    // compute quaternion difference component
    Eigen::Quaternion<T> qij_hat = qi_hat.inverse() * qj_hat;
    Eigen::Quaternion<T> q_err = qij_hat.inverse() * static_cast<Eigen::Quaternion<T>>(qij_);
    Eigen::Matrix<T,3,1> q_res;
    if (q_err.w() < static_cast<T>(0.0))
    {
      q_err.coeffs() *= static_cast<T>(-1.0);
    }
    const Eigen::Matrix<T,3,1> qv(q_err.vec());
    T sinha = qv.norm();
    if (sinha > static_cast<T>(0.0))
    {
      T angle = static_cast<T>(2.0) * atan2(sinha, q_err.w());
      q_res = qv * (angle / sinha);
    }
    else
    {
      q_res = qv * (static_cast<T>(2.0) / q_err.w());
    }

    // assign to residual and weight by covariance
    r.template block<3,1>(0,0) = t_res;
    r.template block<3,1>(3,0) = q_res;
    r = Wij_ * r;

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Matrix<double,7,1> &X, Eigen::Matrix<double,6,6> &Q) {
    return new ceres::AutoDiffCostFunction<Transform3DFunctor,
                                           6,
                                           7,
                                           7>(new Transform3DFunctor(X, Q));
  }

private:
  Eigen::Matrix<double,3,1> tij_;
  Eigen::Quaternion<double> qij_;
  Eigen::Matrix<double,6,6> Wij_;
};

// AutoDiff cost function (factor) for the difference between a measured quaternion,
// q_, and an estimated quaternion, q_hat.
class QuaterniondFunctor
{
public:
  // store internal measured quaternion
  QuaterniondFunctor(Eigen::Vector4d &x)
  {
    q_ = Eigen::Quaternion<double>(x.data());
    q_inv_ = q_.inverse();
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _q_hat, T* res) const
  {
    Eigen::Quaternion<T> q_hat(_q_hat);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> r(res);

    // implementation of boxminus using templated Eigen types
    // https://eigen.tuxfamily.org/bz/show_bug.cgi?id=1244
    //  q_err = q^{-1} * q_hat
    Eigen::Quaternion<T> q_err(static_cast<Eigen::Quaternion<T>>(q_inv_)*(q_hat));
    //  q_err -> theta vector
    if (q_err.w() < static_cast<T>(0.0))
    {
      q_err.coeffs() *= static_cast<T>(-1.0);
    }
    const Eigen::Matrix<T,3,1> qv(q_err.vec());
    T sinha = qv.norm();
    if (sinha > static_cast<T>(0.0))
    {
      T angle = static_cast<T>(2.0) * atan2(sinha, q_err.w());
      r = qv * (angle / sinha);
    }
    else
    {
      r = qv * (static_cast<T>(2.0) / q_err.w());
    }

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Vector4d &x) {
    return new ceres::AutoDiffCostFunction<QuaterniondFunctor, 3, 4>(new QuaterniondFunctor(x));
  }

private:
  Eigen::Quaternion<double> q_;
  Eigen::Quaternion<double> q_inv_;
};

// AutoDiff local parameterization for the compact SE3 pose [t q] object.
// The boxplus operator informs Ceres how the manifold evolves and also
// allows for the calculation of derivatives.
struct cSE3Plus {
  // boxplus operator for both doubles and jets
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const
  {
    // capture argument memory
    Sophus::SE3<T> X;
    cSE3convert(x, X);
    Eigen::Map<const Eigen::Matrix<T, 6, 1>> dX(delta);
    Eigen::Map<Eigen::Matrix<T, 7, 1>>       Yvec(x_plus_delta);

    // increment pose using the exponential map
    Sophus::SE3<T> Exp_dX = Sophus::SE3<T>::exp(dX);
    Sophus::SE3<T> Y = X * Exp_dX;

    // assign SE3 coefficients to compact vector
    Eigen::Matrix<T,7,1> YvecCoef;
    cSE3convert(Y, YvecCoef);
    Yvec = YvecCoef;

    return true;
  }

  // local parameterization generator--ONLY FOR PYTHON WRAPPER
  static ceres::LocalParameterization *Create() {
    return new ceres::AutoDiffLocalParameterization<cSE3Plus,
                                                    7,
                                                    6>(new cSE3Plus);
  }
};

// AutoDiff local parameterization for the Transform3D object. The boxplus
// operator informs Ceres how the manifold evolves and also allows for the
// calculation of derivatives
struct Transform3DPlus {
  // boxplus operator for both doubles and jets
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const
  {
    // capture argument memory
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(x);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> dt(delta);
    const Eigen::Quaternion<T> q(x+3);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> dq(delta+3);
    Eigen::Map<Eigen::Matrix<T, 7, 1>> Tplus(x_plus_delta);

    // compute trivial vector addition component
    Tplus.template block<3,1>(0,0) = t + dt;

    // compute quaternion oplus component
    Eigen::Quaternion<T> plusq;
    T th = dq.norm();
    if (th > static_cast<T>(0.0))
    {
      Eigen::Matrix<T, 3, 1> u = dq / th;
      T sth2 = sin(th / static_cast<T>(2.0));
      plusq = Eigen::Quaternion<T>(cos(th / static_cast<T>(2.0)), u.x()*sth2, u.y()*sth2, u.z()*sth2);
    }
    else
    {
      Eigen::Matrix<T, 3, 1> r2 = dq / static_cast<T>(2.0);
      plusq = Eigen::Quaternion<T>(static_cast<T>(1.0), r2.x(), r2.y(), r2.z()).normalized();
    }
    Eigen::Quaternion<T> qplusq = q * plusq;
    Tplus.template block<4,1>(3,0) = qplusq.coeffs();

    return true;
  }

  // local parameterization generator--ONLY FOR PYTHON WRAPPER
  static ceres::LocalParameterization *Create() {
    return new ceres::AutoDiffLocalParameterization<Transform3DPlus,
                                                    7,
                                                    6>(new Transform3DPlus);
  }
};

void add_custom_cost_functions(py::module &m) {

  // Use pybind11 code to wrap your own cost function which is defined in C++

  // Transform3DLocalParameterization
  m.def("Transform3DLocalParameterization", &Transform3DPlus::Create);

  // cSE3LocalParameterization
  m.def("cSE3LocalParameterization", &cSE3Plus::Create);

  // cSE3Factor
  m.def("cSE3Factor", &cSE3Functor::Create);

  // RangeFactor
  m.def("RangeFactor", &RangeFunctor::Create);

  // AltFactor
  m.def("AltFactor", &AltFunctor::Create);

  // Transform3DFactor
  m.def("Transform3DFactor", &Transform3DFunctor::Create);

  // Here is an example
  m.def("CreateCustomExampleCostFunction", &ExampleFunctor::Create);

  // QuaterniondFunctor
  m.def("CreateQuaterniondADCostFunction", &QuaterniondFunctor::Create);

}


/**
 * @file finitevolumesineconslaw.cc
 * @brief NPDE homework "FiniteVolumeSineConsLaw" code
 * @author Oliver Rietmann
 * @date 25.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "finitevolumesineconslaw.h"

#include <Eigen/Core>
#include <cmath>

namespace FiniteVolumeSineConsLaw {

/* SAM_LISTING_BEGIN_1 */
constexpr double PI = 3.14159265358979323846;

double f(double x) { return std::sin(PI * x); }

double sineGodFlux(double v, double w) {
  double result;
  //====================
  // Your code goes here
  //====================

  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd sineClawRhs(const Eigen::VectorXd &mu) {
  int N = mu.size();
  double h = 12.0 / N;
  Eigen::VectorXd result(N);

  //====================
  // Your code goes here
  //====================

  return result;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
bool blowup(const Eigen::VectorXd &mu) {
  return mu.minCoeff() < 0.0 || mu.maxCoeff() > 2.0;
}

unsigned int findTimesteps() {
  const unsigned int N = 600;
  unsigned int ML = 100;
  unsigned int MR = 200;

  //====================
  // Your code goes here
  //====================

  return MR;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd sineClawReactionRhs(const Eigen::VectorXd &mu, double c) {
  Eigen::VectorXd rhs(mu.size());
  //====================
  // Your code goes here
  //====================
  return rhs;
}
/* SAM_LISTING_END_4 */

}  // namespace FiniteVolumeSineConsLaw


#include <math.h>
#include <cstdlib>
#include <iostream>

#include <adept.h>

#define N 100
#define FILLITER 10

using adept::adouble;

void laplacian(adouble x[N], adouble y[N])
{
    int i, p;
    y[0] = -2*x[0] + x[1];
    y[N-1] = -2*x[N-1] + x[N-2];
    for (i = 1; i < N-1; ++i) {
        y[i] = x[i-1] - 2*x[i] + x[i+1];
    }
    for (p = 0; p < FILLITER; ++p) {
        for (i = 1; i < N-1; ++i) {
            y[i] = y[i-1] - 2*y[i] + y[i+1]/(3.1415 + y[i]);
        }
    }
}

void doit(double *x, double *y, double *J)
{
    adept::Stack stack;
    adouble xad[N], yad[N];
    int i;

    adept::set_values(xad, N, x);
    stack.new_recording();

    laplacian(xad, yad);

    stack.independent(xad, N);
    stack.dependent(yad, N);
    stack.jacobian(J);

    for (i = 0; i < N; ++i) {
	y[i] = yad[i].value();
    }
}

int main() {
    double x[N];
    double y[N];
    double J[N*N];
    int rep, i, j;

    for (i = 0; i < N; ++i) {
        x[i] = 1.0 + i + 1;
    }

    for (rep = 0; rep < int(1e8/(N*N)); ++rep) {
        doit(x, y, J);
    }

    for (i = 0; i < 5; ++i) {
        std::cout << y[i] << std::endl;
    }
    for (i = 0; i < 5; ++i) {
        for (j = 0; j < 5; ++j) {
            std::cout << J[i + N*j] << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}


#include "stdafx.h"
#include "EllipticOrbit.h"
#include <tuple>
#include <limits>
#include <boost/math/tools/roots.hpp>

namespace
{
// Матчасть взята отсюда: http://www.astronet.ru/db/msg/1190817/node21.html#ll60

// Уравнение кеплера, задающее связь между эксцентрической аномалией,
// эксцентриситетом орбиты и средней аномалией
//      M = E - e * sin(E)
// где M - средняя аномалия, e - эксцентриситет орбиты, E - эксцентрическая аномалия
// уравнение `M = E - e * sin(E)` преобразуется так, чтобы слева было число 0,
// а справа - некоторая функция F(E):
//      0 = E - M - e * sin(E)
// Функтор возвращает функцию от E, а также две её производные функции.
//      F(E) = E - M - e * sin(E)
//      F'(E) = 1 - e * cos(E)
//      F''(E) = e * sin(E)
using FunctionSnapshot = boost::math::tuple<double, double, double>;
using EquationFunction = std::function<FunctionSnapshot(const double &x)>;

EquationFunction MakeKeplerEquationFunction(double meanAnomaly, double eccentricity)
{
    return [=](const double &x) {
        return boost::math::make_tuple(
                    x - meanAnomaly - eccentricity * sin(x),    // функция
                    1 - eccentricity * cos(x),                  // её первая производная
                    eccentricity * sin(x));                     // её вторая производная
    };
}

// Функция выполняет численое решение уравнения кеплера,
// вычисляя эксцентрическую аномалию
// при известных средней аномалии и эксцентриситете орбиты
// В качестве решения используется метод Halley
// (http://en.wikipedia.org/wiki/Halley's_method),
// используя соответствующие алгоритмы библиотеки boost.
double SolveKeplerEquation(double const& meanAnomaly, double const& eccentricity)
{
    const int digits = (std::numeric_limits<double>::digits) >> 1;
    // ограничиваем максимальное число итераций,
    // поскольку мы рисуем систему планет в реальном времени.
    boost::uintmax_t maxIteractions = 1000;

    return boost::math::tools::halley_iterate(
        MakeKeplerEquationFunction(meanAnomaly, eccentricity),
        meanAnomaly,                // первое приближение корня
        meanAnomaly - eccentricity, // минимальное значение корня
        meanAnomaly + eccentricity, // максимальное значение корня
        digits,                     // число разрядов
        maxIteractions);            // наибольшее число итераций
}
}

double CEllipticOrbit::Eccentricity() const
{
    return m_eccentricity;
}

double CEllipticOrbit::LargeAxis() const
{
    return m_largeAxis;
}

double CEllipticOrbit::MeanAnomaly(const double &time) const
{
    const double anomaly = 2 * M_PI * m_meanMotion * (time - m_periapsisEpoch);
    const double wrappedAnomaly = fmod(anomaly, M_PI + M_PI);
    return wrappedAnomaly;
}

double CEllipticOrbit::EccentricityAnomaly(const double &time) const
{
    return SolveKeplerEquation(MeanAnomaly(time), m_eccentricity);
}

double CEllipticOrbit::TrueAnomaly(const double &eccentricityAnomaly) const
{
    // Тангенс половинчатого угла
    const double tg_v_2 = sqrt((1 + m_eccentricity) / (1 - m_eccentricity)) * tan(eccentricityAnomaly / 2);
    // Половинчатый угол
    const double v_2 = atan(tg_v_2);
    // Истинная аномалия
    return v_2 + v_2;
}

double CEllipticOrbit::RadiusVectorLength(const double &eccentricityAnomaly) const
{
    return m_largeAxis * (1 - m_eccentricity * cos(eccentricityAnomaly));
}

glm::vec2 CEllipticOrbit::PlanetPosition2D(const double &time) const
{
    const double e = EccentricityAnomaly(time);
    const float r = float(RadiusVectorLength(e));
    const float v = float(TrueAnomaly(e));
    return { r * cosf(v), r * sinf(v) };
}


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

#ifndef HEADER_EXT_VECTOR2_HPP_INCLUDED
#define HEADER_EXT_VECTOR2_HPP_INCLUDED

#include <cmath>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Vector2 :
		boost::equality_comparable<Vector2<T>,
		boost::additive<Vector2<T>,
		boost::multiplicative<Vector2<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x;
		Type y;

		Vector2() : x{0}, y{0} {}
		Vector2(Type x_, Type y_) : x{x_}, y{y_} {}
		template <typename U>
		Vector2(Vector2<U> const& v) : x{v.x}, y{v.y} {}
	};

	using Vector2f = Vector2<float>;
	using Vector2d = Vector2<double>;
	using Vector2ld = Vector2<long double>;

	template <typename T>
	bool operator==(Vector2<T> const& v1, Vector2<T> const& v2)
	{
		return v1.x == v2.x && v1.y == v2.y;
	}

	template <typename T>
	Vector2<T>& operator+=(Vector2<T>& v1, Vector2<T> const& v2)
	{
		v1.x += v2.x;
		v1.y += v2.y;
		return v1;
	}

	template <typename T>
	Vector2<T>& operator-=(Vector2<T>& v1, Vector2<T> const& v2)
	{
		v1.x -= v2.x;
		v1.y -= v2.y;
		return v1;
	}

	template <typename T>
	Vector2<T>& operator*=(Vector2<T>& v, T t)
	{
		v.x *= t;
		v.y *= t;
		return v;
	}

	template <typename T>
	Vector2<T>& operator/=(Vector2<T>& v, T t)
	{
		v.x /= t;
		v.y /= t;
		return v;
	}

	template <typename T>
	Vector2<T> operator+(Vector2<T> const& v)
	{
		return v;
	}

	template <typename T>
	Vector2<T> operator-(Vector2<T> const& v)
	{
		return {-v.x, -v.y};
	}

	template <typename T>
	T dot(Vector2<T> const& v1, Vector2<T> const& v2)
	{
		return v1.x * v2.x + v1.y * v2.y;
	}

	template <typename T>
	T norm(Vector2<T> const& v)
	{
		return std::sqrt(dot(v, v));
	}

	template <typename T>
	Vector2<T> normalize(Vector2<T> const& v)
	{
		return v / norm(v);
	}

} // namespace ext

#endif // !HEADER_EXT_VECTOR2_HPP_INCLUDED


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

#pragma once

#include "AlgorithmUtils.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <map>

namespace fluid {
namespace algorithm {

class OnsetDetectionFuncs
{

public:
  enum class ODF {
    kEnergy,
    kHFC,
    kSpectralFlux,
    kMKL,
    kIS,
    kCosine,
    kPhaseDev,
    kWPhaseDev,
    kComplexDev,
    kRComplexDev
  };

  using ArrayXcd = Eigen::ArrayXcd;
  using ArrayXd = Eigen::ArrayXd;
  using ODFMap =
      std::map<ODF, std::function<double(ArrayXcd, ArrayXcd, ArrayXcd)>>;

  static ArrayXd wrapPhase(ArrayXd phase)
  {
    return phase.unaryExpr([=](const double p) {
      return p > (-pi) && p > pi
                 ? p
                 : p + (twoPi) * (1.0 + floor((-pi - p) / twoPi));
    });
  }

  static ODFMap& map()
  {
    static ODFMap _funcs = {

        {ODF::kEnergy,
         [](ArrayXcd cur, ArrayXcd /*prev*/, ArrayXcd /*prevprev*/) {
           return cur.abs().real().square().mean();
         }},
        {ODF::kHFC,
         [](ArrayXcd cur, ArrayXcd /*prev*/, ArrayXcd /*prevprev*/) {
           index   n = cur.size();
           ArrayXd space = ArrayXd(n);
           space.setLinSpaced(0, n);
           return (space * cur.abs().real().square()).mean();
         }},
        {ODF::kSpectralFlux,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd /*prevprev*/) {
           return (cur.abs().real() - prev.abs().real()).max(0.0).mean();
         }},
        {ODF::kMKL,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd /*prevprev*/) {
           ArrayXd mag1 = cur.abs().real().max(epsilon);
           ArrayXd mag2 = prev.abs().real().max(epsilon);
           return (mag1 / mag2).max(epsilon).log().mean();
         }},
        {ODF::kIS,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd /*prevprev*/) {
           ArrayXd mag1 = cur.abs().real().max(epsilon);
           ArrayXd mag2 = prev.abs().real().max(epsilon);
           ArrayXd ratio = (mag1 / mag2).square().max(epsilon);
           return (ratio - ratio.log() - 1).mean();
         }},
        {ODF::kCosine,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd /*prevprev*/) {
           ArrayXd mag1 = cur.abs().real().max(epsilon);
           ArrayXd mag2 = prev.abs().real().max(epsilon);
           double  norm = mag1.matrix().norm() * mag2.matrix().norm();
           double  dot = mag1.matrix().dot(mag2.matrix());
           return dot / norm;
         }},
        {ODF::kPhaseDev,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd prevprev) {
           ArrayXd phaseAcc = (cur.atan().real() - prev.atan().real()) -
                              (prev.atan().real() - prevprev.atan().real());
           return wrapPhase(phaseAcc).mean();
         }},
        {ODF::kWPhaseDev,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd prevprev) {
           ArrayXd mag1 = cur.abs().real().max(epsilon);
           ArrayXd phaseAcc = (cur.atan().real() - prev.atan().real()) -
                              (prev.atan().real() - prevprev.atan().real());
           return wrapPhase(mag1 * phaseAcc).mean();
         }},
        {ODF::kComplexDev,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd prevprev) {
           ArrayXcd target(cur.size());
           ArrayXd  prevMag = prev.abs().real().max(epsilon);
           ArrayXd  prevPhase = prev.atan().real();
           ArrayXd  phaseEst = wrapPhase(
               prevPhase + (prev.atan().real() - prevprev.atan().real()));
           target.real() = prevMag * phaseEst.cos();
           target.imag() = prevMag * phaseEst.sin();
           return (target - cur).abs().real().mean();
         }},
        {ODF::kRComplexDev,
         [](ArrayXcd cur, ArrayXcd prev, ArrayXcd prevprev) {
           ArrayXcd target(cur.size());
           ArrayXd  prevMag = prev.abs().real().max(epsilon);
           ArrayXd  prevPhase = prev.atan().real();
           ArrayXd  phaseEst = wrapPhase(
               prevPhase + (prev.atan().real() - prevprev.atan().real()));
           target.real() = prevMag * phaseEst.cos();
           target.imag() = prevMag * phaseEst.sin();
           return (target - cur).abs().real().max(0.0).mean();
         }},
    };
    return _funcs;
  }
};
} // namespace algorithm
} // namespace fluid


#include "BayesFilters/WhiteNoiseAcceleration.h"

#include <cmath>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

WhiteNoiseAcceleration::WhiteNoiseAcceleration(float T, float tilde_q, unsigned int seed) noexcept :
    T_(T), tilde_q_(tilde_q),
    generator_(std::mt19937_64(seed)), distribution_(std::normal_distribution<float>(0.0, 1.0)), gauss_rnd_sample_([&] { return (distribution_)(generator_); })
{
    F_ << 1.0,  T_, 0.0, 0.0,
          0.0, 1.0, 0.0, 0.0,
          0.0, 0.0, 1.0,  T_,
          0.0, 0.0, 0.0, 1.0;

    float q11 = 1.0/3.0 * std::pow(T_, 3.0);
    float q2  = 1.0/2.0 * std::pow(T_, 2.0);
    Q_ << q11,  q2, 0.0, 0.0,
           q2,  T_, 0.0, 0.0,
          0.0, 0.0, q11,  q2,
          0.0, 0.0,  q2,  T_;
    Q_ *= tilde_q;

    LDLT<Matrix4f> chol_ldlt(Q_);
    sqrt_Q_ = (chol_ldlt.transpositionsP() * Matrix4f::Identity()).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
}

WhiteNoiseAcceleration::WhiteNoiseAcceleration(float T, float tilde_q) noexcept :
    WhiteNoiseAcceleration(T, tilde_q, 1) { }

WhiteNoiseAcceleration::WhiteNoiseAcceleration() noexcept :
    WhiteNoiseAcceleration(1.0, 1.0, 1) { }

WhiteNoiseAcceleration::~WhiteNoiseAcceleration() noexcept { }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(const WhiteNoiseAcceleration& wna) :
    T_(wna.T_), F_(wna.F_), Q_(wna.Q_), tilde_q_(wna.tilde_q_),
    sqrt_Q_(wna.sqrt_Q_), generator_(wna.generator_), distribution_(wna.distribution_), gauss_rnd_sample_(wna.gauss_rnd_sample_) { }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(WhiteNoiseAcceleration&& wna) noexcept :
    T_(wna.T_), F_(std::move(wna.F_)), Q_(std::move(wna.Q_)), tilde_q_(wna.tilde_q_),
    sqrt_Q_(std::move(wna.sqrt_Q_)), generator_(std::move(wna.generator_)), distribution_(std::move(wna.distribution_)), gauss_rnd_sample_(std::move(wna.gauss_rnd_sample_))
{
    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(const WhiteNoiseAcceleration& wna)
{
    WhiteNoiseAcceleration tmp(wna);
    *this = std::move(tmp);

    return *this;
}

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(WhiteNoiseAcceleration&& wna) noexcept
{
    T_       = wna.T_;
    F_       = std::move(wna.F_);
    Q_       = std::move(wna.Q_);
    tilde_q_ = wna.tilde_q_;

    sqrt_Q_           = std::move(wna.sqrt_Q_);
    generator_        = std::move(wna.generator_);
    distribution_     = std::move(wna.distribution_);
    gauss_rnd_sample_ = std::move(wna.gauss_rnd_sample_);

    wna.T_       = 0.0;
    wna.tilde_q_ = 0.0;

    return *this;
}

void WhiteNoiseAcceleration::propagate(const Ref<const MatrixXf>& cur_states, Ref<MatrixXf> prop_states)
{
    prop_states = F_ * cur_states;
}

void WhiteNoiseAcceleration::motion(const Ref<const MatrixXf>& cur_states, Ref<MatrixXf> prop_states)
{
    propagate(cur_states, prop_states);

    prop_states += getNoiseSample(prop_states.cols());
}

MatrixXf WhiteNoiseAcceleration::getNoiseSample(const int num)
{
    MatrixXf rand_vectors(4, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = gauss_rnd_sample_();

    return sqrt_Q_ * rand_vectors;
}

MatrixXf WhiteNoiseAcceleration::getNoiseCovarianceMatrix()
{
    return Q_;
}


#pragma once

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

namespace dr {

/// Calculate average orientation using quaternions
template<typename DataType, typename ForwardIterator>
Eigen::Quaternion<DataType> averageQuaternions(ForwardIterator const & begin, ForwardIterator const & end) {

	if (begin == end) {
		throw std::logic_error("Cannot average orientations over an empty range.");
	}

	Eigen::Matrix<DataType, 4, 4> A = Eigen::Matrix<DataType, 4, 4>::Zero();
	uint sum(0);
	for (ForwardIterator it = begin; it != end; ++it) {
		Eigen::Matrix<DataType, 1, 4> q(1,4);
		q(0) = it->w();
		q(1) = it->x();
		q(2) = it->y();
		q(3) = it->z();
		A += q.transpose()*q;
		sum++;
	}
	A /= sum;

	Eigen::EigenSolver<Eigen::Matrix<DataType, 4, 4>> es(A);

	Eigen::Matrix<std::complex<DataType>, 4, 1> mat(es.eigenvalues());
	int index;
	mat.real().maxCoeff(&index);
	Eigen::Matrix<DataType, 4, 1> largest_ev(es.eigenvectors().real().block(0, index, 4, 1));

	return Eigen::Quaternion<DataType>(largest_ev(0), largest_ev(1), largest_ev(2), largest_ev(3));
}

/// Overloaded function to calculate the average quaternion for some container types
template<typename DataType, typename Container>
typename Eigen::Quaternion<DataType> averageQuaternions(Container const & container) {
	return averageQuaternions<DataType>(container.begin(), container.end());
}

/// Calculate average position
template<typename DataType, typename ForwardIterator>
Eigen::Matrix<DataType, 3, 1> averagePositions(ForwardIterator const & begin, ForwardIterator const & end) {
	if (begin == end) {
		throw std::logic_error("Cannot average orientations over an empty range.");
	}

	DataType x(0), y(0), z(0);
	uint count(0);
	for (ForwardIterator it = begin; it!=end; ++it) {
		x += it->x();
		y += it->y();
		z += it->z();
		count++;
	}
	x /= count;
	y /= count;
	z /= count;

	return Eigen::Matrix<DataType, 3, 1>(x,y,z);
}

/// Overloaded function to calculate the average position for some container types
template<typename DataType, typename Container>
typename Eigen::Matrix<DataType, 3, 1> averagePositions(Container const & container) {
	return averagePositions<DataType>(container.begin(), container.end());
}

template<typename DataType, typename ForwardIterator>
Eigen::Transform<DataType, 3, Eigen::Isometry> averageIsometries(ForwardIterator const & begin, ForwardIterator const & end) {

	std::vector<Eigen::Vector3d> positions;
	std::vector<Eigen::Quaterniond> quaternions;

	for (auto it = begin; it != end; ++it) {
		positions.push_back(it->translation());
		quaternions.push_back(Eigen::Quaternion<DataType>(it->rotation()));
	}

	return Eigen::Translation<DataType, 3>(averagePositions<DataType>(positions)) * averageQuaternions<DataType>(quaternions);
}

template<typename DataType, typename Container>
Eigen::Transform<DataType, 3, Eigen::Isometry> averageIsometries(Container const & container) {
	return averageIsometries<DataType>(container.begin(), container.end());
}

}


/*
 * This file is part of the ProVANT simulator project.
 * Licensed under the terms of the MIT open source license. More details at
 * https://github.com/Guiraffo/ProVANT-Simulator/blob/master/LICENSE.md
 */
/**
 * @file This file contains the implementation of the LQRQuadcopter class.
 *
 * @author Jonatan Campos
 */

#include <control_strategies_base/icontroller.hpp>
#include <Eigen/Eigen>
#include "simulator_msgs/Sensor.h"
#include <ros/ros.h>

static const int NINPUTS = 4;
static const int NSTATES = 12;

/**
 * @brief The LQRQuadcopter implements a Linear Quadratic Regulator control
 * strategy for the quadrotor UAV that is available in the ProVANT simulator.
 *
 * This controller uses a helicoidal reference trajectory.
 *
 * This control strategy expects a state vector composed of the generalized
 * coordinates and their derivatives in the following order:
 * [x y z phi theta psi x_dot y_dot z_dot phi_dot theta_dot psi_dot]
 *
 * x, y and z are the cartesian coordinates of the UAV center of rotation in
 * relation with the inertial reference frame and expressed in meters,
 * phi is the rotation around the x axis, theta is the rotation around the y
 * axis and psi is the rotation around the z axis, all of the angles of rotation
 * are expressed in radians.
 *
 * x_dot, y_dot and z_dot are the time derivatives of the cartesian coordinates,
 * that equal the linear velocities of the UAV in relation with the inertial
 * frame. The values are given in m/s.
 *
 * Finally, phi_dot, theta_dot and psi_dot are the time derivatives of the Euler
 * Angles, given in rad/s.
 *
 * The control inputs of this system are the forces generated by each propeller
 * in the order f1, f2, f3 and f4, where fi is the force of the i-th propeller.
 * All of the forces are expressed in Newtons.
 *
 */
class LQRQuadcopter : public Icontroller
{
private:
  //! Stores the reference vector
  Eigen::VectorXd Xref;
  //! Stores the error vector
  Eigen::VectorXd Erro;
  //! Stores the control inputs
  Eigen::VectorXd Input;
  //! Stores the gain matrix of the system
  Eigen::MatrixXd K;
  //! Stores the state vector
  Eigen::VectorXd X;

public:
  /**
   * @brief Construct a new LQRQuadcopter object and initialize the size of
   * the matrices used in the controller.
   */
  LQRQuadcopter() : Xref(NSTATES), K(NINPUTS, NSTATES), X(NSTATES), Erro(NSTATES), Input(NINPUTS)
  {
    config();
  }

  /**
   * @brief Destroy the LQRQuadcopter object.
   */
  virtual ~LQRQuadcopter()
  {
  }

  /**
   * @brief Initialize the gain matrix K.
   */
  void config()
  {
    K << -70.711, 2.1302e-11, 7.0711, -2.3813e-12, -156.46, 5, -165.1, 4.7388e-11, 11.53, 8.2065e-13, -7.5435, 5.7987,
        1.3204e-11, -70.711, 7.0711, 156.46, 8.257e-11, -5, 3.1756e-11, -165.1, 11.53, 7.5435, 8.1385e-12, -5.7987,
        70.711, 3.9104e-11, 7.0711, -4.7265e-11, 156.46, 5, 165.1, 8.9701e-11, 11.53, 9.2039e-13, 7.5435, 5.7987,
        -5.6384e-12, 70.711, 7.0711, -156.46, 6.4636e-11, -5, -8.3103e-12, 165.1, 11.53, -7.5435, 8.4109e-12, -5.7987;
  }

  /**
   * @brief Execute the LQR control law.
   *
   * This method is called every simulation step.
   * It is responsible for reading the state vector from the arraymsg,
   * calculating the reference trajectory, the error vector, and must return
   * the control inputs to be applied on the system.
   *
   * @param arraymsg Array of simulator messages containing the sensors
   * of the quadrotor model. In this case, the only sensor is the state vector.
   * @returns Control inputs vector to be applied on the system. See the
   * class documentation for more details.
   */
  std::vector<double> execute(simulator_msgs::SensorArray arraymsg)
  {
    // Search for the state vector in the recevied messages
    simulator_msgs::Sensor msg;
    bool found = false;
    for (int i = 0; i < arraymsg.values.size(); i++)
    {
      if (arraymsg.values.at(i).name == "Estados")
      {
        msg = arraymsg.values.at(i);
        found = true;
        break;
      }
    }
    if (!found)
    {
      // In case of error, report the problem in a ROS_LOG, and returns an
      // empty array
      ROS_FATAL("[lqr_quadcopter] State vector not found.");
      std::vector<double> out(Input.data(), Input.data() + Input.size());
      return out;
    }

    // Get the current simulation time and calculate the reference trajectory.
    ros::Time time = ros::Time::now();
    double tempo = time.toSec();

    Xref << sin(tempo / 2), cos(tempo / 2), tempo / 10, 0, 0, 0, 0, 0, 0, 0, 0, 0;

    // Read the values of the state vector from the message
    X << msg.values.at(0),  // x
        msg.values.at(1),   // y
        msg.values.at(2),   // z
        msg.values.at(3),   // roll
        msg.values.at(4),   // pitch
        msg.values.at(5),   // yaw
        msg.values.at(6),   // x_dot
        msg.values.at(7),   // y_dot
        msg.values.at(8),   // z_dot
        msg.values.at(9),   // roll_dot
        msg.values.at(10),  // pitch_dot
        msg.values.at(11);  // yaw_dot

    // Calculate the error vector of the system
    Erro = X - Xref;

    // Execute the control law (Multiply the gain matrix by the error vector)
    Input = -K * Erro;

    // Add equilibirium forces to the outputs.

    Input(0) = Input(0) + 5.77611;
    Input(1) = Input(1) + 5.77611;
    Input(2) = Input(2) + 5.77611;
    Input(3) = Input(3) + 5.77611;

    ROS_DEBUG_STREAM_NAMED("lqr_quadcopter", "[LQRQuadcopter]: F1: " << Input(0));
    ROS_DEBUG_STREAM_NAMED("lqr_quadcopter", "[LQRQuadcopter]: F2: " << Input(1));
    ROS_DEBUG_STREAM_NAMED("lqr_quadcopter", "[LQRQuadcopter]: F3: " << Input(2));
    ROS_DEBUG_STREAM_NAMED("lqr_quadcopter", "[LQRQuadcopter]: F4: " << Input(3));

    // Copy the control inputs values to the output vector
    std::vector<double> out(Input.data(), Input.data() + Input.size());
    return out;
  }

  /**
   * @brief Reference()
   *
   * This method is called by the logging functions to report the
   * reference values for the current step.
   *
   * @returns Reference vector for the current step. This vector is in the same
   * order of the state vector.
   */
  std::vector<double> Reference()
  {
    // Copy the references to a std::vector
    std::vector<double> out(Xref.data(), Xref.data() + Xref.rows() * Xref.cols());
    return out;
  }

  /**
   * @brief Error()
   *
   * This method is called by the logging functions to get the error vector
   * used in the control law execution.
   *
   * @returns The error vector of the system in the current step. The error
   * vector is in the same order of the state vector.
   */
  std::vector<double> Error()
  {
    // Copy the references to a std::vector
    std::vector<double> out(Erro.data(), Erro.data() + Erro.rows() * Erro.cols());
    return out;
  }

  /**
   * @brief State()
   *
   * This method is called by the logging functions to get the state vector
   * used in the control law execution.
   * Note that if the implmeneted controller uses an augmented state vector,
   * for example, with integral sates, these values should also be returned
   * by this function.
   *
   * @returns The state vector of the system used in the calculation of the
   * control law in the current step time. This vector is in the same order
   * as the states vector.
   */
  std::vector<double> State()
  {
    std::vector<double> out(X.data(), X.data() + X.rows() * X.cols());
    return out;
  }
};

// Functions used by the ProVANT Simulator to instantiate this control law
extern "C" {
Icontroller* create(void)
{
  return new LQRQuadcopter;
}
void destroy(Icontroller* p)
{
  delete p;
}
}


#ifndef _CELERITE2_FORWARD_HPP_DEFINED_
#define _CELERITE2_FORWARD_HPP_DEFINED_

#include <Eigen/Core>
#include "internal.hpp"
namespace celerite2 {
namespace core {

/**
 * \brief Get the dense representation of a celerite matrix
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param a     (N,): The diagonal component
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param K_out (N, N): The dense matrix
 */
template <typename Input, typename Coeffs, typename Diag, typename LowRank, typename Dense>
void to_dense(const Eigen::MatrixBase<Input> &t,    // (N,)
              const Eigen::MatrixBase<Coeffs> &c,   // (J,)
              const Eigen::MatrixBase<Diag> &a,     // (N,)
              const Eigen::MatrixBase<LowRank> &U,  // (N, J)
              const Eigen::MatrixBase<LowRank> &V,  // (N, J)
              Eigen::MatrixBase<Dense> const &K_out // (N,)
) {
  typedef typename Eigen::internal::plain_row_type<LowRank>::type RowVector;

  Eigen::Index N = U.rows(), J = U.cols();
  CAST_MAT(Dense, K, N, N);

  RowVector p(1, J);
  for (Eigen::Index m = 0; m < N; ++m) {
    p.setConstant(1.0);
    K(m, m) = a(m);
    for (Eigen::Index n = m + 1; n < N; ++n) {
      p.array() *= exp(-c.array() * (t(n) - t(n - 1)));
      K(n, m) = (U.row(n).array() * V.row(m).array() * p.array()).sum();
      K(m, n) = K(n, m);
    }
  }
}

/**
 * \brief Compute the Cholesky factorization of the system
 *
 * This computes `d` and `W` such that:
 *
 * `K = L*diag(d)*L^T`
 *
 * where `K` is the celerite matrix and
 *
 * `L = 1 + tril(U*W^T)`
 *
 * This can be safely applied in place: `d_out` can point to `a` and `W_out` can
 * point to `V`, and the memory will be reused. In this particular case, the
 * `celerite2::core::factor_rev` function doesn't use `a` and `V`, but this
 * won't be true for all `_rev` functions.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param a     (N,): The diagonal component
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param d_out (N,): The diagonal component of the Cholesky factor
 * @param W_out (N, J): The second low rank component of the Cholesky factor
 * @param S_out (N, J*J): The cached value of the S matrix at each step
 */
template <bool update_workspace = true, typename Input, typename Coeffs, typename Diag, typename LowRank, typename DiagOut, typename LowRankOut,
          typename Work>
Eigen::Index factor(const Eigen::MatrixBase<Input> &t,          // (N,)
                    const Eigen::MatrixBase<Coeffs> &c,         // (J,)
                    const Eigen::MatrixBase<Diag> &a,           // (N,)
                    const Eigen::MatrixBase<LowRank> &U,        // (N, J)
                    const Eigen::MatrixBase<LowRank> &V,        // (N, J)
                    Eigen::MatrixBase<DiagOut> const &d_out,    // (N,)
                    Eigen::MatrixBase<LowRankOut> const &W_out, // (N, J)
                    Eigen::MatrixBase<Work> const &S_out        // (N, J*J)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename Diag::Scalar Scalar;
  typedef typename Eigen::internal::plain_row_type<LowRank>::type RowVector;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;

  Eigen::Index N = U.rows(), J = U.cols();
  CAST_VEC(DiagOut, d, N);
  CAST_MAT(LowRankOut, W, N, J);
  CAST_BASE(Work, S);
  if (update_workspace) {
    S.derived().resize(N, J * J);
    S.row(0).setZero();
  }

  // This is a temporary vector used to minimize computations internally
  RowVector tmp;
  CoeffVector p;

  // This holds the accumulated value of the S matrix at each step
  Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, LowRank::ColsAtCompileTime, Eigen::ColMajor> Sn(J, J);

  // This is a flattened pointer to Sn that is used for copying the data
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Sn.data(), 1, J * J);

  // First row
  Sn.setZero();
  d(0)               = a(0);
  W.row(0).noalias() = V.row(0) / d(0);

  // The rest of the rows
  for (Eigen::Index n = 1; n < N; ++n) {
    p = exp(c.array() * (t(n - 1) - t(n)));

    // Update S_n = diag(P) * (S_n-1 + d*W*W.T) * diag(P)
    Sn.noalias() += d(n - 1) * W.row(n - 1).transpose() * W.row(n - 1);
    Sn = p.asDiagonal() * Sn;

    // Save the current value of Sn to the workspace
    // Note: This is actually `diag(P) * (S + d*W*W.T)` without the final `* diag(P)`
    internal::update_workspace<update_workspace>::apply(n, ptr, S);

    // Incorporate the second diag(P) that we didn't include above for bookkeeping
    Sn *= p.asDiagonal();

    // Update d = a - U * S * U.T
    tmp  = U.row(n) * Sn;
    d(n) = a(n) - tmp * U.row(n).transpose();
    if (d(n) <= 0.0) return n;

    // Update W = (V - U * S) / d
    W.row(n).noalias() = (V.row(n) - tmp) / d(n);
  }

  return 0;
}

/**
 * \brief Apply a strictly lower matrix multiply
 *
 * This computes:
 *
 * `Z += tril(U * V^T) * Y`
 *
 * where `tril` is the strictly lower triangular function.
 *
 * Note that this will *update* the value of `Z`.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param W     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The matrix to be multiplied
 * @param Z_out (N, Nrhs): The matrix to be updated
 * @param F_out (N, J*Nrhs): The workspace
 */
template <bool update_workspace = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void solve_lower(const Eigen::MatrixBase<Input> &t,                // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                 const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                 const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                 const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                 Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                 Eigen::MatrixBase<Work> const &F_out              // (N, J*nrhs)
) {
  ASSERT_ROW_MAJOR(Work);
  CAST_BASE(RightHandSideOut, Z);
  Z = Y;
  internal::forward<true, update_workspace>(t, c, U, W, Y, Z, F_out);
}

/**
 * \brief Compute the solution of a upper triangular linear equation
 *
 * This computes `Z` such that:
 *
 * `Y = L^T * Y`
 *
 * where
 *
 * `L = 1 + tril(U*W^T)`
 *
 * This can be safely applied in place.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param W     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The right hand side
 * @param Z_out (N, Nrhs): The solution of this equation
 * @param F_out (N, J*Nrhs): The workspace
 */
template <bool update_workspace = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void solve_upper(const Eigen::MatrixBase<Input> &t,                // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                 const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                 const Eigen::MatrixBase<LowRank> &W,              // (N, J)
                 const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                 Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                 Eigen::MatrixBase<Work> const &F_out              // (N, J*nrhs)
) {
  ASSERT_ROW_MAJOR(Work);
  CAST_BASE(RightHandSideOut, Z);
  Z = Y;
  internal::backward<true, update_workspace>(t, c, U, W, Y, Z, F_out);
}

/**
 * \brief Apply a strictly lower matrix multiply
 *
 * This computes:
 *
 * `Z += tril(U * V^T) * Y`
 *
 * where `tril` is the strictly lower triangular function.
 *
 * Note that this will *update* the value of `Z`.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The matrix to be multiplied
 * @param Z_out (N, Nrhs): The matrix to be updated
 * @param F_out (N, J*Nrhs): The workspace
 */
template <bool update_workspace = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void matmul_lower(const Eigen::MatrixBase<Input> &t,                // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                  const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                  Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                  Eigen::MatrixBase<Work> const &F_out              // (N, J*nrhs)
) {
  internal::forward<false, update_workspace>(t, c, U, V, Y, Z_out, F_out);
}

/**
 * \brief Apply a strictly upper matrix multiply
 *
 * This computes:
 *
 * `Z += triu(V * U^T) * Y`
 *
 * where `triu` is the strictly lower triangular function.
 *
 * Note that this will *update* the value of `Z`.
 *
 * @param t     (N,): The input coordinates (must be sorted)
 * @param c     (J,): The transport coefficients
 * @param U     (N, J): The first low rank matrix
 * @param V     (N, J): The second low rank matrix
 * @param Y     (N, Nrhs): The matrix to be multiplied
 * @param Z_out (N, Nrhs): The matrix to be updated
 * @param F_out (N, J*Nrhs): The workspace
 */
template <bool update_workspace = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void matmul_upper(const Eigen::MatrixBase<Input> &t,                // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                  const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,              // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,        // (N, nrhs)
                  Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                  Eigen::MatrixBase<Work> const &F_out              // (N, J*nrhs)
) {
  internal::backward<false, update_workspace>(t, c, U, V, Y, Z_out, F_out);
}

/**
 * \brief The general lower-triangular dot product of a rectangular celerite system
 *
 * @param t1     (N,): The left input coordinates (must be sorted)
 * @param t2     (M,): The right input coordinates (must be sorted)
 * @param c      (J,): The transport coefficients
 * @param U      (N, J): The first low rank matrix
 * @param V      (M, J): The second low rank matrix
 * @param Y      (M, Nrhs): The matrix that will be left multiplied by the celerite model
 * @param Z_out  (N, Nrhs): The result of the operation
 * @param F_out  (M, J*Nrhs): The workspace
 */
template <bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void general_matmul_lower(const Eigen::MatrixBase<Input> &t1,               // (N,)
                          const Eigen::MatrixBase<Input> &t2,               // (M,)
                          const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                          const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                          const Eigen::MatrixBase<LowRank> &V,              // (M, J)
                          const Eigen::MatrixBase<RightHandSide> &Y,        // (M, nrhs)
                          Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                          Eigen::MatrixBase<Work> const &F_out              // (M, J*nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = t1.rows(), M = t2.rows(), J = c.rows(), nrhs = Y.cols();

  CAST_BASE(RightHandSideOut, Z);
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(M, J * nrhs);
    F.row(0).setZero();
  }

  CoeffVector p(J);
  Inner Fm = V.row(0).transpose() * Y.row(0);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fm.data(), 1, J * nrhs);
  internal::update_workspace<do_update>::apply(0, ptr, F);

  Scalar tn = t2(0);
  Eigen::Index n, m = 1;
  for (n = 0; n < N; ++n)
    if (t1(n) >= tn) break;
  for (; n < N; ++n) {
    tn = t1(n);
    while (m < M && t2(m) <= tn) {
      p  = exp(c.array() * (t2(m - 1) - t2(m)));
      Fm = p.asDiagonal() * Fm;
      Fm.noalias() += V.row(m).transpose() * Y.row(m);
      internal::update_workspace<do_update>::apply(m, ptr, F);
      m++;
    }
    p = exp(c.array() * (t2(m - 1) - tn));
    Z.row(n).noalias() += U.row(n) * p.asDiagonal() * Fm;
  }
}

/**
 * \brief The general upper-triangular dot product of a rectangular celerite system
 *
 * @param t1     (N,): The left input coordinates (must be sorted)
 * @param t2     (M,): The right input coordinates (must be sorted)
 * @param c      (J,): The transport coefficients
 * @param U      (N, J): The first low rank matrix
 * @param V      (M, J): The second low rank matrix
 * @param Y      (M, Nrhs): The matrix that will be left multiplied by the celerite model
 * @param Z_out  (N, Nrhs): The result of the operation
 * @param F_out  (M, J*Nrhs): The workspace
 */
template <bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void general_matmul_upper(const Eigen::MatrixBase<Input> &t1,               // (N,)
                          const Eigen::MatrixBase<Input> &t2,               // (M,)
                          const Eigen::MatrixBase<Coeffs> &c,               // (J,)
                          const Eigen::MatrixBase<LowRank> &U,              // (N, J)
                          const Eigen::MatrixBase<LowRank> &V,              // (M, J)
                          const Eigen::MatrixBase<RightHandSide> &Y,        // (M, nrhs)
                          Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, nrhs)
                          Eigen::MatrixBase<Work> const &F_out              // (M, J*nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = t1.rows(), M = t2.rows(), J = c.rows(), nrhs = Y.cols();

  CAST_BASE(RightHandSideOut, Z);
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(M, J * nrhs);
    F.row(0).setZero();
  }

  CoeffVector p(J);
  Inner Fm = V.row(M - 1).transpose() * Y.row(M - 1);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fm.data(), 1, J * nrhs);

  Scalar tn = t2(M - 1);
  Eigen::Index n, m = M - 2;
  for (n = N - 1; n >= 0; --n)
    if (t1(n) < tn) break;
  for (; n >= 0; --n) {
    tn = t1(n);
    while (m >= 0 && t2(m) > tn) {
      p  = exp(c.array() * (t2(m) - t2(m + 1)));
      Fm = p.asDiagonal() * Fm;
      Fm.noalias() += V.row(m).transpose() * Y.row(m);
      internal::update_workspace<do_update>::apply(m, ptr, F);
      m--;
    }
    p = exp(c.array() * (tn - t2(m + 1)));
    Z.row(n).noalias() += U.row(n) * p.asDiagonal() * Fm;
  }
}

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_FORWARD_HPP_DEFINED_


#ifndef SE3_HPP_
#define SE3_HPP_

#include <Eigen/Geometry>

/**
 * @brief Bare-bones implementation of the SE(3) Lie Group
 *
 * For benchmarking purposes only---do not use in important code.
 */
template<typename _Scalar>
struct SE3
{
  using Scalar = _Scalar;
  using Tangent = Eigen::Matrix<Scalar, 6, 1>;

  // group composition
  SE3<Scalar> operator*(const SE3<Scalar> & other) const
  {
    return SE3<Scalar>{q * other.q, t + q * other.t};
  }

  // in-place group composition
  SE3<Scalar> & operator*=(const SE3<Scalar> & other)
  {
    t += q * other.t;
    q *= other.q;
    return *this;
  }

  // group action on 3-vector
  template<typename Derived>
  Derived operator*(const Eigen::EigenBase<Derived> & vec) const
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3)

    return t + q * vec;
  }

  // group inverse
  SE3<Scalar> inv() const
  {
    return SE3<Scalar>{q.inverse(), -(q.inverse() * t)};
  }

  // cast to different scalar type
  template<typename NewScalar>
  SE3<NewScalar> cast() const
  {
    return SE3<NewScalar>{q.template cast<NewScalar>(), t.template cast<NewScalar>()};
  }

  // exponential (does not work for special case of zero orientation (yields 0/0))
  template<typename Derived>
  static SE3<Scalar> exp(const Eigen::MatrixBase<Derived> & tangent)
  {
    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent, Derived)

    using std::sin, std::cos;

    const Scalar w_norm = tangent.template tail<3>().norm();
    const Eigen::Matrix<Scalar, 3, 1> q_diff_n = tangent.template tail<3>().normalized();

    Eigen::Matrix<Scalar, 3, 3> what;
    what << Scalar(0), -q_diff_n(2), q_diff_n(1),
      q_diff_n(2), Scalar(0), -q_diff_n(0),
      -q_diff_n(1), q_diff_n(0), Scalar(0);

    const Eigen::Matrix<Scalar, 3, 3> J = Eigen::Matrix<Scalar, 3, 3>::Identity() +
      ((w_norm - sin(w_norm)) / w_norm) * what * what +
      ((Scalar(1.) - cos(w_norm)) / w_norm) * what;

    return SE3<Scalar>{
      Eigen::Quaternion<Scalar>(Eigen::AngleAxis<Scalar>(w_norm, q_diff_n)),
      J * tangent.template head<3>()
    };
  }

  // log (does not work for special case of zero orientation (yields 0/0))
  Tangent log() const
  {
    using std::atan2;

    const Scalar alpha = atan2(q.coeffs().template head<3>().norm(), q.w());

    const Eigen::Matrix<Scalar, 3, 1> so3log =
      Scalar(2.) * (alpha / sin(alpha)) * q.coeffs().template head<3>();

    // TODO: could derive more efficient expression for the inverse...
    const Scalar w_norm = so3log.norm();
    const Eigen::Matrix<Scalar, 3, 1> q_diff_n = so3log.normalized();

    Eigen::Matrix<Scalar, 3, 3> what;
    what << Scalar(0), -q_diff_n(2), q_diff_n(1),
      q_diff_n(2), Scalar(0), -q_diff_n(0),
      -q_diff_n(1), q_diff_n(0), Scalar(0);

    const Eigen::Matrix<Scalar, 3, 3> J = Eigen::Matrix<Scalar, 3, 3>::Identity() +
      ((w_norm - sin(w_norm)) / w_norm) * what * what +
      ((Scalar(1.) - cos(w_norm)) / w_norm) * what;

    return (Tangent() << J.inverse() * t, so3log).finished();
  }

  Eigen::Quaternion<Scalar> q = Eigen::Quaternion<Scalar>::Identity();
  Eigen::Matrix<Scalar, 3, 1> t = Eigen::Matrix<Scalar, 3, 1>::Zero();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * @brief boost::numerical::odeint operations for numerical integration on lie groups
 */
struct lie_operations
{
  template<class Fac1 = double, class Fac2 = Fac1>
  struct scale_sum2
  {
    const Fac2 m_alpha2;

    scale_sum2(Fac1 alpha1, Fac2 alpha2)
    : m_alpha2(alpha2)
    {
      if (alpha1 != Fac1(1.0)) {
        std::cerr << "alpha1 != 1 not expected for Lie type" << std::endl;
        exit(EXIT_FAILURE);
      }
    }

    template<class T1, class T2, class T3>
    void operator()(T1 & t1, const T2 & t2, const T3 & t3) const
    {
      t1 = t2 * T2::exp(t3 * m_alpha2);
    }

    typedef void result_type;
  };

  template<class Fac1 = double, class Fac2 = Fac1, class Fac3 = Fac2>
  struct scale_sum3
  {
    const Fac2 m_alpha2;
    const Fac3 m_alpha3;

    scale_sum3(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3)
    : m_alpha2(alpha2), m_alpha3(alpha3)
    {
      if (alpha1 != Fac1(1.0)) {
        std::cerr << "alpha1 != 1 not expected for Lie type" << std::endl;
        exit(EXIT_FAILURE);
      }
    }

    template<class T1, class T2, class T3, class T4>
    void operator()(T1 & t1, const T2 & t2, const T3 & t3, const T4 & t4) const
    {
      t1 = t2 * T2::exp(t3 * m_alpha2 + t4 * m_alpha3);
    }

    typedef void result_type;
  };

  template<class Fac1 = double, class Fac2 = Fac1, class Fac3 = Fac2, class Fac4 = Fac3>
  struct scale_sum4
  {
    const Fac2 m_alpha2;
    const Fac3 m_alpha3;
    const Fac4 m_alpha4;

    scale_sum4(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4)
    : m_alpha2(alpha2), m_alpha3(alpha3), m_alpha4(alpha4)
    {
      if (alpha1 != Fac1(1.0)) {
        std::cerr << "alpha1 != 1 not expected for Lie type" << std::endl;
        exit(EXIT_FAILURE);
      }
    }

    template<class T1, class T2, class T3, class T4, class T5>
    void operator()(T1 & t1, const T2 & t2, const T3 & t3, const T4 & t4, const T5 & t5) const
    {
      t1 = t2 * T2::exp(t3 * m_alpha2 + t4 * m_alpha3 + t5 * m_alpha4);
    }

    typedef void result_type;
  };

  template<class Fac1 = double, class Fac2 = Fac1, class Fac3 = Fac2, class Fac4 = Fac3,
    class Fac5 = Fac4>
  struct scale_sum5
  {
    const Fac2 m_alpha2;
    const Fac3 m_alpha3;
    const Fac4 m_alpha4;
    const Fac5 m_alpha5;

    scale_sum5(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5)
    : m_alpha2(alpha2), m_alpha3(alpha3), m_alpha4(alpha4), m_alpha5(alpha5)
    {
      if (alpha1 != Fac1(1.0)) {
        std::cerr << "alpha1 != 1 not expected for Lie type" << std::endl;
        exit(EXIT_FAILURE);
      }
    }

    template<class T1, class T2, class T3, class T4, class T5, class T6>
    void operator()(
      T1 & t1, const T2 & t2, const T3 & t3, const T4 & t4, const T5 & t5,
      const T6 & t6) const
    {
      t1 = t2 * T2::exp(t3 * m_alpha2 + t4 * m_alpha3 + t5 * m_alpha4 + t6 * m_alpha5);
    }

    typedef void result_type;
  };
};

#endif  // SE3_HPP_


#include <cassert>
#include <cstdio>
#include <cstring>
#include <map>
#include <vector>

#include <Eigen/Core>

#include "igl/components.h"
#include "igl/jet.h"
#include "igl/polygon_mesh_to_triangle_mesh.h"
#include "igl/readOFF.h"
#include "igl/viewer/Viewer.h"
#include "igl/writeOFF.h"

double volume(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F) {

  double total_det = 0;

  for (int i = 0; i < F.rows(); ++i) {
    Eigen::Matrix3d p;
    p << V(F(i, 0), 0), V(F(i, 1), 0), V(F(i, 2), 0),
         V(F(i, 0), 1), V(F(i, 1), 1), V(F(i, 2), 1),
         V(F(i, 0), 2), V(F(i, 1), 2), V(F(i, 2), 2);

    total_det += p.determinant();
  }

  return std::abs(total_det / 6.0);
}

void selectValidVerts(const Eigen::MatrixXd &Vi, const Eigen::MatrixXi &Fi,
                      const Eigen::VectorXi &Ci, int comp_num,
                      Eigen::MatrixXd &Vo, Eigen::MatrixXi &Fo) {
  printf("Extracting component number %d\n", comp_num);

  std::map<int, int> old_to_new;
  int n_i = 0;
  for (int i = 0; i < Vi.rows(); ++i) {
    if (Ci(i) == comp_num) {
      old_to_new[i] = n_i;
      n_i++;
    } else {
      // not valid
      old_to_new[i] = -1;
    }
  }
  printf("  Found %d/%d valid vertices\n", n_i, Vi.rows());

  // Resize the vertices to be the number we need.
  Vo.resize(n_i, 3);
  int v_idx = 0;
  for (int i = 0; i < Vi.rows(); ++i) {
    if (old_to_new[i] != -1) {
      Vo.row(v_idx++) = Vi.row(i);
    }
  }
  // Same thing for the faces.
  // Do this twice so we know how many we need.
  std::vector<int> f_idxs;
  for (int i = 0; i < Fi.rows(); ++i) {
    bool contained = true;
    for (int j = 0; j < Fi.row(i).size(); ++j) {
      if (old_to_new[Fi(i, j)] == -1) {
        contained = false;
        break;
      }
    }
    if (contained) {
      f_idxs.push_back(i);
    }
  }
  fprintf(stderr,"  Found %d/%d valid faces\n", f_idxs.size(), Fi.rows());
  Fo.resize(f_idxs.size(), 3);
  for (int i = 0; i < f_idxs.size(); ++i) {
    for (int j = 0; j < 3; ++j) {
      Fo(i, j) = old_to_new[Fi(f_idxs[i], j)];
    }
  }
  printf("  Done! has size %dx%d\n", Fo.rows(), Fo.cols());
}

void extractValidVerts(const Eigen::MatrixXd &Vi, const Eigen::MatrixXi &Fi,
                       const Eigen::VectorXi &Ci, const std::vector<bool> &valid,
                       Eigen::MatrixXd &Vo, Eigen::MatrixXi &Fo, Eigen::VectorXi &Co) {
  std::map<int, int> old_to_new;
  int n_i = 0;
  for (int i = 0; i < Vi.rows(); ++i) {
    if (valid[Ci(i)]) {
      old_to_new[i] = n_i;
      n_i++;
    } else {
      // not valid
      old_to_new[i] = -1;
    }
  }
  printf("  Found %d/%d valid vertices\n", n_i, Vi.rows());

  // Resize the vertices to be the number we need.
  Vo.resize(n_i, 3);
  Co.resize(n_i);
  int v_idx = 0;
  for (int i = 0; i < Vi.rows(); ++i) {
    if (old_to_new[i] != -1) {
      Co(v_idx) = Ci(i);
      Vo.row(v_idx++) = Vi.row(i);
    }
  }
  // Same thing for the faces.
  // Do this twice so we know how many we need.
  std::vector<int> f_idxs;
  for (int i = 0; i < Fi.rows(); ++i) {
    bool contained = true;
    for (int j = 0; j < Fi.row(i).size(); ++j) {
      if (old_to_new[Fi(i, j)] == -1) {
        contained = false;
        break;
      }
    }
    if (contained) {
      f_idxs.push_back(i);
    }
  }
  printf("  Found %d/%d valid faces\n", f_idxs.size(), Fi.rows());
  Fo.resize(f_idxs.size(), 3);
  for (int i = 0; i < f_idxs.size(); ++i) {
    for (int j = 0; j < 3; ++j) {
      Fo(i, j) = old_to_new[Fi(f_idxs[i], j)];
    }
  }
  printf("  Done! has size %dx%d\n", Fo.rows(), Fo.cols());
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    fprintf(stderr, "Used for selecting connected components and saving them to a file.\n");
    fprintf(stderr, "usage: %s <input.off> <output.off>\n", argv[0]);
    return -1;
  }

  Eigen::MatrixXd Vi, Vo;
  Eigen::MatrixXi Fi, Fo;
  Eigen::VectorXi Comps;

  // Read in the input file.
  igl::readOFF(argv[1], Vi, Fi);
  igl::components(Fi, Comps);
  // Get the number of components
  int max_comp = Comps.maxCoeff();
  std::vector<bool> valid_comps(max_comp + 1);

  int component_num = 0;
  igl::viewer::Viewer viewer;
  viewer.callback_init = [&](igl::viewer::Viewer& viewer) {
    viewer.callback_key_down = [&](igl::viewer::Viewer& viewer, unsigned char key, int modifier) {
      // Can increase or decrease number of components.
      if (key == 'C' || key == 'V') {
        if (key == 'C') {
          printf("Increasing component with %c\n", key);
          component_num++;
          if (component_num > max_comp) {
            component_num = 0;
          }
        } else {
          printf("Decreasing component with %c\n", key);
          component_num--;
          component_num = std::max(component_num, 0);
        }
        selectValidVerts(Vi,Fi,Comps, component_num, Vo,Fo);
        double comp_vol = volume(Vo,Fo);
        printf("Selecting component %d with volume %lf. Press 'X' to delete, ' ' to include\n",
               component_num, comp_vol);
        viewer.data.clear();
        viewer.data.set_mesh(Vo, Fo);
        return true;

      } else if (key == ' ' || key == 'X') {
        if (key == ' ') {
          valid_comps[component_num] = true;
        } else {
          valid_comps[component_num] = false;
        }

        Eigen::VectorXi Co;
        Eigen::MatrixXd cols;
        extractValidVerts(Vi,Fi,Comps, valid_comps, Vo,Fo,Co);
        igl::jet(Co, true, cols);
        viewer.data.clear();
        viewer.data.set_mesh(Vo, Fo);
        viewer.data.set_colors(cols);
        return true;
      } else if (key == 'w' || key == 'W') {
        int n_comps = 0;
        for (int i = 0; i < valid_comps.size(); ++i) {
          if (valid_comps[i]) ++n_comps;
        }
        printf("Writing output with %d components to %s\n", n_comps, argv[2]);
        igl::writeOFF(argv[2], Vo,Fo);
        printf("Finished! You may close the program.\n");
      }
      return false;
    };
    viewer.screen->performLayout();
    return false;
  };

  Eigen::MatrixXd cols;
  igl::jet(Comps, true, cols);
  viewer.data.set_mesh(Vi, Fi);
  viewer.data.set_colors(cols);
  printf("Press 'C' or 'V' to (respectively) increase or decrease the components id.\n");
  printf("Press ' ' to add the current component to the total components\n");
  printf("Press 'x' to remove the current component from the total components\n");
  printf("Press 'w' to write the resulting mesh to the output file\n");
  viewer.launch();
}


#ifndef SDM_STATS_HPP_
#define SDM_STATS_HPP_

#include "utils.hpp"

#include <boost/math/distributions.hpp>

namespace sdm {

// Eigen's Matrix class has mean function

template <typename RealType> double mean(const std::vector<RealType> &vec) {
  double sum = 0.0;
  if (!vec.empty()) {
    for (auto i = vec.begin(); i != vec.end(); ++i)
      sum += *i;
    sum /= static_cast<RealType>(vec.size());
  }
  return sum;
}

// trimProb% trimmed mean
template <typename RealType, int Options>
inline double
trimedMean(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
           const double trimProb = 0.5) {
  int l = vec.size();
  Eigen::Matrix<RealType, Eigen::Dynamic, 1> v_copy = vec;
  std::sort(v_copy.data(), v_copy.data() + v_copy.size());
  return (v_copy.segment((int)(nearbyint(l * trimProb)),
                         (int)(nearbyint(l * (1.0 - trimProb))))).mean();
}

// ----- scale mesure -----
template <typename RealType, int Options>
inline double
vari(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
     const double the_mean) {
  double v = 0.0;
  if (vec.size() > 2)
    v = ((vec.array() - the_mean).square().sum()) / (vec.size() - 1);
  return v;
}

template <typename RealType, int Options>
inline double vari(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &mat,
    const double the_mean) {
  double v = 0.0;
  if (mat.rows() * mat.cols() > 2)
    v = ((mat.array() - the_mean).square().sum()) /
        (mat.rows() * mat.cols() - 1);
  return v;
}

template <typename RealType, int Options>
inline double
vari(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return vari(vec, vec.mean());
}

template <typename RealType, int Options>
inline double vari(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic,
                                       Options> &mat) {
  return vari(mat, mat.mean());
}

template <typename RealType, int Options>
inline double
stdv(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
     const double the_mean) {
  return sqrt(vari(vec, the_mean));
}

template <typename RealType, int Options>
inline double stdv(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &mat,
    const double the_mean) {
  return sqrt(vari(mat, the_mean));
}

template <typename RealType, int Options>
inline double
stdv(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return sqrt(vari(vec));
}

template <typename RealType, int Options>
inline double stdv(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic,
                                       Options> &mat) {
  return sqrt(vari(mat));
}

template <typename RealType, int Options>
inline double
MLvari(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
       const double the_mean) {
  double v = 0.0;
  if (!vec.empty())
    v = ((vec.array() - the_mean).squared().sum()) / (vec.size());
  return v;
}

template <typename RealType, int Options>
inline double MLvari(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &mat,
    const double the_mean) {
  double v = 0.0;
  if (mat.rows() * mat.cols() > 1)
    v = ((mat.array() - the_mean).square().sum()) / (mat.rows() * mat.cols());
  return v;
}

template <typename RealType, int Options>
inline double
MLvari(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return MLvari(vec, vec.mean());
}

template <typename RealType, int Options>
inline double MLvari(const Eigen::Matrix<RealType, Eigen::Dynamic,
                                         Eigen::Dynamic, Options> &mat) {
  return MLvari(mat, mat.mean());
}

template <typename RealType, int Options>
inline double
MLstdv(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
       const double the_mean) {
  return sqrt(MLvari(vec, the_mean));
}

template <typename RealType, int Options>
inline double MLstdv(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &mat,
    const double the_mean) {
  return sqrt(MLvari(mat, the_mean));
}

template <typename RealType, int Options>
inline double
MLstdv(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return sqrt(MLvari(vec));
}

template <typename RealType, int Options>
inline double MLstdv(const Eigen::Matrix<RealType, Eigen::Dynamic,
                                         Eigen::Dynamic, Options> &mat) {
  return sqrt(MLvari(mat));
}

template <typename RealType, int Options>
inline double
quantileSorted(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
               const double order) {
  // SDMAssert(0.0 <= order && order <= 1.0);
  int l = vec.size();
  double tmp = (l - 1) * order;
  int lowerIndex = int(floor(tmp));
  int upperIndex = int(ceil(tmp));
  tmp -= lowerIndex;
  return (1.0 - tmp) * vec[lowerIndex] + tmp * vec[upperIndex];
}

template <typename RealType, int Options>
inline double
quantileUnsorted(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
                 const double order) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> vv = vec;
  std::sort(vv.data(), vv.data() + vv.size());
  return quantileSorted(vv, order);
}

template <typename RealType, int Options>
inline double
quantile(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
         const double order) {
  return quantileUnsorted(vec, order);
}

template <typename RealType, int Options>
inline double
quartileRange(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> vv = vec;
  std::sort(vv.data(), vv.data() + vv.size());
  return quantileSorted(vv, 0.75) - quantileSorted(vv, 0.25);
}

template <typename RealType, int Options>
inline double meanAbsoluteDeviation(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return meanAbsoluteDeviation(vec, vec.maen());
}

template <typename RealType, int Options>
inline double meanAbsoluteDeviation(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
    const double the_mean) {
  int l = vec.size();
  double ans = 0.0;
  if (!vec.empty()) {
    for (int i = 0; i < l; ++i)
      ans += fabs(vec[i] - the_mean);
    ans /= static_cast<double>(l);
  }
  return ans;
}

template <typename RealType, int Options>
RealType range(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return static_cast<RealType>(vec.maxCoeff() - vec.minCoeff());
}

template <typename RealType, int Options>
inline double stdvByQuartileRange(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> vv = vec;
  std::sort(vv.data(), vv.data() + vv.size());
  boost::math::normal_distribution<double> n_dstr(0.0, 1.0);
  double x = boost::math::quantile(n_dstr, 0.75);
  return (quantileSorted(vv, 0.75) - quantileSorted(vv, 0.25)) / x;
}

template <typename RealType, int Options>
inline double
lpNorm(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
       const int order) {
  double ans;
  if (order == 1) {
    ans = vec.cwiseAbs().sum();
  } else if (order == 2) {
    ans = vec.norm();
  } else {
    using std::pow;
    ans = pow(vec.cwiseAbs().array().pow(order).sum(), RealType(1) / order);
  }
  return ans;
}

template <typename RealType, int Options>
inline double
moment(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
       const int order) {
  double lp = lpNorm(vec, order);
  return lp / static_cast<double>(vec.size());
}

template <typename RealType, int Options>
inline double
normalizedMoment(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
                 const int order, const double the_mean,
                 const double the_stdv) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> vv =
      ((vec.array() - the_mean) / the_stdv).matrix();
  return moment(vv, order);
}

template <typename RealType, int Options>
inline double
normalizedMoment(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
                 const int order) {
  double m = vec.mean();
  double s = stdv(vec, m);
  return normalizedMoment(vec, order, m, s);
}

template <typename RealType, int Options>
inline double
skew(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return normalizedMoment(vec, 3);
}

template <typename RealType, int Options>
inline double
skew(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
     const double the_mean, const double the_stdv) {
  return normalizedMoment(vec, 3, the_mean, the_stdv);
}

template <typename RealType, int Options>
inline double
kurt(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return normalizedMoment(vec, 4) - 3.0;
}

template <typename RealType, int Options>
inline double
kurt(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
     const double the_mean, const double the_stdv) {
  return normalizedMoment(vec, 4, the_mean, the_stdv) - 3.0;
}

template <typename RealType, int Options>
double median(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  return quantile(vec, 0.5);
}

template <typename RealType, int Options>
double medianAbsoluteDeviation(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
    double the_median) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> mad =
      ((vec.array() - the_median).abs()).matrix();
  return median(mad);
}

template <typename RealType, int Options>
double medianAbsoluteDeviation(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  double the_median = median(vec);
  return medianAbsoluteDeviation(vec, the_median);
}

template <typename RealType, int Options>
void meanAndVari(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
                 double &the_mean, double &the_vari) {
  the_mean = vec.mean();
  the_vari = vari(vec, the_mean);
}

template <typename RealType, int Options>
void meanAndStdv(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec,
                 double &the_mean, double &the_stdv) {
  the_mean = vec.mean();
  the_stdv = stdv(vec, the_mean);
}

template <typename RealType, int Options>
double coefficientOfVariation(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  double m, s;
  meanAndVari(vec, m, s);
  // if (fabs(m) < 1e-12)
  //   SDMError("in coefficientOfVariation(...), the mean is almost 0");
  return s / m;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> covMat(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &mat,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &mean_vec) {
  int n = mat.cols();
  // SDMAssert(2 <= n);
  int p = mat.rows();
  Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> ans(p, p);
  double ans_jk = 0.0;
  for (int j = 0; j < p; ++j) {
    for (int k = j; k < p; ++k) {
      for (int i = 0; i < n; ++i)
        ans_jk += (mat.coeffRef(i, j) - mean_vec[j]) *
                  (mat.coeffRef(i, k) - mean_vec[k]);
      ans.coeffRef(j, k) = ans.coeffRef(k, j) = ans_jk / (n - 1);
    }
  }
  return ans;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options>
correlationMat(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic,
                                   Options> &covMat) {
  int p = covMat.cols();
  // if (p != covMat.rows())
  //   SDMError("covMat must be square matrix");
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> diag(p);
  for (int i = 0; i < p; ++i)
    diag[i] = sqrt(covMat.coeffRef(i, i));
  Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> ans = covMat;
  for (int i = 0; i < p; ++i) {
    for (int j = i + 1; j < p; ++j)
      ans.coeffRef(i, j) = ans.coeffRef(j, i) /= (diag[i] * diag[j]);
    ans.coeffRef(i, i) = 1.0;
  }
  return ans;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> pooledCovMat(
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &
        data1,
    const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> &
        data2,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &meanVec1,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &meanVec2) {
  int n1 = data1.cols();
  int n2 = data2.cols();
  int n = n1 + n2;
  int p = data1.rows();
  // SDMAssert(2 <= n1 && 2 <= n2 && data2.rows() == p);

  Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic, Options> ans(p, p);
  double ans_jk = 0.0;
  for (int j = 0; j < p; ++j) {
    for (int k = j; k < p; ++k) {
      for (int i = 0; i < n1; ++i)
        ans_jk += (data1.coeffRef(i, j) - meanVec1[j]) *
                  (data1.coeffRef(i, k) - meanVec1[k]);
      for (int i = 0; i < n2; ++i)
        ans_jk += (data2.coeffRef(i, j) - meanVec2[j]) *
                  (data2.coeffRef(i, k) - meanVec2[k]);
      ans.coeffRef(j, k) = ans.coeffRef(k, j) = ans_jk / (n - 2);
    }
  }
  return ans;

  // return static_cast<double>(n1 - 1)/(n - 2)*covMat(data1, meanVec1) +
  // static_cast<double>(n2 - 1)/(n - 2)*covMat(data2, meanVec2);
}

template <typename RealType, int Options>
RealType correlationCoefficient(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v1,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v2) {
  int l = v1.size();
  // SDMAssert(v2.size() == l);
  RealType m1 = v1.mean();
  RealType m2 = v2.mean();
  RealType enumerator = 0.0, denominator1 = 0.0, denominator2 = 0.0;
  for (int i = 0; i < l; i++) {
    RealType tmp1 = v1[i] - m1;
    RealType tmp2 = v2[i] - m2;
    enumerator += tmp1 * tmp2;
    denominator1 += tmp1 * tmp1;
    denominator2 += tmp2 * tmp2;
  }
  return enumerator / sqrt(denominator1) / sqrt(denominator2);
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> correlationCoefficientTest(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v1,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v2) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> ans(3);
  RealType r = ans[0] = correlationCoefficient(v1, v2);
  RealType df = static_cast<RealType>(v1.size() - 2.0);
  RealType t = ans[1] = r * sqrt(df) / sqrt(1.0 - r * r);
  boost::math::students_t_distribution<RealType> st_dstr(df);
  ans[2] = 1.0 - boost::math::cdf(st_dstr, t);
  return ans;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> extractEqualIndices(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v,
    const RealType &val) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> ans(v.size());
  int count = 0;
  for (int i = 0; i < v.size(); ++i)
    if (val == v[i])
      ans[count++] = i;
  ans.conservativeResize(count);
  return ans;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> doubleMeanRank(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &rank) {
  int l = v.size();
  // SDMAssert(l == rank.size());
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> dRank(l);
  for (int i = 0; i < l; ++i) {
    Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> tmpVec1 =
        extractEqualIndices(v, v[i]);
    int len = tmpVec1.size();
    if (len > 1) {
      Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> tmpVec2(len);
      for (int j = 0; j < len; ++j)
        tmpVec2[j] = rank[tmpVec1[j]];

      dRank[i] = tmpVec2.mean();
    } else {
      dRank[i] = rank[i];
    }
  }
  return dRank;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> argsortDecOrder(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {

  using pair_sort = std::pair<int, RealType>;
  std::vector<pair_sort> sort_vec;
  for (int i = 0; i < vec.size(); ++i)
    sort_vec.push_back(i, vec[i]);
  std::sort(sort_vec.begin(), sort_vec.end(), lessPair);
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> ans(vec.size());
  for (int i = 0; i < vec.size(); ++i)
    ans[i] = (sort_vec[i])->first;
  return ans;
}

template <typename RealType, int Options>
Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options>
rankDecOrder(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &vec) {
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> idx =
      argsortDecOrder(vec);
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> ans(vec.size());
  for (int i = 0; i < vec.size(); ++i)
    ans[idx[i]] = i;
  return ans;
}

template <typename RealType, int Options>
double spearmanRankCorrelation(
    Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v1,
    Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v2) {
  int l = v1.size();
  // SDMAssert(v2.size() == l);
  // Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> rank1 =
  // rankDecOrder(v1);
  // Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> rank2 =
  // rankDecOrder(v2);
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> dRank1 =
      doubleMeanRank(v1, rankDecOrder(v1));
  Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> dRank2 =
      doubleMeanRank(v2, rankDecOrder(v2));
  double sum = 0.0;
  for (int i = 0; i < l; ++i) {
    double tmp_i = (dRank1[i] - dRank2[i]);
    sum += tmp_i * tmp_i;
  }
  return 1.0 - 6.0 * sum / (double)(l * (l * l - 1));
}

template <typename RealType, int Options>
double mixQuantile(const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &v,
                   const double order) {
  // SDMAssert(0.0 <= order && order <= 1.0);
  int numKernel = v.size() / 3;
  // SDMAssert(numKernel > 3);
  double ans = 0.0;
  boost::math::normal_distribution<double> n_dstr(0.0, 1.0);
  for (int i = 0; i < numKernel; ++i)
    ans +=
        (boost::math::quantile(n_dstr, order) * v[i * 3 + 1] + v[i * 3 + 2]) *
        v[i * 3];

  return ans;
}

template <typename RealType, int Options>
double quantileFromNormalMixture(
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &coefficient,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &mu,
    const Eigen::Matrix<RealType, Eigen::Dynamic, 1, Options> &sigma,
    const double order) {
  int numComponent = coefficient.size();
  // SDMAssert(mu.size() == numComponent && sigma.size() == numComponent);
  // SDMAssert(0.0 <= order && order <= 1.0);
  double ans = 0.0;
  boost::math::normal_distribution<double> n_dstr(0.0, 1.0);
  for (int k = 0; k < numComponent; ++k)
    ans += coefficient[k] *
           (boost::math::quantile(n_dstr, order) * sigma[k] + mu[k]);
  return ans;
}

} // namespace sdm

#endif


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

#include <boost/range/adaptor/reversed.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(175,
                     "Fractions involving the number of different ways a number can be expressed as a sum of powers of 2") {
    // Define f(0)=1 and f(n) to be the number of ways to write n as a sum of powers of 2 where no power occurs more
    // than twice.
    //
    // For example, f(10)=5 since there are five different ways to express 10:
    // 10 = 8+2 = 8+1+1 = 4+4+2 = 4+2+2+1+1 = 4+4+1+1
    //
    // It can be shown that for every fraction p/q (p>0, q>0) there exists at least one integer n such that
    // f(n)/f(n-1)=p/q.
    //
    // For instance, the smallest n for which f(n)/f(n-1)=13/17 is 241.
    // The binary expansion of 241 is 11110001.
    // Reading this binary number from the most significant bit to the least significant bit there are 4 one's, 3 zeroes
    // and 1 one. We shall call the string 4,3,1 the Shortened Binary Expansion of 241.
    //
    // Find the Shortened Binary Expansion of the smallest n for which
    // f(n)/f(n-1)=123456789/987654321.
    //
    // Give your answer as comma separated integers, without any whitespaces.
    nombre p = 123456789;
    nombre q = 987654321;

    vecteur resultat;

    while (q != 0) {
        if (p > q) {
            resultat.push_back(p / q - (p % q == 0 ? 1 : 0));
            p = p % q == 0 ? q : p % q;
        } else {
            resultat.push_back(q / p);
            q %= p;
        }
    }

    std::ostringstream oss;
    bool premier = true;
    for (auto &i : boost::adaptors::reverse(resultat)) {
        if (premier)
            premier = false;
        else
            oss << ",";

        oss << i;
    }

    return oss.str();
}


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example power-iter.cpp
*
*   This tutorial demonstrates the calculation of the eigenvalue with largest modulus using the power iteration method.
*
*   We start with including the necessary headers:
**/

// Sparse matrices in uBLAS are *very* slow if debug mode is enabled. Disable it:
#ifndef NDEBUG
  #define BOOST_UBLAS_NDEBUG
#endif

// Include necessary system headers
#include <iostream>
#include <fstream>
#include <limits>
#include <string>

#define VIENNACL_WITH_UBLAS

// Include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/power_iter.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include <iostream>

// Boost includes:
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

/**
*  To run the power iteration, we set up a sparse matrix using Boost.uBLAS, transfer it over to a ViennaCL matrix, and then run the algorithm.
**/
int main()
{
  // This example relies on double precision to be available and will provide only poor results with single precision
  typedef double     ScalarType;

  /**
  * Create the sparse uBLAS matrix and read the matrix from the matrix-market file:
  **/
  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_A;

  if (!viennacl::io::read_matrix_market_file(ublas_A, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  /**
  *  Transfer the data from the uBLAS matrix over to the ViennaCL sparse matrix:
  **/
  viennacl::compressed_matrix<ScalarType>  vcl_A(ublas_A.size1(), ublas_A.size2());
  viennacl::copy(ublas_A, vcl_A);

  /**
  *  Run the power iteration up until the largest eigenvalue changes by less than the specified tolerance.
  *  Print the results of running with uBLAS as well as ViennaCL and exit.
  **/
  viennacl::linalg::power_iter_tag ptag(1e-6);

  std::cout << "Starting computation of eigenvalue with largest modulus (might take about a minute)..." << std::endl;
  std::cout << "Result of power iteration with ublas matrix (single-threaded): " << viennacl::linalg::eig(ublas_A, ptag) << std::endl;
  std::cout << "Result of power iteration with ViennaCL (OpenCL accelerated): " << viennacl::linalg::eig(vcl_A, ptag) << std::endl;

  /**
   *  You can also obtain the associated *approximated* eigenvector by passing it as a third argument to eig()
   *  Tighten the tolerance passed to ptag above in order to obtain more accurate results.
   **/
  viennacl::vector<ScalarType> eigenvector(vcl_A.size1());
  viennacl::linalg::eig(vcl_A, ptag, eigenvector);
  std::cout << "First three entries in eigenvector: " << eigenvector[0] << " " << eigenvector[1] << " " << eigenvector[2] << std::endl;
  viennacl::vector<ScalarType> Ax = viennacl::linalg::prod(vcl_A, eigenvector);
  std::cout << "First three entries in A*eigenvector: " << Ax[0] << " " << Ax[1] << " " << Ax[2] << std::endl;

  return EXIT_SUCCESS;
}


// Copyright 2020, Madhur Chauhan

// 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_SPECIAL_FIBO_HPP
#define BOOST_MATH_SPECIAL_FIBO_HPP

#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <cmath>
#include <limits>

#ifdef _MSC_VER
#pragma once
#endif

namespace boost {
namespace math {

namespace detail {
   constexpr double fib_bits_phi = 0.69424191363061730173879026;
   constexpr double fib_bits_deno = 1.1609640474436811739351597;
} // namespace detail

template <typename T>
inline BOOST_CXX14_CONSTEXPR T unchecked_fibonacci(unsigned long long n) noexcept(std::is_fundamental<T>::value) {
    // This function is called by the rest and computes the actual nth fibonacci number
    // First few fibonacci numbers: 0 (0th), 1 (1st), 1 (2nd), 2 (3rd), ...
    if (n <= 2) return n == 0 ? 0 : 1;
    /*
     * This is based on the following identities by Dijkstra:
     *   F(2*n)   = F(n)^2 + F(n+1)^2
     *   F(2*n+1) = (2 * F(n) + F(n+1)) * F(n+1)
     * The implementation is iterative and is unrolled version of trivial recursive implementation.
     */
    unsigned long long mask = 1;
    for (int ct = 1; ct != std::numeric_limits<unsigned long long>::digits && (mask << 1) <= n; ++ct, mask <<= 1)
        ;
    T a{1}, b{1};
    for (mask >>= 1; mask; mask >>= 1) {
        T t1 = a * a;
        a = 2 * a * b - t1, b = b * b + t1;
        if (mask & n)
            t1 = b, b = b + a, a = t1; // equivalent to: swap(a,b), b += a;
    }
    return a;
}

template <typename T, class Policy>
T inline BOOST_CXX14_CONSTEXPR fibonacci(unsigned long long n, const Policy &pol) {
    // check for overflow using approximation to binet's formula: F_n ~ phi^n / sqrt(5)
    if (n > 20 && n * detail::fib_bits_phi - detail::fib_bits_deno > std::numeric_limits<T>::digits)
        return policies::raise_overflow_error<T>("boost::math::fibonacci<%1%>(unsigned long long)", "Possible overflow detected.", pol);
    return unchecked_fibonacci<T>(n);
}

template <typename T>
T inline BOOST_CXX14_CONSTEXPR fibonacci(unsigned long long n) {
    return fibonacci<T>(n, policies::policy<>());
}

// generator for next fibonacci number (see examples/reciprocal_fibonacci_constant.hpp)
template <typename T>
class fibonacci_generator {
  public:
    // return next fibonacci number
    T operator()() noexcept(std::is_fundamental<T>::value) {
        T ret = a;
        a = b, b = b + ret; // could've simply: swap(a, b), b += a;
        return ret;
    }

    // after set(nth), subsequent calls to the generator returns consecutive
    // fibonacci numbers starting with the nth fibonacci number
    void set(unsigned long long nth) noexcept(std::is_fundamental<T>::value) {
        n = nth;
        a = unchecked_fibonacci<T>(n);
        b = unchecked_fibonacci<T>(n + 1);
    }

  private:
    unsigned long long n = 0;
    T a = 0, b = 1;
};

} // namespace math
} // namespace boost

#endif