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

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

#define NDIM 4

int main() {
	mf_size_type n = 4;

	// coefficient matrix
	DenseMatrixCM a(n, n), aLu(n,n), aQr(n,n), aS(n,n);
	a(0,0) = 1.0;
	a(0,1) = -1.0;
	a(0,2) = 2.0;
	a(0,3) = -1.0;
	a(1,0) = 2.0;
	a(1,1) = -2.0;
	a(1,2) = 3.0;
	a(1,3) = -3.0;
	a(2,0) = 1.0;
	a(2,1) = 1.0;
	a(2,2) = 1.0;
	a(2,3) = 0.0;
	a(3,0) = 1.0;
	a(3,1) = -1.0;
	a(3,2) = 4.0;
	a(3,3) = 3.0;

	// right hand side
	boost::numeric::ublas::vector<double> b(n), x(n), tau(n);
	b[0] = -8.0;
	b[1] = -20.0;
	b[2] = -2.0;
	b[3] = 4.0;

	// print
	cout << "A = " << a << endl;
	cout << "b = " << b << endl;

	// compute LU factorization
	boost::numeric::ublas::vector<clapack::integer> ipiv(n);
	aLu = a;
	cout << lu(aLu, ipiv) << endl;
	cout << "A(LU) = " << aLu << endl;
	cout << "Row permutations = " << ipiv << endl;

	// solve LP
	x = b;
	cout << lpLu(aLu, ipiv, &x.data()[0]) << endl;

	// print
	cout << "x = " << x << endl;
	cout << "Ax = " << prod(a, x) << endl;

	// qr decomposition
	aQr = a;
	cout << qr(aQr, tau) << endl;
	cout << "A(QR) = " << aQr << endl;
	cout << "tau = " << tau << endl;

	// solve least squares problem
	std::pair<clapack::integer, clapack::integer> swork = llsWork(n,n);
	boost::numeric::ublas::vector<double> s(n); // singular values
	boost::numeric::ublas::vector<double> work(swork.first);
	boost::numeric::ublas::vector<clapack::integer> iwork(swork.second);
	aS = a;
	x = b;
	cout << lls(aS, x.data().begin(), s, work, iwork) << endl;
	cout << "A(S) = " << aS << endl;
	cout << "s = " << s << endl;
	cout << "x = " << x << endl;
	cout << "Ax = " << prod(a, x) << endl;

	return 0;
}


// Unit test for the computeTimeStep function in fvm_1d_functions.cpp
// This function calculates the time step based on the fastest speed in the system, delta x, and the CFL number
// COMPLETE. This function works as intended. 2021/11/22

#include <iostream>
#include <Eigen/Dense>
#include "../main/fvm_1D_functions.h"
#include "../main/testing_functions.h"

using namespace std;
using namespace Eigen;

int main() {

    // Print what we are doing
    cout << "------------------------------------- " << \
    "This is a test of the computeTimeStep function." << \
    " -------------------------------------" << endl;

    // Set the input parameters to the cons2prim function
    double gasGamma = 5./3.;   // Use standard gasGamma of 5/3
    double CFL = 0.9;          // Set CFL number
    double dx = 0.5;           // Set delta x

    // Make an array of primitive variables such that there are two elements in each 1D array
    Array<ArrayXd, 3, 1> V;
    V(0) = ArrayXd::Zero(2);
    V(0)(0) = 5.;
    V(0)(1) = 1.;
    V(1) = ArrayXd::Zero(2);
    V(1)(0) = 1.0;
    V(1)(1) = -0.2;
    V(2) = ArrayXd::Zero(2);
    V(2)(0) = 12.;
    V(2)(1) = 27./5.;

    // Set the exact value of the time step. Calculated by hand based on input values chosen
    double dt_exact = 0.140625;

    // Calculate the time step
    double dt = computeTimeStep(CFL, dx, gasGamma, V);

    // Set the acceptable tolerance. (Somewhat arbitrarily chosen)
    double TOL = 1e-14;

    // Determine if the two values are equal
    bool boolVal = isEqualDouble(dt, dt_exact, TOL);

    // Get the minimum value of the boolean array.
    // If it is 0, then at least one value in this array is incorrect
    if (boolVal == 1) {
        cout << "Test of computeTimeStep is a success." << endl;
    }
    else  {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
            << "Test of computeTimeStep is a failure." << endl \
            << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }
    cout << endl << endl;
}


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch3/useEigen/eigenMatrix.cpp 学习记录
								Eigen矩阵库
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.03.26, William Yu

=================================================================================*/
/// Include Files
#include<iostream>
#include<ctime>
#include <Eigen/Core>
//稠密矩阵的代数运算（求逆，特征值等）
#include <Eigen/Dense>

using namespace std;

/// Global Variables
#define MATRIX_SIZE 50 //矩阵大小

/// Function Definitions

/**
 * @function main
 * @author William Yu
 * @brief Eigen基本类型使用示例
 * @param  None
 * @retval None
 */
int main(int argc, char** argv )
{
    //---------------------------------------------------------------------------
    //  创建矩阵&初始化操作
    //----------------------------------------------------------------
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float,2,3> matrix_23;

    // 声明一个3*1的矩阵
    Eigen::Matrix<float,3,1>vd_3d;
    // 或者下列方式也可以
    Eigen::Vector3d v_3d;

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零

    // 建立动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的写法
    Eigen::MatrixXd matrix_x;

    // 随机数矩阵Random填充
    matrix_33 = Eigen::Matrix3d::Random();
    cout << "创建随机矩阵" << endl;
    cout << matrix_33 << endl << endl;

    //---------------------------------------------------------------------------
    //  矩阵元素操作
    //----------------------------------------------------------------
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 直接整体输出
    cout << "matrix_23:" << endl;
    cout << matrix_23 << endl;
    // 用for()访问矩阵中的元素
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    //---------------------------------------------------------------------------
    //  矩阵运算
    //----------------------------------------------------------------
    // 矩阵和向量相乘（实际上仍是矩阵和矩阵相乘）
    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;
    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    //-- 矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    cout << matrix_33.transpose() << endl;      // 转置
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 迹
    cout << 10*matrix_33 << endl;               // 数乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.determinant() << endl;    // 行列式

    //---------------------------------------------------------------------------
    //  特征值求解
    //----------------------------------------------------------------
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    //---------------------------------------------------------------------------
    //  矩阵方程
    //----------------------------------------------------------------
    // 求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大
    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock(); // 计时函数
    //[1]-- 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;

    //[2]-- 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include "class.hpp"
#include "coefficient_computations.hpp"
#include <unistd.h>
#include <limits.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
#include "nanoflann.hpp"
using namespace std;

SOLIDIFICATION::SOLIDIFICATION(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, int temporal_order1)
{
    temporal_order = temporal_order1;
    if (temporal_order != 1 && temporal_order != 2)
    {
        printf("\n\nERROR from SOLIDIFICATION::SOLIDIFICATION temporal_order should be either '1' or '2'; current value: %i\n\n", temporal_order);
        throw bad_exception();
    }
    if (temporal_order == 2)
        T_old_old = Eigen::VectorXd::Zero(points.nv), fs_old_old = Eigen::VectorXd::Zero(points.nv);
    dfs_dT = Eigen::VectorXd::Zero(points.nv);
    T_source = Eigen::VectorXd::Zero(points.nv);
}

void SOLIDIFICATION::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &T_new, Eigen::VectorXd &T_old, Eigen::VectorXd &fs_new, Eigen::VectorXd &fs_old, int it)
{
    if (temporal_order == 1 || it == 0)
    {
        T_source = T_old + (parameters.dt * alpha * (points.laplacian_matrix_EIGEN * T_old));
        T_source = T_source - ((Lf / Cp) * dfs_dT.cwiseProduct(T_old));
        T_source = T_source.cwiseQuotient(Eigen::VectorXd::Ones(points.nv) - ((Lf / Cp) * dfs_dT));
    }
    else
    {
        T_source = T_old + (parameters.dt * alpha * (points.laplacian_matrix_EIGEN * (1.5 * T_old - 0.5 * T_old_old)));
        T_source = T_source - ((1.5 * Lf / Cp) * dfs_dT.cwiseProduct(T_old));
        T_source = T_source + ((0.5 * Lf / Cp) * (3 * fs_old - 4 * fs_old + fs_old_old));
        T_source = T_source.cwiseQuotient(Eigen::VectorXd::Ones(points.nv) - ((1.5 * Lf / Cp) * dfs_dT));
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv]) //boundary points should have dirichlet
            T_new[iv] = T_source[iv];  //non-boundary points only

    double fs_hat = 1.0 - pow((Tsol + Teps - Tf) / (Tliq - Tf), 1.0 / (k_partition - 1.0));
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (T_new[iv] <= Tsol)
            fs_new[iv] = 1.0;
        else if (T_new[iv] >= Tliq)
            fs_new[iv] = 0.0;
        else
            fs_new[iv] = 1.0 - pow((T_new[iv] - Tf) / (Tliq - Tf), 1.0 / (k_partition - 1.0));
        if ((T_new[iv] <= Tsol - Teps) || (T_new[iv] >= Tliq))
            dfs_dT[iv] = 0.0;
        else if ((T_new[iv] >= Tsol - Teps) && (T_new[iv] <= Tsol + Teps))
            dfs_dT[iv] = -(1.0 - fs_hat) / (2.0 * Teps);
        else
        {
            dfs_dT[iv] = pow((T_new[iv] - Tf) / (Tliq - Tf), (2.0 - k_partition) / (k_partition - 1.0));
            dfs_dT[iv] = -dfs_dT[iv] / ((k_partition - 1.0) * (Tliq - Tf));
        }
    }
    if (temporal_order == 2)
        T_old_old = T_old, fs_old_old = fs_old;
}


#include "average_case_relative_error.hpp"
#include "average_case_error.hpp"
#include "cudd_helpers.hpp"
#include <boost/multiprecision/cpp_int.hpp>

using abo::util::NumberRepresentation;
using boost::multiprecision::cpp_dec_float_100;
using boost::multiprecision::uint256_t;

namespace abo::error_metrics {

std::pair<cpp_dec_float_100, cpp_dec_float_100>
    average_relative_value(const Cudd& mgr,
                           const std::vector<BDD>& f,
                           const std::vector<BDD>& g)
{

    BDD zero_so_far = mgr.bddOne();

    cpp_dec_float_100 min_average_result = 0;
    cpp_dec_float_100 max_average_result = 0;
    std::vector<BDD> max_one = abo::util::bdd_max_one(mgr, g);
    for (int i = int(g.size()) - 1; i >= 0; i--)
    {
        std::vector<BDD> partial_result;
        partial_result.reserve(max_one.size());
        BDD modifier = zero_so_far & max_one[i];
        for (const BDD& b : f)
        {
            partial_result.push_back(b & modifier);
        }
        auto average = average_value(partial_result);
        min_average_result += average / (std::pow(2.0, i + 1) - 1);
        max_average_result += average / std::pow(2.0, i);

        zero_so_far &= !max_one[i];
    }

    return {min_average_result, max_average_result};
}

std::pair<cpp_dec_float_100, cpp_dec_float_100>
    acre_bounds(const Cudd& mgr, const std::vector<BDD>& f,
              const std::vector<BDD>& f_hat,
              const util::NumberRepresentation num_rep)
{

    std::vector<BDD> absolute_difference =
        abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
    std::vector<BDD> f_absolute = abo::util::bdd_abs(mgr, f, num_rep);
    return average_relative_value(mgr, absolute_difference, f_absolute);
}

cpp_dec_float_100
    acre_add(const Cudd& mgr,
            const std::vector<BDD>& f,
            const std::vector<BDD>& f_hat,
            const NumberRepresentation num_rep)
{

    ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
    std::vector<BDD> f_absolute = abo::util::bdd_abs(mgr, f, num_rep);
    ADD respective_diff = diff.Divide(
        abo::util::bdd_forest_to_add(mgr, f_absolute).Maximum(mgr.addOne()));
    std::vector<std::pair<double, unsigned long>> terminal_values =
        abo::util::add_terminal_values(respective_diff);

    cpp_dec_float_100 sum = 0;
    uint256_t path_sum = 0;
    for (auto p : terminal_values)
    {
        cpp_dec_float_100 value(p.first);
        sum += value * p.second;
        path_sum += p.second;
    }
    return cpp_dec_float_100(sum) /
           cpp_dec_float_100(path_sum);
}

cpp_dec_float_100
    acre_symbolic_division(const Cudd& mgr,
                            const std::vector<BDD>& f,
                            const std::vector<BDD>& f_hat,
                            unsigned int num_extra_bits,
                            const NumberRepresentation num_rep)
{

    std::vector<BDD> absolute_difference =
        abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);

    std::vector<BDD> f_absolute = abo::util::bdd_abs(mgr, f, num_rep);
    std::vector<BDD> no_zero = abo::util::bdd_max_one(mgr, f_absolute);
    std::vector<BDD> divided =
        abo::util::bdd_divide(mgr, absolute_difference,
                              no_zero, num_extra_bits);

    return average_value(divided) / std::pow(2.0, num_extra_bits);
}

} // namespace abo::error_metrics


#include "pch.h"
#include "ssdsMath.h"
#include "ssdsTypes.h"
#include <Eigen/Eigenvalues>

namespace ssds {

static float4x4 calcRegistrationT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    double3 cs{}, cd{}; // double for precision
    for (size_t i = 0; i < num_points; ++i) {
        cs += s[i];
        cd += d[i];
    }
    return translate(float3((cd - cs) / double(num_points)));
}

static float4x4 calcRegistrationRT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    double3 cs{}, cd{}; // double for precision
    for (size_t i = 0; i < num_points; ++i) {
        cs += s[i];
        cd += d[i];
    }
    cs /= double(num_points);
    cd /= double(num_points);
    if (num_points < 3)
        return translate(cd - cs);

    Eigen::Matrix<double, 4, 4> moment;
    {
        auto sit = s.data();
        auto dit = d.data();
        double sxx = 0, sxy = 0, sxz = 0, syx = 0, syy = 0, syz = 0, szx = 0, szy = 0, szz = 0;
        for (int i = 0; i < num_points; ++i, ++sit, ++dit) {
            sxx += (sit->x - cs.x) * (dit->x - cd.x);
            sxy += (sit->x - cs.x) * (dit->y - cd.y);
            sxz += (sit->x - cs.x) * (dit->z - cd.z);
            syx += (sit->y - cs.y) * (dit->x - cd.x);
            syy += (sit->y - cs.y) * (dit->y - cd.y);
            syz += (sit->y - cs.y) * (dit->z - cd.z);
            szx += (sit->z - cs.z) * (dit->x - cd.x);
            szy += (sit->z - cs.z) * (dit->y - cd.y);
            szz += (sit->z - cs.z) * (dit->z - cd.z);
        }
        moment(0, 0) = sxx + syy + szz;
        moment(0, 1) = syz - szy;        moment(1, 0) = moment(0, 1);
        moment(0, 2) = szx - sxz;        moment(2, 0) = moment(0, 2);
        moment(0, 3) = sxy - syx;        moment(3, 0) = moment(0, 3);
        moment(1, 1) = sxx - syy - szz;
        moment(1, 2) = sxy + syx;        moment(2, 1) = moment(1, 2);
        moment(1, 3) = szx + sxz;        moment(3, 1) = moment(1, 3);
        moment(2, 2) = -sxx + syy - szz;
        moment(2, 3) = syz + szy;        moment(3, 2) = moment(2, 3);
        moment(3, 3) = -sxx - syy + szz;
    }

    float4x4 transform;
    if (moment.norm() > 0) {
        Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> es(moment);
        int maxi = 0;
        for (int i = 1; i < 4; ++i) {
            if (es.eigenvalues()(maxi).real() < es.eigenvalues()(i).real()) {
                maxi = i;
            }
        }

        quatf rot = {
            (float)es.eigenvectors()(0, maxi).real(),
            (float)es.eigenvectors()(1, maxi).real(),
            (float)es.eigenvectors()(2, maxi).real(),
            (float)es.eigenvectors()(3, maxi).real(),
        };
        transform = to_mat4x4(rot);
    }
    float3 cs0 = mul_p(transform, float3(cs));
    (float3&)transform[3] = cd - cs0;
    return transform;
}

#if 0
float4x4 calcRegistrationSRT(span<float3> s, span<float3> d)
{
    size_t num_points = s.size();
    const int num_iterations = 50;
    const double threshold = 1.0e-8;
    const double scaleLowerBound = 1.0e-3;
    float4x4 transform = calcRegistrationRT(s, d);

    float3 scale = float3::one();
    float3 denom = float3::zero();
    {
        auto sit = s.data();
        for (int i = 0; i < num_points; ++i, ++sit) {
            denom[0] += (*sit)[0] * (*sit)[0];
            denom[1] += (*sit)[1] * (*sit)[1];
            denom[2] += (*sit)[2] * (*sit)[2];
        }
    }

    std::vector<float3> sstm(num_points);
    {
        auto sit = s.data();
        for (int i = 0; i < num_points; ++i, ++sit) {
            sstm[i][0] = (*sit)[0];
            sstm[i][1] = (*sit)[1];
            sstm[i][2] = (*sit)[2];
        }
    }

    std::vector<float3> dstm(num_points);
    for (size_t l = 0; l < num_iterations; ++l) {
        float4x4 im = invert(transform);
        auto dit = d.data();
        for (size_t i = 0; i < num_points; ++i, ++dit)
            dstm[i] = mul_p(im, *dit);

        for (size_t d = 0; d < 3; ++d) {
            double ds = 0.0;
            auto sit = s.data();
            for (size_t i = 0; i < num_points; ++i, ++sit) {
                ds += (*sit)[d] * dstm[i][d];
            }
            scale[d] = denom[d] < threshold ? 1.0 : ds / denom[d];
            sit = s.data();
            for (size_t i = 0; i < num_points; ++i, ++sit) {
                sstm[i][d] = scale[d] * (*sit)[d];
            }
        }
        transform = calcRegistrationRT(num_points, sstm.begin(), pd);
    }
    double sv[3] = { scale[0], scale[1], scale[2] };
    transform.setScale(sv, MSpace::kTransform);
    return transform;
}

using RegistrationFunc = float4x4(*)(span<float3> ps, span<float3> pd);

static RegistrationFunc registrationFuncs[3] = {
    calcRegistrationT,
    calcRegistrationRT,
    calcRegistrationSRT
};

void computeSamplePoints(std::vector<float3>& sample, int sid, int joint, const Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int& num_indices = output.num_indices;

    for (int v = 0; v < num_vertices; ++v) {
        sample[v] = input.sample[sid * num_vertices + v];
        const float3& s = input.rest_shape[v];
        for (int i = 0; i < num_indices; ++i) {
            const int jnt = output.skin_indices[v * num_indices + i];
            if (jnt >= 0 && jnt != joint) {
                const double w = output.skin_weights[v * num_indices + i];
                const float4x4& at = output.skinMatrix[sid * input.num_joints + jnt];
                sample[v] -= w * (s * at.asMatrix());
            }
        }
    }
}

void subtractCentroid(std::vector<float3>& model, std::vector<float3>& sample, float3& corModel, float3& corSample, const Eigen::VectorXd& weight, const Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;

    double wsqsum = 0;
    corModel = {};
    corSample = {};
    for (int v = 0; v < num_vertices; ++v) {
        const double w = weight[v];
        corModel += w * w * input.restShape[v];
        corSample += w * sample[v];
        wsqsum += w * w;
    }
    corModel = corModel / wsqsum;
    corSample = corSample / wsqsum;
    for (int v = 0; v < num_vertices; ++v) {
        model[v] = weight[v] * (input.restShape[v] - corModel);
        sample[v] -= weight[v] * corSample;
    }
}

class JointTransformUpdator
{
private:
    Output* output;
    const Input* input;
    const Eigen::VectorXd* weight;
    int transformType;
    int joint;

public:
    JointTransformUpdator(Output* output_, const Input* input_,
        const Eigen::VectorXd* weight_, int joint_, int transformType_)
        : output(output_), input(input_),
        weight(weight_), joint(joint_), transformType(transformType_)
    {
    }

    void operator ()(const tbb::blocked_range<int>& range) const
    {
        std::vector<float3> model(input->num_vertices);
        std::vector<float3> sample(input->num_vertices);
        for (int s = range.begin(); s != range.end(); ++s) {
            if (s == 0) {
                output->skinMatrix[joint] = float4x4::identity;
                continue;
            }
            computeSamplePoints(sample, s, joint, *output, *input);
            float3 corModel(0, 0, 0), corSample(0, 0, 0);
            subtractCentroid(model, sample, corModel, corSample, *weight, *output, *input);
            float4x4 transform = registrationFuncs[transformType](model.size(), model.begin(), sample.begin());
            float3 d = corSample - corModel * transform.asMatrix();
            transform.setTranslation(d + transform.getTranslation(MSpace::kTransform), MSpace::kTransform);
            output->skinMatrix[s * input->num_joints + joint] = transform;
        }
    }
};

void updateJointTransformProc(Output& output, int transformType, const Input& input, int selection = -1)
{
    const int num_vertices = input.sample.front().points.size();
    const int num_samples  = input.sample.size();
    const int num_joints    = output.num_joints;
    const int num_indices  = output.num_indices;

    Eigen::VectorXd weight = Eigen::VectorXd::Zero(num_vertices);
    int begin = selection < 0 ? 0 : selection;
    int end = selection < 0 ? num_joints : selection + 1;
    for (int joint = begin; joint < end; ++joint) {
        for (int v = 0; v < num_vertices; ++v) {
            weight[v] = 0.0;
            for (int i = 0; i < num_indices; ++i) {
                int jnt = output.skin_indices[v * num_indices + i];
                if (jnt == joint) {
                    weight[v] = output.skin_weights[v * num_indices + i];
                    break;
                }
            }
        }
        double wsqsum = weight.dot(weight);
        if (wsqsum > 1.0e-8) {
            tbb::blocked_range<int> blockedRange(0, num_samples);
            JointTransformUpdator transformUpdator(&output, &input, &weight, joint, transformType);
            tbb::parallel_for(blockedRange, transformUpdator);
        }
        else {
            for (int s = 0; s < num_samples; ++s) {
                output.skinMatrix[s * input.num_joints + joint] = float4x4::identity;
            }
            char buf[256];
            printf(buf, "Fixed joint #%d", joint);
        }
    }
}

static PyObject* updateJointTransform(PyObject *self, PyObject *args)
{
    PyObject* pin            = PyTuple_GET_ITEM(args, 0);
    PyObject* pout           = PyTuple_GET_ITEM(args, 1);
    const int transformType = PyInt_AsLong(PyTuple_GET_ITEM(args, 2));
    Input* const input   = reinterpret_cast<Input*>(PyCapsule_GetPointer(pin, "SSDSInput"));
    Output* const output = reinterpret_cast<Output*>(PyCapsule_GetPointer(pout, "SSDSOutput"));
    updateJointTransformProc(*output, transformType, *input);
    return Py_None;
}

// see https://sites.google.com/view/fumiyanarita/project/la_ssdr_mdmc
static void detectNeighborClusters(const Input& input, Output& output)
{
    MGlobal::displayInfo("Detecting neighbor clusters");
    const int& num_indices = output.num_indices;

    std::vector<std::set<int>> clusters(output.num_joints);
    for (int v = 0; v < input.num_vertices; ++v) {
        const int primjid = output.skinIndex[v * num_indices];
        if (input.numRings == 0) {
            for (int j = 0; j < output.num_joints; ++j) {
                for (int v = 0; v < output.num_joints; ++v) {
                    clusters[j].insert(v);
                }
            }
        }
        else {
            clusters[primjid].insert(0);
            clusters[primjid].insert(primjid);
            // one-ring neighbors
            for (auto nvit = input.neighbor[v].begin(); nvit != input.neighbor[v].end(); ++nvit) {
                const int nvid = nvit->first;
                clusters[primjid].insert(output.skinIndex[nvid * num_indices]);
                if (input.numRings == 1) {
                    continue;
                }
                // two-ring neighbors
                for (auto nnvit = input.neighbor[nvid].begin(); nnvit != input.neighbor[nvid].end(); ++nnvit) {
                    const int nnvid = nnvit->first;
                    clusters[primjid].insert(output.skinIndex[nnvid * num_indices]);
                }
            }
        }
    }
    for (int v = 0; v < input.num_vertices; ++v) {
        const int primjid = output.skinIndex[v * num_indices];
        output.vertCluster[v] = std::vector<int>(clusters[primjid].begin(), clusters[primjid].end());
    }
}

int bindVertexToJoint(Output& output, const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int num_samples  = input.num_samples;
    const int num_indices  = output.num_indices;

    std::vector<int> numJointVertices(output.num_joints, 0);
    for (int v = 0; v < num_vertices; ++v) {
        int bestJoint = 0;
        double minErr = std::numeric_limits<double>::max();
        const float3& restShapePos = input.restShape[v];
        for (int j = 0; j < output.num_joints; ++j) {
            double errsq = 0;
            for (int s = 0; s < num_samples; ++s) {
                float4x4 am = output.skinMatrix[s * input.num_joints + j].asMatrix();
                float3 diff = input.sample[s * num_vertices + v] - restShapePos * am;
                errsq += diff * diff;
            }
            errsq *= (input.restShape[v] - output.restJointPos[j]).length();
            if (errsq < minErr) {
                bestJoint = j;
                minErr = errsq;
            }
        }
        ++numJointVertices[bestJoint];
        output.skinIndex[v * num_indices + 0] = bestJoint;
    }

    std::vector<int>::iterator smallestBoneSize = std::min_element(numJointVertices.begin(), numJointVertices.end());
    while (*smallestBoneSize <= 0) {
        const int smallestBone = static_cast<int>(smallestBoneSize - numJointVertices.begin());
        numJointVertices.erase(numJointVertices.begin() + smallestBone);
        output.restJointPos.erase(output.restJointPos.begin() + smallestBone);
        for (int s = 0; s < input.num_samples; ++s) {
            for (int j = output.num_joints - 2; j >= smallestBone; --j) {
                output.skinMatrix[s * input.num_joints + j] = output.skinMatrix[s * input.num_joints + j + 1];
            }
        }
        for (int v = 0; v < num_vertices; ++v) {
            if (output.skinIndex[v * num_indices + 0] >= smallestBone) {
                --output.skinIndex[v * num_indices + 0];
            }
        }
        --output.num_joints;
        smallestBoneSize = std::min_element(numJointVertices.begin(), numJointVertices.end());
    }
    return static_cast<int>(numJointVertices.size());
}

int findMostStableVertex(const Input& input)
{
    const int num_vertices = input.num_vertices;
    const int num_samples = input.num_samples;
    double minErrorSq = std::numeric_limits<double>::max();
    int mostStableVertex = -1;
    for (int v = 0; v < num_vertices; ++v) {
        double errSq = 0;
        for (int s = 0; s < num_samples; ++s) {
            float3 diff = input.sample[s * num_vertices + v] - input.restShape[v];
            errSq += diff * diff;
        }
        if (errSq < minErrorSq) {
            minErrorSq = errSq;
            mostStableVertex = v;
        }
    }
    return mostStableVertex;
}

int findDistantVertex(const Input& input, const Output& output, const std::set<int>& covered)
{
    const int num_vertices = input.num_vertices;
    const int num_samples  = input.num_samples;
    const int num_indices  = output.num_indices;
    double maxErrorSq      = -std::numeric_limits<double>::max();
    int mostDistantVertex = -1;
    for (int v = 0; v < num_vertices; ++v) {
        if (covered.find(v) != covered.end()) {
            continue;
        }
        int index = output.skinIndex[v * num_indices + 0];
        double errSq = 0;
        for (int s = 0; s < num_samples; ++s) {
            float4x4 bm = output.skinMatrix[s * input.num_joints + index].asMatrix();
            float3 diff = input.sample[s * num_vertices + v] - input.restShape[v] * bm;
            errSq += diff * diff;
        }
        if (errSq > maxErrorSq) {
            maxErrorSq = errSq;
            mostDistantVertex = v;
        }
    }
    return mostDistantVertex;
}

// solving p-center problem
static void clusterVerticesPcenter(Output& output, const Input& input, int transformType)
{
    const int& num_indices = output.num_indices;

    output.num_joints = input.num_joints;
    std::fill(output.skinIndex.begin(), output.skinIndex.end(), -1);
    std::fill(output.skinWeight.begin(), output.skinWeight.end(), 0.0);
    std::fill(output.skinMatrix.begin(), output.skinMatrix.end(), float4x4::identity);
    std::fill(output.restJointPos.begin(), output.restJointPos.end(), float3::origin);

    char buf[512];
    int stable_vertex = findMostStableVertex(input);
    output.weights[stable_vertex * num_indices] = { 0, 1.0f };
    output.restJointPos[0] = input.restShape[stable_vertex];
    std::vector<int> joint_vertices(output.num_joints);
    joint_vertices[0] = stable_vertex;
    sprintf(buf, "Added to Vertex %d (stable)", stable_vertex);

    for (int jid = 1; jid < input.num_joints; ++jid) {
        double max_dist = -1.0;
        int distant_vertex = -1;
        for (int i = 0; i < input.num_vertices; ++i) {
            if (std::find(joint_vertices.begin(), joint_vertices.end(), i) != joint_vertices.end()) {
                continue;
            }
            double mindist = std::numeric_limits<double>::max();
            for (int j = 0; j < jid; ++j) {
                const int joint = joint_vertices[j];
                float3 v = input.restShape[i] - input.restShape[joint];
                if (mindist > v.length()) {
                    mindist = v.length();
                }
            }
            if (mindist > max_dist)
            {
                max_dist = mindist;
                distant_vertex = i;
            }
        }
        output.restJointPos[jid] = input.restShape[distant_vertex];
        joint_vertices[jid] = distant_vertex;
        sprintf(buf, "Added to Vertex %d, %f", distant_vertex, max_dist);
    }

    for (int i = 0; i < input.num_vertices; ++i) {
        double min_dist = std::numeric_limits<double>::max();
        int nearest_joint = -1;
        for (int j = 0; j < output.num_joints; ++j) {
            const int joint = joint_vertices[j];
            float3 v = input.restShape[i] - input.restShape[joint];
            if (v.length() < min_dist) {
                min_dist = v.length();
                nearest_joint = j;
            }
        }
        output.skinIndex[i * num_indices] = nearest_joint;
        output.skinWeight[i * num_indices] = 1.0;
    }
    detectNeighborClusters(input, output);
    updateJointTransformProc(output, transformType, input);
}

static void clusterVerticesAdaptive(Output& output, const Input& input, int transformType)
{
    const int& num_indices = output.num_indices;

    output.num_joints = 0;
    std::fill(output.skinIndex.begin(), output.skinIndex.end(), -1);
    std::fill(output.skinWeight.begin(), output.skinWeight.end(), 0.0);
    std::fill(output.skinMatrix.begin(), output.skinMatrix.end(), float4x4::identity);
    output.restJointPos.clear();

    std::set<int> covered;
    char buf[512];

    int stable_vertex = findMostStableVertex(input);
    output.skinIndex[stable_vertex * num_indices] = 0;
    output.skinWeight[stable_vertex * num_indices] = 1.0;
    output.restJointPos.push_back(input.restShape[stable_vertex]);
    covered.insert(stable_vertex);
    sprintf(buf, "Added to Vertex %d (stable)", stable_vertex);

    for (int i = 0; i < input.neighbor[stable_vertex].size(); ++i) {
        int neighbor = input.neighbor[stable_vertex][i].first;
        if (neighbor < 0) {
            continue;
        }
        output.skinIndex[neighbor * num_indices] = 0;
        output.skinWeight[neighbor * num_indices] = 1.0;
        covered.insert(neighbor);
    }
    updateJointTransformProc(output, transformType, input, 0);
    for (int v = 0; v < input.num_vertices; ++v) {
        output.skinIndex[v * num_indices] = 0;
        output.skinWeight[v * num_indices] = 1.0;
    }
    output.num_joints = 1;

    for (int iteration = 0; iteration < input.num_joints + 10; ++iteration) {
        if (output.num_joints >= input.num_joints || covered.size() >= input.num_vertices) {
            break;
        }
        int distant_vertex = findDistantVertex(input, output, covered);
        output.skinIndex[distant_vertex * num_indices] = output.num_joints;
        output.restJointPos.push_back(input.restShape[distant_vertex]);
        covered.insert(distant_vertex);
        for (int i = 0; i < input.neighbor[distant_vertex].size(); ++i) {
            int neighbor = input.neighbor[distant_vertex][i].first;
            if (neighbor < 0) {
                continue;
            }
            output.skinIndex[neighbor * num_indices] = output.num_joints;
            covered.insert(neighbor);
        }
        updateJointTransformProc(output, transformType, input, output.num_joints);
        ++output.num_joints;
        output.num_joints = bindVertexToJoint(output, input);

        sprintf(buf, "Added to Vertex %d", distant_vertex);
    }
    detectNeighborClusters(input, output);
}

static void initialize(Output& output, Input& input)
{
    PyArrayObject* initPos = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 0));
    PyArrayObject* shapeSample = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 1));
    PyArrayObject* neighborVertices = reinterpret_cast<PyArrayObject*>(PyTuple_GET_ITEM(args, 2));
    const long numJoints = PyInt_AsLong(PyTuple_GET_ITEM(args, 3));
    const long numIndices = PyInt_AsLong(PyTuple_GET_ITEM(args, 4));
    const long numRings = PyInt_AsLong(PyTuple_GET_ITEM(args, 5));
    const long numVertices = static_cast<long>(initPos->dimensions[0]);
    const long numSamples = static_cast<long>(shapeSample->dimensions[0]);
    // allocation
    input.numVertices = numVertices;
    input.numSamples = numSamples;
    input.numJoints = numJoints;
    input.numRings = numRings;
    input.rest_shape.points.resize(numVertices);
    input.sample.resize(numSamples);
    input.neighbor.resize(numVertices);

    output.joints.resize(numJoints);
    for (auto& jd : output.joints) {
        jd.weights.resize(numVertices);
        jd.matrix.resize(numSamples);
    }

    output.skinMatrix = std::vector<MTransformationMatrix>(numSamples * numJoints, MTransformationMatrix::identity);
    output.restJointPos = std::vector<MPoint>(numJoints);
    output.vertCluster = std::vector<std::vector<int>>(numVertices);

    // size normalization
    MVector centroid(0, 0, 0);
    MVector minPos(1.0e10, 1.0e10, 1.0e10), maxPos(-1.0e10, -1.0e10, -1.0e10);
    for (int v = 0; v < numVertices; ++v) {
        MVector p(GET_DOUBLE2(initPos, v, 0),
            GET_DOUBLE2(initPos, v, 1),
            GET_DOUBLE2(initPos, v, 2));
        input->restShape[v] = p;
        centroid += p;
        for (int i = 0; i < 3; ++i) {
            minPos[i] = std::min(minPos[i], p[i]);
            maxPos[i] = std::max(maxPos[i], p[i]);
        }
    }
    centroid /= numVertices;
    double scale = std::max(std::max(maxPos.x - minPos.x, maxPos.y - minPos.y), maxPos.z - minPos.z);
    MMatrix m = MMatrix::identity;
    m(0, 0) = m(1, 1) = m(2, 2) = 1.0 / scale;
    m(3, 0) = -centroid.x / scale;
    m(3, 1) = -centroid.y / scale;
    m(3, 2) = -centroid.z / scale;
    input->normalizer = m;
    // normalized samples
    for (int v = 0; v < numVertices; ++v) {
        input->restShape[v] = input->restShape[v] * input->normalizer;
        for (int s = 0; s < numSamples; ++s) {
            MPoint p(GET_DOUBLE3(shapeSample, s, v, 0),
                GET_DOUBLE3(shapeSample, s, v, 1),
                GET_DOUBLE3(shapeSample, s, v, 2));
            input->sample[s * numVertices + v] = p * input->normalizer;
        }
    }

    // one-ring neighbor [Le and Deng 2014]
    for (int v = 0; v < numVertices; ++v) {
        double lv = 0.0;
        for (int i = 0; i < neighborVertices->dimensions[1]; ++i) {
            long n = GET_LONG2(neighborVertices, v, i);
            if (n < 0) {
                continue;
            }
            double len = (input->restShape[v] - input->restShape[n]).length();
            if (len > 0) {
                // [Le and Deng 2014]
                double dsqsum = 0;
                for (int s = 0; s < input->numSamples; ++s) {
                    float diff = (input->restShape[v] - input->restShape[n]).length()
                        - (input->sample[s * numVertices + v] - input->sample[s * numVertices + n]).length();
                    dsqsum += diff * diff;
                }
                dsqsum += 1.0e-10;
                double dvn = 1.0 / std::sqrt(dsqsum / input->numSamples);
                input->neighbor[v].push_back(std::make_pair(n, dvn));
                lv += dvn;
            }
        }
        for (int n = 0; n < input->neighbor[v].size(); ++n) {
            input->neighbor[v][n].second /= -lv;
        }
    }
}
#endif

} // namespace ssds


#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>

#include <iostream>
#include <vector>
#include <algorithm>

#include <Eigen/Core>
#include <Eigen/Geometry>
// From libshell by Etienne Vouga
double angle(const Eigen::Vector3d &v, const Eigen::Vector3d &w, const Eigen::Vector3d &axis,
    Eigen::Matrix<double, 1, 6> *derivative, // v, w
    Eigen::Matrix<double, 6, 6> *hessian
)
{
  const auto crossMatrix = [](Eigen::Vector3d v)->Eigen::Matrix3d
  {
      Eigen::Matrix3d ret;
      ret << 0, -v(2), v(1),
          v(2), 0, -v(0),
          -v(1), v(0), 0;
      return ret;
  };

  // This is a bad idea, if v and w are colinear, this derivatives below still
  // make sense but this will make axis 0 and nuke everything.
  //// This is unnecessary if the caller would promise that axis is unit and
  //// orthogonal to v and w
  const Eigen::Vector3d vcrossw = v.cross(w);
  //const Eigen::Vector3d axis = ((vcrossw.dot(in_axis)>0?1:-1) * vcrossw).eval().normalized();
  //std::cout<<"in: "<<in_axis.transpose()<<std::endl;
  //std::cout<<"vcrossw: "<<vcrossw.transpose()<<std::endl;
  //std::cout<<"ef: "<<axis.transpose()<<std::endl;

    double theta = 2.0 * atan2((vcrossw.dot(axis)), v.dot(w) + v.norm() * w.norm());

    if (derivative)
    {
        derivative->segment(0, 3) = -axis.cross(v) / v.squaredNorm();
        derivative->segment(3, 3) = axis.cross(w) / w.squaredNorm();
    }
    if (hessian)
    {
        hessian->setZero();
        hessian->block(0, 0, 3, 3) += 2.0 * (axis.cross(v))*v.transpose() / v.squaredNorm() / v.squaredNorm();
        hessian->block(3, 3, 3, 3) += -2.0 * (axis.cross(w))*w.transpose() / w.squaredNorm() / w.squaredNorm();
        hessian->block(0, 0, 3, 3) += -crossMatrix(axis) / v.squaredNorm();
        hessian->block(3, 3, 3, 3) += crossMatrix(axis) / w.squaredNorm();

        double sigma = 1.0;
        if (v.cross(w).dot(axis) < 0)
            sigma = -1.0;

        double vwnorm = v.cross(w).norm();
        if (vwnorm > 1e-8)
        {
            Eigen::Matrix3d da = sigma * (1.0 / vwnorm * Eigen::Matrix3d::Identity() - 1.0 / vwnorm / vwnorm / vwnorm * (v.cross(w)) * (v.cross(w)).transpose());
            hessian->block(0, 0, 3, 3) += crossMatrix(v) / v.squaredNorm() * da * -crossMatrix(w);
            hessian->block(3, 0, 3, 3) += crossMatrix(v) / v.squaredNorm() * da * crossMatrix(v);
            hessian->block(0, 3, 3, 3) += -crossMatrix(w) / w.squaredNorm() * da * -crossMatrix(w);
            hessian->block(3, 3, 3, 3) += -crossMatrix(w) / w.squaredNorm() * da * crossMatrix(v);
        }
    }

    return theta;
}

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  Eigen::MatrixXd V,W,A;
  mexErrMsgTxt(nrhs>=2,"nrhs should be == 2");
  parse_rhs_double(prhs+0,V);
  parse_rhs_double(prhs+1,W);
  mexErrMsgTxt(V.cols()==W.cols(),"dims should be the same");
  mexErrMsgTxt(V.rows()==W.rows(),"dims should be the same");
  const int n = V.rows();
  const int dim = V.cols();
  if(dim == 3)
  {
    mexErrMsgTxt(nrhs>=3,"nrhs should be == 3");
    parse_rhs_double(prhs+2,A);
  }else if(dim == 2)
  {
    V.conservativeResize(V.rows(),3);
    W.conservativeResize(W.rows(),3);
    V.col(2).setZero();
    W.col(2).setZero();
    A.resize(n,3);
    A.col(0).setConstant(0);
    A.col(1).setConstant(0);
    A.col(2).setConstant(1);
  }
  Eigen::MatrixXd theta(n,1);
  Eigen::MatrixXd dthetadV(n,dim);
  Eigen::MatrixXd dthetadW(n,dim);
  Eigen::MatrixXd d2thetadV2(n,dim==2?3:6);
  Eigen::MatrixXd d2thetadW2(n,dim==2?3:6);
  Eigen::MatrixXd d2thetadVW(n,dim==2?3:6);
  const Eigen::MatrixXi voigt2 =
    (Eigen::MatrixXi(3,2)<<0,0,1,1,0,1).finished();
  const Eigen::MatrixXi voigt = (dim==2? voigt2 :
    (Eigen::MatrixXi(6,2)<<0,0,1,1,2,2,1,2,0,2,0,1).finished());
  for(int i = 0;i<n;i++)
  {
    Eigen::Matrix<double, 1, 6> derivative;
    Eigen::Matrix<double, 6, 6> hessian;
    theta(i) = angle(
        V.row(i).transpose(),W.row(i).transpose(),A.row(i).transpose(),
        &derivative,&hessian);
    dthetadV.row(i) = derivative.segment(0,dim);
    dthetadW.row(i) = derivative.segment(3,dim);
    for(int b = 0;b<voigt2.rows();b++)
    {
      const int bi = voigt2(b,0);
      const int bj = voigt2(b,1);
      const auto & block = hessian.block(bi*3,bj*3,dim,dim);
      for(int v = 0;v<voigt.rows();v++)
      {
        const int vi = voigt(v,0);
        const int vj = voigt(v,1);
        const double h = block(vi,vj);
        switch(b)
        {
          case 0: d2thetadV2(i,v) = h; break;
          case 1: d2thetadW2(i,v) = h; break;
          case 2: d2thetadVW(i,v) = h; break;
        }
      }
    }
  }

  switch(nlhs)
  {
    case 6:
      prepare_lhs_double(d2thetadVW,plhs+5);
    case 5:
      prepare_lhs_double(d2thetadW2,plhs+4);
    case 4:
      prepare_lhs_double(d2thetadV2,plhs+3);
    case 3:
      prepare_lhs_double(dthetadW,plhs+2);
    case 2:
      prepare_lhs_double(dthetadV,plhs+1);
    case 1:
      prepare_lhs_double(theta,plhs+0);
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
{
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef AngleAxis<Scalar> AngleAxisx;
  using std::abs;

  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
    Vector3 eabis = m.eulerAngles(I,J,K); \
    Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \
    VERIFY_IS_APPROX(m,  mbis); \
    /* If I==K, and ea[1]==0, then there no unique solution. */ \
    /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ \
    if( (I!=K || ea[1]!=0) && (I==K || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision<Scalar>())) ) VERIFY((ea-eabis).norm() <= test_precision<Scalar>()); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
  VERIFY_EULER(0,1,0, X,Y,X);
  VERIFY_EULER(0,2,1, X,Z,Y);
  VERIFY_EULER(0,2,0, X,Z,X);

  VERIFY_EULER(1,2,0, Y,Z,X);
  VERIFY_EULER(1,2,1, Y,Z,Y);
  VERIFY_EULER(1,0,2, Y,X,Z);
  VERIFY_EULER(1,0,1, Y,X,Y);

  VERIFY_EULER(2,0,1, Z,X,Y);
  VERIFY_EULER(2,0,2, Z,X,Z);
  VERIFY_EULER(2,1,0, Z,Y,X);
  VERIFY_EULER(2,1,2, Z,Y,Z);
}

template<typename Scalar> void eulerangles()
{
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Array<Scalar,3,1> Array3;
  typedef Quaternion<Scalar> Quaternionx;
  typedef AngleAxis<Scalar> AngleAxisx;

  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
  Quaternionx q1;
  q1 = AngleAxisx(a, Vector3::Random().normalized());
  Matrix3 m;
  m = q1;

  Vector3 ea = m.eulerAngles(0,1,2);
  check_all_var(ea);
  ea = m.eulerAngles(0,1,0);
  check_all_var(ea);

  ea = (Array3::Random() + Array3(1,1,0))*Scalar(M_PI)*Array3(0.5,0.5,1);
  check_all_var(ea);

  ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(M_PI));
  check_all_var(ea);

  ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(M_PI));
  check_all_var(ea);

  ea[1] = 0;
  check_all_var(ea);

  ea.head(2).setZero();
  check_all_var(ea);

  ea.setZero();
  check_all_var(ea);
}

void test_geo_eulerangles()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( eulerangles<float>() );
    CALL_SUBTEST_2( eulerangles<double>() );
  }
}


// BLAS level 3 -- complex numbers

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (2, 2);
  a (0, 0) = cmplx_t (1., 0.);
  a (0, 1) = cmplx_t (2., 0.);
  a (1, 0) = cmplx_t (3., 0.);
  a (1, 1) = cmplx_t (4., 0.);
  print_m (a, "A");
  cout << endl;

  m_t b (2, 3);
  b (0, 0) = cmplx_t (1., 0.);
  b (0, 1) = cmplx_t (2., 0.);
  b (0, 2) = cmplx_t (3., 0.);
  b (1, 0) = cmplx_t (1., 0.);
  b (1, 1) = cmplx_t (2., 0.);
  b (1, 2) = cmplx_t (3., 0.);
  print_m (b, "B");
  cout << endl;

  m_t c (2, 3);

  // c = a b
  atlas::gemm (a, b, c);
  print_m (c, "A B");
  cout << endl;

  a (0, 0) = cmplx_t (0., 1.);
  a (0, 1) = cmplx_t (0., 2.);
  a (1, 0) = cmplx_t (0., 3.);
  a (1, 1) = cmplx_t (0., 4.);
  print_m (a, "A");
  cout << endl;

  // c = a b
  atlas::gemm (CblasNoTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  // c = a^T b
  atlas::gemm (CblasTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A^T B");
  cout << endl;

  // c = a^H b
  atlas::gemm (CblasConjTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "A^H B");

  cout << endl;

}


#include "model-magick/MeshStatsCalculator.h"

#include <cmath>

#include <Eigen/Geometry>

namespace ModelMagick
{

using namespace std;
using namespace oneapi::tbb;
using namespace oneapi::tbb::flow;

Eigen::VectorXf calculateSolidAnglesForPlanarTriangles(
    const Mesh& mesh,
    const Eigen::RowVector3f& queryPoint)
{
    // https://igl.ethz.ch/projects/winding-number/robust-inside-outside-segmentation-using-generalized-winding-numbers-siggraph-2013-jacobson-et-al.pdf
    // Eq. 6.

    Eigen::VectorXf solidAngles;
    const int m = mesh.numFaces();
    solidAngles.resize(m);
    parallel_for(
        blocked_range<size_t>(0, m),
        [&solidAngles, &mesh, &queryPoint](const blocked_range<size_t>& r) {
            for (size_t i = r.begin(); i != r.end(); ++i) {
                // triangle points in 3x3 matrix
                //       [a1, a2, a3]
                // abc = [b1, b2, b3]
                //       [c1, c2, c3]
                const Eigen::Matrix3f abc
                    = mesh.vertices(mesh.indices(i, Eigen::all), Eigen::all).rowwise() - queryPoint;
                // row norms in column vector
                const Eigen::Vector3f normAbc = abc.rowwise().norm();
                const float d0 = normAbc.prod();
                const float d1 = abc.row(0).dot(abc.row(1)) * normAbc(2);
                const float d2 = abc.row(1).dot(abc.row(2)) * normAbc(0);
                const float d3 = abc.row(0).dot(abc.row(2)) * normAbc(1);
                solidAngles(i) = 2 * std::atan2(abc.determinant(), d0 + d1 + d2 + d3);
            }
        });
    return solidAngles;
}

Eigen::VectorXf calculateGeneralizedWindingNumber(
    const Mesh& mesh,
    const Eigen::MatrixX3f& queryPoints)
{
    Eigen::VectorXf windingNumbers;
    const int numQuery = queryPoints.rows();
    windingNumbers.resize(numQuery, 1);
    parallel_for(
        blocked_range<size_t>(0, numQuery),
        [&windingNumbers, &mesh, &queryPoints](const blocked_range<size_t>& r) {
            for (size_t i = r.begin(); i != r.end(); ++i) {
                windingNumbers(i)
                    = 0.25 * M_1_PI
                      * calculateSolidAnglesForPlanarTriangles(mesh, queryPoints.row(i)).sum();
            }
        });
    return windingNumbers;
}

// oneapi::tbb::flow::function_node<Mesh, Eigen::MatrixX3f, Eigen::VectorXf>
// createGeneralizedWindingNumberCalculator(
//     oneapi::tbb::flow::graph& graph,
//     std::size_t concurrency = oneapi::tbb::flow::unlimited);

}  // namespace ModelMagick


// TODO: Change from BOOST to LEMON
#define BOOST_TEST_MODULE sub_graph_iso_test

#include <boost/test/included/unit_test.hpp> // Testing

#include <boost/graph/adjacency_list.hpp> // Boost Graph
#include <boost/graph/graphviz.hpp>

#include "sub_graph_iso/sub_graph_iso.hpp" // Modified Boost sub_graph_iso file

#include "lib/complete_graph.hpp" // Helper to create complete graphs.

using namespace boost;

BOOST_AUTO_TEST_SUITE( test_sub_graph_induced )

BOOST_AUTO_TEST_CASE( small_sub_graph_finds_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	// Build graph
	int num_vertices1 = 8;
	graph_type sub(num_vertices1);
	add_edge(0, 6, sub);
	add_edge(0, 7, sub);
	add_edge(1, 5, sub);
	add_edge(1, 7, sub);
	add_edge(2, 4, sub);
	add_edge(2, 5, sub);
	add_edge(2, 6, sub);
	add_edge(3, 4, sub);

	// Build sub
	int num_vertices2 = 9;
	graph_type graph(num_vertices2);
	add_edge(0, 6, graph);
	add_edge(0, 8, graph);
	add_edge(1, 5, graph);
	add_edge(1, 7, graph);
	add_edge(2, 4, graph);
	add_edge(2, 7, graph);
	add_edge(2, 8, graph);
	add_edge(3, 4, graph);
	add_edge(3, 5, graph);
	add_edge(3, 6, graph);

	// Create callback to print mappings
	vf2_empty_callback < graph_type, graph_type > callback(sub, graph);

	// Print out all subgraph isomorphism mappings between graph and sub.
	// Vertices and edges are assumed to be always equivalent.
	const bool result = vf2_subgraph_mono(sub, graph, callback);

	BOOST_TEST(result);

}

// Test complete graph K4 embedding in K7.
BOOST_AUTO_TEST_CASE( K4_K7_finds_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	graph_type graph_K7 = complete_undirected(7);

	graph_type graph_K4 = complete_undirected(4);

	vf2_empty_callback < graph_type, graph_type > callback(graph_K4, graph_K7);

	const bool result = vf2_subgraph_mono(graph_K4, graph_K7, callback);

	BOOST_TEST(result);

}

// Test medium-sized embedding of K6 into K9.
BOOST_AUTO_TEST_CASE ( K6_K9_finds_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	graph_type graph_K9 = complete_undirected(9);
	graph_type graph_K6 = complete_undirected(6);

	vf2_empty_callback < graph_type, graph_type > callback(graph_K6, graph_K9);

	const bool result = vf2_subgraph_mono(graph_K6, graph_K9, callback);

	BOOST_TEST(result);
}

// Test no embedding of K4 into complete bipartite graph KB3,4.
BOOST_AUTO_TEST_CASE ( K4_bipartite_no_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	graph_type graph_KB3c4(7);
	for (int i = 0; i <= 3; i++) {
		for (int j = 4; j <= 7; j++) {
			add_edge(i, j, graph_KB3c4);
		}
	}

	graph_type graph_K4 = complete_undirected(4);

	vf2_empty_callback < graph_type, graph_type > callback(graph_K4, graph_KB3c4);

	const bool result = vf2_subgraph_mono(graph_K4, graph_KB3c4, callback);

	BOOST_TEST(result == false);

}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( test_sub_graph_non_induced )

// Test sqaure embeds in K4.
BOOST_AUTO_TEST_CASE( square_K4_finds_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	graph_type graph_square(4);
	add_edge(0, 1, graph_square);
	add_edge(1, 2, graph_square);
	add_edge(2, 3, graph_square);
	add_edge(3, 0, graph_square);

	graph_type graph_K4 = complete_undirected(4);

	vf2_empty_callback < graph_type, graph_type > callback(graph_square, graph_K4);

	const bool result = vf2_subgraph_mono(graph_square, graph_K4, callback);

	BOOST_TEST(result);

}

// Test almost complete graph K4 embedding in K7.
BOOST_AUTO_TEST_CASE( almost_K4_K7_finds_iso )
{

	typedef adjacency_list< setS, vecS, undirectedS > graph_type;

	graph_type graph_K7 = complete_undirected(7);

	graph_type graph_almost_K4 = complete_undirected(4);
	remove_edge(0, 1, graph_almost_K4);

	// Create callback to print mappings
	vf2_empty_callback < graph_type, graph_type > callback(graph_almost_K4, graph_K7);

	// Find subgraph isomorphism mappings between K7 and K4.
	// Vertices and edges are assumed to be always equivalent.
	const bool result = vf2_subgraph_mono(graph_almost_K4, graph_K7, callback);

	BOOST_TEST(result);

}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

TEST(ProbDistributionBinomiali, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::binomial_rng(4, 0.6, rng));
  EXPECT_THROW(stan::math::binomial_rng(-4, 0.6, rng),std::domain_error);
  EXPECT_THROW(stan::math::binomial_rng(4,-0.6, rng),std::domain_error);
  EXPECT_THROW(stan::math::binomial_rng(4, 2.6, rng),std::domain_error);
  EXPECT_THROW(stan::math::binomial_rng(4,stan::math::positive_infinity(), rng),
               std::domain_error);
}

TEST(ProbDistributionsBinomial, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::binomial_distribution<>dist (100, 0.6);
  boost::math::chi_squared mydist(K-1);

  int loc[K - 1];
  for(int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  int bin [K];
  double expect [K];
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K-1] = N * (1 - cdf(dist, K-2));

  while (count < N) {
    int a = stan::math::binomial_rng(100, 0.6, rng);
    int i = 0;
    while (i < K-1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;

  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


#include <cstring>
#include <iostream>
#include <fstream>
#include <armadillo>

#include "lap3d.h"
#include "shiftengine3d.h"

using namespace arma;

float CG_PSNR3D(CubeType Base_image, CubeType Recon_image);

int main(int argc, char *argv[])
{
	std::string firstImagePath;
	std::string secondImagePath;
	std::string resultImagePath;

	SizeType rows;
	SizeType cols;
	SizeType slices;

	ColType i1_v;
	ColType i2_v;

	if (argc < 2) {
		std::cerr << "Usage: " << std::endl;
		std::cerr << std::endl;
		std::cerr << "  " << argv[0] << " <path-to-image1> <path-to-image2> <path-to-result-image> <image-rows> <image-columns> <image-slices>" << std::endl;
		std::cerr << "  " << argv[0] << " <pointer-to-image1> <pointer-to-image2> <pointer-to-result-image>, <image-rows> <image-columns> <image-slices>" << std::endl;
		std::cerr << std::endl;
		std::cerr << "Run '" << argv[0] << " --help' for more information" << std::endl;
		std::cerr << std::endl;

		return EXIT_FAILURE;
	}

	// Output help string and exit (otherwise segmentation fault!)
	if (strcmp(argv[1], "--help") == 0) {
		std::cout << "There are no more information implemented yet." << std::endl;

		return EXIT_FAILURE;
	}

	firstImagePath = argv[1];
	secondImagePath = argv[2];
	resultImagePath = argv[3];

	rows = strtol(argv[4], NULL, 10);
	cols = strtol(argv[5], NULL, 10);
	slices = strtol(argv[6], NULL, 10);

	//Load iamges (saved as raw ascii)
	i1_v.load(firstImagePath, raw_binary);
	i2_v.load(secondImagePath, raw_binary);

	//Shape them to 3D pictures with correct dimensions
	CubeType i1(i1_v.memptr(), rows, cols, slices);
	CubeType i2(i2_v.memptr(), rows, cols, slices);

	i1.subcube(span(0,3),span(0,3),span(0,3)).print("i1");

	//Construct two 3D Images
	//Construct the LocalAllpass Algorithm Object with Level min and max
	Gadgetron::LAP3D mLAP3D(i1, i2, 0, 4);

	field<CubeType> flow_estimation = mLAP3D.exec();

	//Shift first image according to estimated optical flow
	Gadgetron::ShiftEngine3D shifter(i1, flow_estimation(0), flow_estimation(1), flow_estimation(2));
	CubeType i_hat_fast = shifter.execCubicShift();
	//Save result image
	Col<PixelType> i_hat_fast_v = vectorise(i_hat_fast);
	i_hat_fast_v.save(resultImagePath, raw_ascii);

	std::cout << "PSNR_uB_Fast: " << CG_PSNR3D(i2, i_hat_fast) << std::endl;

	return EXIT_SUCCESS;
}

float CG_PSNR3D(CubeType Base_image, CubeType Recon_image)
{
	int m = Recon_image.n_rows;
	int n = Recon_image.n_cols;
	int p = Recon_image.n_slices;

	float Max_I = arma::max(arma::abs(vectorise(Base_image)));

	float MSE = accu(arma::pow(arma::abs(Base_image-Recon_image),2)) / (n*m*p);

	float PSNR = 10 * std::log10(std::pow(Max_I, 2) / MSE);

	return PSNR;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EVecPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <gtest/gtest.h>

#include <Eigen/Core>

#include "smooth/feedback/compat/ipopt.hpp"
#include "smooth/feedback/ocp.hpp"

template<typename T>
using Vec = Eigen::VectorX<T>;

/// @brief Objective function
const auto theta = []<typename T>(T, const Vec<T> &, const Vec<T> &, const Vec<T> & q) -> T {
  return q.x();
};

/// @brief Dynamics
const auto f = []<typename T>(T, const Vec<T> & x, const Vec<T> & u) -> Vec<T> {
  return Vec<T>{{x.y(), u.x()}};
};

/// @brief Integrals
const auto g = []<typename T>(T, const Vec<T> & x, const Vec<T> & u) -> Vec<T> {
  return Vec<T>{{x.squaredNorm() + u.squaredNorm()}};
};

/// @brief Running constraints
const auto cr = []<typename T>(T, const Vec<T> &, const Vec<T> & u) -> Vec<T> {
  return Vec<T>{{u.x()}};
};

/// @brief End constraints
const auto ce =
  []<typename T>(T tf, const Vec<T> & x0, const Vec<T> & xf, const Vec<T> &) -> Vec<T> {
  Vec<T> ret(5);
  ret << tf, x0, xf;
  return ret;
};

/// @brief Range to std::vector
const auto r2v = []<std::ranges::range R>(const R & r) {
  return std::vector(std::ranges::begin(r), std::ranges::end(r));
};

TEST(OcpIpopt, Solve)
{
  // define optimal control problem
  smooth::feedback::FlatOCP<decltype(theta), decltype(f), decltype(g), decltype(cr), decltype(ce)>
    ocp{
      .nx    = 2,
      .nu    = 1,
      .nq    = 1,
      .ncr   = 1,
      .nce   = 5,
      .theta = theta,
      .f     = f,
      .g     = g,
      .cr    = cr,
      .crl   = Vec<double>{{-1}},
      .cru   = Vec<double>{{1}},
      .ce    = ce,
      .cel   = Vec<double>{{3, 1, 1, 0, 0}},
      .ceu   = Vec<double>{{6, 1, 1, 0, 0}},
    };

  // define mesh
  smooth::feedback::Mesh mesh;
  mesh.refine_ph(0, 4 * 5);
  const auto [nodes, weights] = mesh.all_nodes_and_weights();

  // transcribe optimal control problem to nonlinear programming problem
  const auto nlp = smooth::feedback::ocp_to_nlp(ocp, mesh);

  // solve nonlinear programming problem
  const auto nlp_sol = smooth::feedback::solve_nlp_ipopt(
    nlp,
    std::nullopt,
    {
      {"print_level", 0},
    },
    {
      {"linear_solver", "mumps"},
      {"hessian_approximation", "limited-memory"},
    },
    {
      {"tol", 1e-8},
    });

  ASSERT_EQ(nlp_sol.status, smooth::feedback::NLPSolution::Status::Optimal);

  // convert solution of nlp insto solution of ocp
  const auto ocp_sol = smooth::feedback::nlpsol_to_ocpsol(ocp, mesh, nlp_sol);

  const auto nlp_sol_copy = smooth::feedback::ocpsol_to_nlpsol(ocp, mesh, ocp_sol);

  ASSERT_LE((nlp_sol_copy.x - nlp_sol.x).norm(), 1e-8);
  ASSERT_LE((nlp_sol_copy.zl - nlp_sol.zl).norm(), 1e-8);
  ASSERT_LE((nlp_sol_copy.zu - nlp_sol.zu).norm(), 1e-8);
  ASSERT_LE((nlp_sol_copy.lambda - nlp_sol.lambda).norm(), 1e-8);

  // solve again with warmstart
  const auto nlp_sol_warm = smooth::feedback::solve_nlp_ipopt(
    nlp,
    nlp_sol_copy,
    {
      {"print_level", 0},
    },
    {
      {"linear_solver", "mumps"},
      {"hessian_approximation", "limited-memory"},
    },
    {
      {"tol", 1e-8},
    });

  ASSERT_LE(nlp_sol_warm.iter, 6);
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Free energy, energy, and specific heat of triangular lattice Ising model

// reference: R. M. F. Houtappel, Physica 16, 425 (1950)

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/simpson.hpp>
#include "common.hpp"

namespace ising {
namespace free_energy {
namespace triangular {

namespace {

template<typename T, typename FVAR>
struct functor {
  functor(T Ja, T Jb, T Jc, FVAR beta) {
    using std::cosh; using std::sinh;
    T pi = boost::math::constants::pi<T>();
    c_ = 1 / (8 * pi * pi);
    sh2a_ = sinh(2 * beta * Ja);
    sh2b_ = sinh(2 * beta * Jb);
    sh2c_ = sinh(2 * beta * Jc);
    cshabc_ = cosh(2 * beta * Ja) * cosh(2 * beta * Jb) * cosh(2 * beta * Jc)
      + sh2a_ * sh2b_ * sh2c_;
  }
  FVAR operator()(T t1, T t2) const {
    using std::cos; using std::log;
    return c_ * log(cshabc_ - sh2a_ * cos(t1) - sh2b_ * cos(t2) - sh2c_ * cos(t1 + t2));
  }
  T c_;
  FVAR sh2a_, sh2b_, sh2c_, cshabc_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Ja, T Jb, T Jc, FVAR beta) {
  return functor<T, FVAR>(Ja, Jb, Jc, beta);
}

}

template<typename T, typename U>
inline U infinite(T Ja, T Jb, T Jc, U beta) {
  const unsigned long max_n = 1 << 16;
  typedef T real_t;
  if (Ja * Jb * Jc <= 0)
    throw(std::invalid_argument("Ja * Jb * Jc should be positive"));
  if (beta <= 0)
    throw(std::invalid_argument("beta should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  auto logZ = log(real_t(2)) +
    standards::simpson_2d(func(Ja, Jb, Jc, beta), real_t(0), real_t(0), 2*pi, 2*pi, 8, 8);
  auto pz = logZ;
  auto pe = abs(beta * beta * logZ.derivative(2) / logZ);
  for (unsigned long n = 16; n <= max_n; n *= 2) {
    logZ = log(real_t(2)) +
      standards::simpson_2d(func(Ja, Jb, Jc, beta), real_t(0), real_t(0), 2*pi, 2*pi, n, n);
    auto pn = abs(beta * beta * (logZ - pz).derivative(2) / logZ);
    if (pn < 2 * std::numeric_limits<real_t>::epsilon() || pn > pe) break;
    if (n == max_n) std::cerr << "Warning: integration not converge with n = " << max_n << std::endl;
    pz = logZ;
    pe = pn;
  }
  return - logZ / beta;
}

} // end namespace triangular
} // end namespace free_energy
} // end namespace ising


// Statistical Computing //

// Load typical packages
#include<cmath>
#include<iostream>
#include<iomanip>

// Typiccal C libraries
#include<cmath>
#include<cstdlib>

// C++ Data Structures
#include<set>
#include<map>

// Include Eigen Package for Matrices
#include <Eigen/Core>

// To calculate Time
#include<ctime>

// Load Matrix615 header file to read in from file
#include "Matrix615.h"

using namespace std;
// we avoid using namespace Eigen to be able to clearly see where we are calling the Eigen Package

// Driver Code
int main(int argc, char* argv[]) {

	// Steps corresponding to the commented code is associated with the following documents:

	// Tentative Read Matrix using Matrix615.h

	Matrix615<double> read_Design; // throw-away Matrices to read in data
	Matrix615<double> read_Y;
	Matrix615<double> read_ID;

	// Read the data in
	read_Design.readFromFile(argv[3]); // t
	read_Y.readFromFile(argv[2]);
	read_ID.readFromFile(argv[1]);

	// Step (1): Store the design matrix, response, and Id into an Eigen Matrix
	Eigen::MatrixXd	design_X;
	Eigen::MatrixXd outcome_Y;

	read_Design.cloneToEigen(design_X);
	read_Y.cloneToEigen(outcome_Y);

	// cout << Weights.rows() << endl << endl;
	// cout << Weights << endl;
	Eigen::MatrixXd Weights(outcome_Y.rows(), 1);
	// Weights.setOnes();

	// cout << "argc is " << argc << endl;

	if (int (argc) == 5) {
		Matrix615<double> read_Weights;
		read_Weights.readFromFile(argv[4]);

		Eigen::MatrixXd Weights;
		read_Weights.cloneToEigen(Weights);
	}

	else {
		Weights.setOnes();
	}

	// Step (2): Count number of unique ID's, patients
	Eigen::MatrixXd ID_Mat;
	read_ID.cloneToEigen(ID_Mat);

	std::set<int> unique_ID; // to dynamically count n
	std::map<int, int> m_map; // to dynamically count m
	std::map<int, double> weight_map;

	for (int i = 0; i < int(ID_Mat.rows()); ++i) {
		unique_ID.insert(  ID_Mat(i,0) );
		m_map[ID_Mat(i, 0)] += 1;
		weight_map[ID_Mat(i, 0)] = Weights(i, 0);
	}

	// int n = int( unique_ID.size() );

	// cout << "n is << " << n << endl;

	// cout << "size of the id to m map is " << m_map.size() << endl;

	// cout << "m of 1st patient is " << m_map[1] << " " << m_map[100] << " " << m_map[2] << endl << endl;
	// cout << "size of weight map " << weight_map.size() << endl;

	// q = (p + 1) : # number of paramters to be estimated using GEE
	int q = int ( design_X.cols() );

	// cout << "q is " << q << endl;

	// Correlation Structure

	// Step : Weights

	// Step (3): Initialize betas *********** Should we incorporate the beta's that come from a fast linear regression or no?

	Eigen::MatrixXd Betas_new(q , 1); // dimensions (p+1)  x 1
	Betas_new.setZero();

	// cout << "Betas ***********************" << endl;
	// cout << Betas_new << endl << endl;

	// Step (4): Initialize rho and phi (rho: off diagonals of correlation structure & phi: )
	double rho = 0.0;
	// double phi = 0.0;

	// Step (5): Set up variables for iteration convergence check

	// Should we make this dynamic?

	double diff_beta = 1; // updated difference in beta estimation {beta (new) - beta (old)}

	double diff_threshold = 0.00000010; // threshold for the update difference in betas

	int iteration_threshold = 1000; // threshold on how many iterations there will be

	// Step (6): Initialize the iteration
	int iteration_count = 0;

	// Step (7): Assign appropriate value to n*
	double n_star_sum = 0.0;
	double m_sum = 0.0;

	// calculate n_star_sum

	// for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
	// 	n_star_sum = n_star_sum + (0.5) * double(m_map[*iter]) * double(double(m_map[*iter]) - double(1.0));
	// }

	// Step (8): Start the GEE Estimation

	Eigen::MatrixXd Betas_updated = Betas_new;

	Eigen::MatrixXd sandwhich_Mat;

	Eigen::MatrixXd tempGI(q, q);

	// // cout << Betas_updated << endl << endl;

	int before_EE = clock();

	// Step (10)
	while ( (diff_beta > diff_threshold) && (iteration_count < iteration_threshold) ) {
		// (1)
		Betas_updated = Betas_new;

		// (2) Vector EE ((px1) x 1) or (q x 1)
		Eigen::MatrixXd EE(q, 1);
		EE.setZero();

		// (3) Matrix GI (q x q)
		Eigen::MatrixXd GI(q, q);
		GI.setZero();

		// // (4) Matrix G ( (p+1) x (p+1) )
		// // Meat of the Sandwhich Estimator
		// Eigen::MatrixXd G(q , q);
		// G.setZero();

		// (5) Initialize phi(sum) and tau(sum)
		long double phi_sum = 0.0;
		long double tau_sum = 0.0;

		// (6) Initialize indexes for block matrix multiplications
		int start = 0;
		int end = -1;

		// inner loop, we will loop using the unique_ID set (size = n)
		// Therefore, it loops n times
		for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {

			int m = m_map[*iter];
			start = end + 1;
			end = start + m - 1;

			n_star_sum = n_star_sum + (0.5) * double(m) * double(double(m) - 1.0);
			m_sum = m_sum + double(m);

			// cout << "start is " << start << endl;
			// cout << "end is " << end << endl << endl;

			// assign mu for ith observation
			// Eigen.block(starting_row = , starting_col = , dim_row = , dim_col = )
			Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

			// cout << "mu_i is here " << endl;
			// cout << mu_i << endl << endl;

			// assign r_i (mi x 1)
			Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

			// cout << "r_i is here " << endl;
			// cout << r_i << endl;

			// add r_i ^2 to phi_sum from the ith observation
			for (int j = 0; j < int(m); ++j) {
				phi_sum = phi_sum + (r_i(j,0) * r_i(j,0));
			}

			// cout << "phi_sum is " << phi_sum << endl;

			// add to tau_sum
			for (int j = 0; j < ( int(m) - 1 ); ++j) {
				for (int k = (j + 1); k < int(m); ++k) {
					// cout << " j is " << j << " and k is " << k << endl;
					tau_sum = tau_sum + (r_i(j,0) * r_i(k,0));
				}
			}

			// cout << "tau_sum is " << tau_sum << endl;

			// create R matrix (mi x mi) for each observation
			Eigen::MatrixXd R(m, m);
			R.setConstant(rho);
			for (int d = 0; d < int(m); ++d) {
				R(d, d) = 1;
			}

			// cout << R << endl << endl << endl;
			// update EE ((p+1) x 1)
			EE = EE + ((design_X.block(start, 0, m, q).transpose()) * (R.inverse() * (r_i) )); // weight_map

			// update GI ((p+1) x (p+1)) or (q x q)
			GI = GI + ((design_X.block(start, 0, m, q).transpose()) * R.inverse() *  design_X.block(start, 0, m, q));

			// update G ((p+1) x (p+1)) or (q x q)
			// G = G + ((design_X.block(start, 0, m, q).transpose()) * ( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));

		}

		// Update beta using Newton Raphson method
		Betas_new = Betas_updated + (GI.inverse() * EE);

		// calculate the difference in the betas
		diff_beta = (Betas_new - Betas_updated).norm();
		// diff_beta = (Betas_new - Betas_updated).array().abs().sum();

		// update rho
		// cout << "tau_sum is " << tau_sum << endl;
		// cout << "phi_sum is " << phi_sum << endl;

		rho = ( (( double(m_sum) - double(q) ) * tau_sum) / ( (double(n_star_sum) - double(q) ) * phi_sum) );

		// cout << "rho is " << rho << endl << endl;

		// sandwhich_Mat = GI.inverse() * G * GI.inverse();
		tempGI = GI;

		iteration_count += 1;

	}

	// (4) Matrix G ( (p+1) x (p+1) )
	// Meat of the Sandwhich Estimator
	Eigen::MatrixXd G(q , q);
	G.setZero();

	// To calculate the sandwhich estimator
	// for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
	// 	n_star_sum = n_star_sum + (0.5) * double(m_map[*iter]) * double(double(m_map[*iter]) - double(1.0));
	// }

	int start = 0;
	int end = -1;

	for (std::set<int>::iterator iter = unique_ID.begin(); iter != unique_ID.end(); ++iter) {
		int m = m_map[*iter];
		start = end + 1;
		end = start + m - 1;

		Eigen::MatrixXd mu_i = design_X.block(start, 0, m, q) * Betas_updated;

		// cout << "mu_i is here " << endl;
		// cout << mu_i << endl << endl;

		// assign r_i (mi x 1)
		Eigen::MatrixXd r_i = outcome_Y.block(start, 0, m, 1) - mu_i;

		Eigen::MatrixXd R(m, m);

		R.setConstant(rho);

		for (int d = 0; d < int(m); ++d) {
			R(d, d) = 1;
		}

		// update G ((p+1) x (p+1)) or (q x q)
		G = G + ((design_X.block(start, 0, m, q).transpose()) * ( ((R.inverse()) * r_i) * (r_i.transpose()*(R.inverse())) ) * design_X.block(start, 0, m, q));
	}

	sandwhich_Mat = tempGI.inverse() * G * tempGI.inverse();

	int after_EE = clock();

	// cout << " n start sum ended up being " << n_star_sum << endl;
	// cout << "m sum ended up being " << m_sum << endl;
	cout << "**************** GEE Results ****************" << endl;
	cout << endl << endl << "Iteration count at: " << iteration_count << endl;

	cout << "The correlation parameter: " << rho << endl;

	cout << endl << "Estimated Betas are: " << endl;
	cout << Betas_new.transpose() << endl << endl << endl;

	// cout << "The sandwhich matrix is: " << endl << endl;
	// cout << sandwhich_Mat << endl << endl;

	cout << "The beta Robust S.E.'s are: " << endl << endl;
	cout << sandwhich_Mat.diagonal().array().sqrt() << endl << endl;

	cout << " Estimated convergence time for EE is: " << endl;
	cout << " Time (secs): " << (after_EE - before_EE) / double(CLOCKS_PER_SEC) << endl;

	return 0;

}


#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>

#include "csv_reader.h"

#define SMOOTH(s) for(size_t _smooth=0;_smooth<s;++_smooth)

#define ERROR_RETURN(retval) { fprintf(stderr, "Error %d %s:line %d: \n", retval,__FILE__,__LINE__);  exit(retval); }

#define TRESHOLD (0.00001)

#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)

int main() {

  using namespace Eigen;
  using namespace pinocchio;

  // Set robot model
  bool floating_base;
  std::string robot_model = ROBOT_MODEL;

  if (robot_model == "iiwa")
  {
    std::cout << "Robot Model = " << robot_model << std::endl;
    floating_base = false;
  }
  else if ((robot_model == "hyq")|(robot_model == "atlas"))
  {
    std::cout << "Robot Model = " << robot_model << std::endl;
    floating_base = true;
  }
  else
  {
    std::cerr << "Invalid robot model: " << robot_model << "\nChoices are: iiwa, hyq, atlas" << std::endl;
    return 2;
  }

  // Import URDF model
  Model model;
  std::string urdf_filename;
  urdf_filename = RBD_BENCHMARKS_DIR"/description/urdf/"+robot_model+".urdf";
  if (floating_base)
    pinocchio::urdf::buildModel(urdf_filename,JointModelFreeFlyer(),model);
  else
    pinocchio::urdf::buildModel(urdf_filename,model);

  model.gravity = Eigen::Vector3d(0, 0, 0);
  Data data(model);
  int dof = model.nv;
  std::cout << "dof = " << dof << std::endl;

  // Import CSV inputs
  std::string input_filename;
  input_filename = RBD_BENCHMARKS_DIR"/csv/pinocchio/"+robot_model+"_inputs.csv";
  std::ifstream input_csv(input_filename.c_str());

  CSVRow row;
  VectorXd qs     = VectorXd::Zero(model.nq);
  VectorXd qdots  = VectorXd::Zero(model.nv);
  VectorXd qddots = VectorXd::Zero(model.nv);
  VectorXd taus   = VectorXd::Zero(model.nv);

  input_csv >> row;
  int tot_q, tot_qdot; // FIXME: What vector sizes do I expect from each model?
  tot_q    = model.nq;
  tot_qdot = model.nv;
  std::cout << "qs, qdots = " << tot_q << ", " << tot_qdot << std::endl;
  int col, start_col;
  input_csv >> row;
  start_col = 0.0;                 // 0
  for(int j=0;j<tot_q;++j)
  {
    col = start_col+j;
    qs[j] = atof(row[col].c_str());
  }
  //qs.segment<4>(3) /= qs.segment<4>(3).norm();
  start_col = tot_q;               // 1xQ
  for(int j=0;j<tot_qdot;++j)
  {
    col = start_col+j;
    qdots[j] = atof(row[col].c_str());
  }
  start_col = tot_q+tot_qdot;      // 1xQ+1xQd
  for(int j=0;j<tot_qdot;++j)
  {
    col = start_col+j;
    qddots[j] = atof(row[col].c_str());
  }
  start_col = tot_q+2*tot_qdot;  // 1xQ+2xQd
  for(int j=0;j<tot_qdot;++j)
  {
    col = start_col+j;
    taus[j] = atof(row[col].c_str());
  }

  std::cout << "--" << std::endl;

  // Dynamics Algorithms
#ifdef RNEA_ALG
  std::cout << "RNEA" << std::endl;
  const VectorXd& returned_value = rnea(model,data,qs,qdots,qddots);
  std::string nmcsv = "_inverse_dynamics_expected.csv";
#elif  CRBA_ALG
  std::cout << "CRBA" << std::endl;
  const MatrixXd& returned_value = crba(model,data,qs);
  std::string nmcsv = "_mass_matrix_expected.csv";
#elif  ABA_ALG
  std::cout << "ABA" << std::endl;
  const VectorXd& returned_value = aba(model,data,qs,qdots, taus);
  std::string nmcsv = "_dynamics_expected.csv";
#endif /* Dynamics Algorithms */
  std::cout << "--" << std::endl;

  std::cout << "TEST RESULTS" << std::endl;

  // Import CSV output
  std::string output_filename = RBD_BENCHMARKS_DIR"/csv/pinocchio/"+robot_model+nmcsv;
  std::ifstream output_csv(output_filename.c_str());
  output_csv >> row;

  bool flag_wrong = false;
#ifdef RNEA_ALG
  for(int j=0;j<tot_qdot;++j)
  {
    double result = returned_value[j];
    double truth  = atof(row[j].c_str());
#elif  CRBA_ALG
  for(int j=0;j<tot_qdot * tot_qdot;++j)
  {
    if((j/tot_qdot) < (j%tot_qdot)) //Only the uper triangle of the matrix is computed
      continue;

    double result = returned_value(j);
    double truth  = atof(row[j].c_str());
#elif  ABA_ALG
  for(int j=0;j<tot_qdot;++j)
  {
    double result = returned_value[j];
    double truth  = atof(row[j].c_str());
#endif

    printf("%f == %f\n", result, truth);
    double delta = fabs(truth - result);
    printf("%f\n", delta);
    if(delta > TRESHOLD)
      flag_wrong=true;
  }
  if(flag_wrong){
    std::cout << "Wrong results!!!" << std::endl;
    return 1;
  }
  else
    std::cout << "All the results were good" << std::endl;

  return 0;
}


/*
 * Copyright Nick Thompson, 2020
 * 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)
 */

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/core/demangle.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/math/quadrature/wavelet_transforms.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::constants::pi;
using boost::math::constants::root_two;
using boost::math::quadrature::daubechies_wavelet_transform;
using boost::math::quadrature::trapezoidal;

template<typename Real, int p>
void test_wavelet_transform()
{
    std::cout << "Testing wavelet transform of " << p << " vanishing moment Daubechies wavelet on type " << boost::core::demangle(typeid(Real).name()) << "\n";
    auto psi = boost::math::daubechies_wavelet<Real, p>();

    auto abs_psi = [&psi](Real x) {
        return abs(psi(x));
    };
    auto [a, b] = psi.support();
    auto psil1 = trapezoidal(abs_psi, a, b, 100*std::numeric_limits<Real>::epsilon());
    Real psi_sup_norm = 0;
    for (double x = a; x < b; x += 0.00001)
    {
        Real y = psi(x);
        if (std::abs(y) > psi_sup_norm)
        {
            psi_sup_norm = std::abs(y);
        }
    }
    // An even function:
    auto f = [](Real x) {
        return std::exp(-abs(x));
    };
    Real fmax = 1;
    Real fl2 = 1;
    Real fl1 = 2;

    auto Wf = daubechies_wavelet_transform(f, psi);
    for (double s = 0; s < 10; s += 0.01)
    {
        Real w1 = Wf(s, 0.0);
        Real w2 = Wf(-s, 0.0);
        // Since f is an even function, we get w1 = w2:
        CHECK_ULP_CLOSE(w1, w2, 12);
    }

    // The wavelet transform with respect to Daubechies wavelets
    for (double s = -10; s < 10; s += 0.1)
    {
        for (double t = -10; t < 10; t+= 0.1)
        {
            Real w = Wf(s, t);
            // Integral inequality:
            Real r1 = sqrt(abs(s))*fmax*psil1;
            if (!CHECK_LE(abs(w), r1))
            {
                std::cerr << "  Integral inequality |W[f](s,t)| <= ||f||_infty ||psi||_1 is violated.\n";
            }
            if (!CHECK_LE(abs(w), fl2))
            {
                std::cerr << "  Integral inequality | int f psi_s,t| <= ||f||_2 ||psi||_2 violated.\n";
            }
            Real r4 = sqrt(abs(s))*fl1*psi_sup_norm;
            if (!CHECK_LE(abs(w), r4))
            {
                std::cerr << "  Integral inequality |W[f](s,t)| <= sqrt(|s|)||f||_1 ||psi||_infty is violated.\n";
            }
            Real r5 = sqrt(abs(s))*fmax*psil1;
            if (!CHECK_LE(abs(w), r5))
            {
                std::cerr << "  Integral inequality |W[f](s,t)| <= sqrt(|s|)||f||_infty ||psi||_1 is violated.\n";
            }
            if (s != 0)
            {
                Real r2 = fl1*psi_sup_norm/sqrt(abs(s));
                if(!CHECK_LE(abs(w), r2))
                {
                    std::cerr << "  Integral inequality |W[f](s,t)| <= ||f||_1 ||psi||_infty/sqrt(|s|) is violated.\n";
                }
            }

        }
    }

    if (p > 5)
    {
        // Wavelet transform of a constant is zero.
        // The quadrature sum is horribly ill-conditioned (technically infinite),
        // so we'll only test on the more rapidly converging sums.
        auto g = [](Real ) { return Real(7); };
        auto Wg = daubechies_wavelet_transform(g, psi);
        for (double s = -10; s < 10; s += 0.1)
        {
            for (double t = -10; t < 10; t+= 0.1)
            {
                Real w = Wg(s, t);
                if (!CHECK_LE(abs(w), 10*sqrt(std::numeric_limits<Real>::epsilon())))
                {
                    std::cerr << "  Wavelet transform of constant with respect to " << p << " vanishing moment Daubechies wavelet is insufficiently small\n";
                }

            }
        }
        // Wavelet transform of psi evaluated at s = 1, t = 0 is L2 norm of psi:
        auto Wpsi = daubechies_wavelet_transform(psi, psi);
        CHECK_MOLLIFIED_CLOSE(Real(1), Wpsi(1,0), 2*sqrt(std::numeric_limits<Real>::epsilon()));
    }

}

int main()
{
    try{
       test_wavelet_transform<double, 2>();
       test_wavelet_transform<double, 8>();
       test_wavelet_transform<double, 16>();
       // All these tests pass, but the compilation takes too long on CI:
       //boost::hana::for_each(std::make_index_sequence<17>(), [&](auto i) {
       //    test_wavelet_transform<double, i+3>();
       //});
    }
    catch (std::bad_alloc const & e)
    {
        std::cerr << "Ran out of memory in wavelet transform test: " << e.what() << "\n";
       // not much we can do about this, this test uses lots of memory!
    }

    return boost::math::test::report_errors();
}


#pragma once

#include <numeric>
#include <type_traits>
#include <utility>
#include <vector>

#include <Eigen/Dense>

namespace pcv
{

template<class NumericType>
double getLineModelError(
    const Eigen::Vector2d &model,
    const Eigen::Matrix<NumericType, 2, 1> point)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    double yEstimate = (1.0 - model[0]*point.x())/model[1];

    return std::pow(yEstimate-point.y(), 2);
}

template<class NumericType>
Eigen::Vector2d
findLineModelFromPoints(
    std::vector<Eigen::Matrix<NumericType, 2, 1>> points)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    if (points.size() < 2)
    {
        throw std::runtime_error("There must be at least the minimum number of "
                                 "data points required by modeling function.");
    }

    Eigen::Matrix<double, 2, 2> linearEquationsLhs;
    linearEquationsLhs << points[0].x(), points[0].y(),
                          points[1].x(), points[1].y();

    Eigen::Vector2d linearEquationsRhs;
    linearEquationsRhs << 1, 1;

    Eigen::Vector2d model =
        linearEquationsLhs.colPivHouseholderQr().solve(linearEquationsRhs);

    return model;
}

} // end namespace pcv


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearloadvector.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();
  double area;
  Eigen::Matrix midpoints(2, num_nodes);

  //====================
  // Your code goes here
  // supply a builder type for the element vector
  // arising from the linear form
  // use the composite edge midpoint rule with the local definition
  switch (num_nodes){
    case 3:{
      area = 0.5 *(vertices(0,1)-vertices(0,0))*((vertices(1,2)-vertices(1,0))-(vertices(1,1)-vertices(1,0))*(vertices(0,2)*vertices(0,0)));
      midpoints << vertices(0,0)+vertices(0,1),
      vertices(0,1)+vertices(0,1),
      vertices(0,0)+vertices(0,2),
      vertices(1,0)+vertices(1,1),
      vertices(1,1)+vertices(1,2),
      vertices(1,0)+vertices(1,2);

    }
  } case 4:{
    area = (vertices(0,1)-vertices(0,0))*(vertices(1,3)-vertices(1,0));
    midpoints << vertices(0,0)+vertices(0,1),
    vertices(0,1)+vertices(0,2),
    vertices(0,2)+vertices(0,3),
    vertices(0,3)+vertices(0,0),
    vertices(1,0)+vertices(1,1),
    vertices(1,1)+vertices(1,2),
    vertices(1,2)+vertices(1,3),
    vertices(1,3)+vertices(1,0),
  }

  Eigen::VectorXd fal(num_nodes);
  for (int i =0; i < num_nodes; i++){
    fal[i] = f(midpoints.col[i]);
  }

  for (int k=0; k<num_nodes; k++){
    elem_vec[k] = 0.5*fal[k];
    elem_vec[(k+1)% num_nodes] = 0.5*fal[k];
  }

  elem_vec *= area/num_nodes;

  //====================

  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/vcm_estimate_edges.h>
#include <CGAL/property_map.h>
#include <CGAL/IO/read_off_points.h>

#include <utility> // defines std::pair
#include <vector>
#include <fstream>

#include <boost/foreach.hpp>

// Types
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3 Point;
typedef Kernel::Vector_3 Vector;

// Point with normal vector stored in a std::pair.
typedef std::pair<Point, Vector> PointVectorPair;
typedef std::vector<PointVectorPair> PointList;

typedef CGAL::cpp11::array<double,6> Covariance;

int main (int , char**) {
    // Reads a .xyz point set file in points[].
    std::list<PointVectorPair> points;
    std::ifstream stream("data/fandisk.off");
    if (!stream ||
        !CGAL::read_off_points(stream,
                               std::back_inserter(points),
                               CGAL::First_of_pair_property_map<PointVectorPair>()))
    {
        std::cerr << "Error: cannot read file data/fandisk.off" << std::endl;
        return EXIT_FAILURE;
    }

    // Estimates covariance matrices per points.
    double R = 0.2,
           r = 0.1;
    std::vector<Covariance> cov;
    CGAL::First_of_pair_property_map<PointVectorPair> point_pmap;

    CGAL::compute_vcm(points.begin(), points.end(), point_pmap, cov, R, r, Kernel());

    // Find the points on the edges.
    // Note that this step is not expensive and can be done several time to get better results
    double threshold = 0.16;
    std::ofstream output("points_on_edges.xyz");
    int i = 0;
    BOOST_FOREACH(const PointVectorPair& p, points)
    {
      if (CGAL::vcm_is_on_feature_edge(cov[i], threshold))
          output << p.first << "\n";
      ++i;
    }

    return 0;
}