/********************************************************************************************/
/*                                                                                */
/*                                HSO3.hpp header file                                      */
/*                                                                                */
/* This file is not currently part of the Boost library. It is simply an example of the use */
/* quaternions can be put to. Hopefully it will be useful too.                              */
/*                                                                                */
/* This file provides tools to convert between quaternions and R^3 rotation matrices.       */
/*                                                                                */
/********************************************************************************************/

//  (C) Copyright Hubert Holin 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#ifndef TEST_HSO3_HPP
#define TEST_HSO3_HPP

#include <algorithm>

#if    defined(__GNUC__) && (__GNUC__ < 3)
#include <boost/limits.hpp>
#else
#include <limits>
#endif

#include <stdexcept>
#include <string>

#include <boost/math/quaternion.hpp>

#if    defined(__GNUC__) && (__GNUC__ < 3)
// gcc 2.x ignores function scope using declarations, put them here instead:
using    namespace ::std;
using    namespace ::boost::math;
#endif

template<typename TYPE_FLOAT>
struct  R3_matrix
{
    TYPE_FLOAT a11, a12, a13;
    TYPE_FLOAT a21, a22, a23;
    TYPE_FLOAT a31, a32, a33;
};

// Note:    the input quaternion need not be of norm 1 for the following function

template<typename TYPE_FLOAT>
R3_matrix<TYPE_FLOAT>    quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
{
    using    ::std::numeric_limits;

    TYPE_FLOAT    a = q.R_component_1();
    TYPE_FLOAT    b = q.R_component_2();
    TYPE_FLOAT    c = q.R_component_3();
    TYPE_FLOAT    d = q.R_component_4();

    TYPE_FLOAT    aa = a*a;
    TYPE_FLOAT    ab = a*b;
    TYPE_FLOAT    ac = a*c;
    TYPE_FLOAT    ad = a*d;
    TYPE_FLOAT    bb = b*b;
    TYPE_FLOAT    bc = b*c;
    TYPE_FLOAT    bd = b*d;
    TYPE_FLOAT    cc = c*c;
    TYPE_FLOAT    cd = c*d;
    TYPE_FLOAT    dd = d*d;

    TYPE_FLOAT    norme_carre = aa+bb+cc+dd;

    if    (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())
    {
        ::std::string            error_reporting("Argument to quaternion_to_R3_rotation is too small!");
        ::std::underflow_error   bad_argument(error_reporting);

        throw(bad_argument);
    }

    R3_matrix<TYPE_FLOAT>    out_matrix;

    out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;
    out_matrix.a12 = 2*(-ad+bc)/norme_carre;
    out_matrix.a13 = 2*(ac+bd)/norme_carre;
    out_matrix.a21 = 2*(ad+bc)/norme_carre;
    out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;
    out_matrix.a23 = 2*(-ab+cd)/norme_carre;
    out_matrix.a31 = 2*(-ac+bd)/norme_carre;
    out_matrix.a32 = 2*(ab+cd)/norme_carre;
    out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;

    return(out_matrix);
}

    template<typename TYPE_FLOAT>
    void    find_invariant_vector(  R3_matrix<TYPE_FLOAT> const & rot,
                                    TYPE_FLOAT & x,
                                    TYPE_FLOAT & y,
                                    TYPE_FLOAT & z)
    {
        using    ::std::sqrt;

        using    ::std::numeric_limits;

        TYPE_FLOAT    b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b12 = rot.a12;
        TYPE_FLOAT    b13 = rot.a13;
        TYPE_FLOAT    b21 = rot.a21;
        TYPE_FLOAT    b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b23 = rot.a23;
        TYPE_FLOAT    b31 = rot.a31;
        TYPE_FLOAT    b32 = rot.a32;
        TYPE_FLOAT    b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);

        TYPE_FLOAT    minors[9] =
        {
            b11*b22-b12*b21,
            b11*b23-b13*b21,
            b12*b23-b13*b22,
            b11*b32-b12*b31,
            b11*b33-b13*b31,
            b12*b33-b13*b32,
            b21*b32-b22*b31,
            b21*b33-b23*b31,
            b22*b33-b23*b32
        };

        TYPE_FLOAT *        where = ::std::max_element(minors, minors+9);

        TYPE_FLOAT          det = *where;

        if    (det <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_invariant_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        switch    (where-minors)
        {
            case 0:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b13*b22+b12*b23)/det;
                y = (-b11*b23+b13*b21)/det;

                break;

            case 1:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b12*b23+b13*b22)/det;
                z = (-b11*b22+b12*b21)/det;

                break;

            case 2:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b11*b23+b13*b21)/det;
                z = (-b12*b21+b11*b22)/det;

                break;

            case 3:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b13*b32+b12*b33)/det;
                y = (-b11*b33+b13*b31)/det;

                break;

            case 4:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b12*b33+b13*b32)/det;
                z = (-b11*b32+b12*b31)/det;

                break;

            case 5:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b11*b33+b13*b31)/det;
                z = (-b12*b31+b11*b32)/det;

                break;

            case 6:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b23*b32+b22*b33)/det;
                y = (-b21*b33+b23*b31)/det;

                break;

            case 7:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b22*b33+b23*b32)/det;
                z = (-b21*b32+b22*b31)/det;

                break;

            case 8:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b21*b33+b23*b31)/det;
                z = (-b22*b31+b21*b32)/det;

                break;

            default:

                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);

                throw(processing_error);

                break;
        }

        TYPE_FLOAT    vecnorm = sqrt(x*x+y*y+z*z);

        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Overflow error in find_invariant_vector!");
            ::std::overflow_error    processing_error(error_reporting);

            throw(processing_error);
        }

        x /= vecnorm;
        y /= vecnorm;
        z /= vecnorm;
    }

    template<typename TYPE_FLOAT>
    void    find_orthogonal_vector( TYPE_FLOAT x,
                                    TYPE_FLOAT y,
                                    TYPE_FLOAT z,
                                    TYPE_FLOAT & u,
                                    TYPE_FLOAT & v,
                                    TYPE_FLOAT & w)
    {
        using    ::std::abs;
        using    ::std::sqrt;

        using    ::std::numeric_limits;

        TYPE_FLOAT    vecnormsqr = x*x+y*y+z*z;

        if    (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        TYPE_FLOAT        lambda;

        TYPE_FLOAT        components[3] =
        {
            abs(x),
            abs(y),
            abs(z)
        };

        TYPE_FLOAT *    where = ::std::min_element(components, components+3);

        switch    (where-components)
        {
            case 0:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    v =
                    w = static_cast<TYPE_FLOAT>(0);
                    u = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -x/vecnormsqr;

                    u = static_cast<TYPE_FLOAT>(1) + lambda*x;
                    v = lambda*y;
                    w = lambda*z;
                }

                break;

            case 1:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    w = static_cast<TYPE_FLOAT>(0);
                    v = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -y/vecnormsqr;

                    u = lambda*x;
                    v = static_cast<TYPE_FLOAT>(1) + lambda*y;
                    w = lambda*z;
                }

                break;

            case 2:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    v = static_cast<TYPE_FLOAT>(0);
                    w = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -z/vecnormsqr;

                    u = lambda*x;
                    v = lambda*y;
                    w = static_cast<TYPE_FLOAT>(1) + lambda*z;
                }

                break;

            default:

                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);

                throw(processing_error);

                break;
        }

        TYPE_FLOAT    vecnorm = sqrt(u*u+v*v+w*w);

        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        u /= vecnorm;
        v /= vecnorm;
        w /= vecnorm;
    }

    // Note:    we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis
    //            of R^3. It might not be orthonormal, however, and we do not check if the
    //            two input vectors are colinear or not.

    template<typename TYPE_FLOAT>
    void    find_vector_for_BOD(TYPE_FLOAT x,
                                TYPE_FLOAT y,
                                TYPE_FLOAT z,
                                TYPE_FLOAT u,
                                TYPE_FLOAT v,
                                TYPE_FLOAT w,
                                TYPE_FLOAT & r,
                                TYPE_FLOAT & s,
                                TYPE_FLOAT & t)
    {
        r = +y*w-z*v;
        s = -x*w+z*u;
        t = +x*v-y*u;
    }

template<typename TYPE_FLOAT>
inline bool                                is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
{
    using    ::std::abs;

    using    ::std::numeric_limits;

    return    (
                !(
                    (abs(mat.a11*mat.a11+mat.a21*mat.a21+mat.a31*mat.a31 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a12*mat.a12+mat.a22*mat.a22+mat.a32*mat.a32 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a13*mat.a13+mat.a23*mat.a23+mat.a33*mat.a33 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())
                )
            );
}

template<typename TYPE_FLOAT>
::boost::math::quaternion<TYPE_FLOAT>    R3_rotation_to_quaternion(    R3_matrix<TYPE_FLOAT> const & rot,
                                                                    ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)
{
    using    ::boost::math::abs;

    using    ::std::abs;
    using    ::std::sqrt;

    using    ::std::numeric_limits;

    if    (!is_R3_rotation_matrix(rot))
    {
        ::std::string        error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");
        ::std::range_error   bad_argument(error_reporting);

        throw(bad_argument);
    }

    ::boost::math::quaternion<TYPE_FLOAT>    q;

    if    (
            (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())
        )
    {
        q = ::boost::math::quaternion<TYPE_FLOAT>(1);
    }
    else
    {
        TYPE_FLOAT    cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    cos_theta_sur_2 = sqrt(stuff);
        TYPE_FLOAT    sin_theta_sur_2 = sqrt(1-stuff);

        TYPE_FLOAT    x;
        TYPE_FLOAT    y;
        TYPE_FLOAT    z;

        find_invariant_vector(rot, x, y, z);

        TYPE_FLOAT    u;
        TYPE_FLOAT    v;
        TYPE_FLOAT    w;

        find_orthogonal_vector(x, y, z, u, v, w);

        TYPE_FLOAT    r;
        TYPE_FLOAT    s;
        TYPE_FLOAT    t;

        find_vector_for_BOD(x, y, z, u, v, w, r, s, t);

        TYPE_FLOAT    ru = rot.a11*u+rot.a12*v+rot.a13*w;
        TYPE_FLOAT    rv = rot.a21*u+rot.a22*v+rot.a23*w;
        TYPE_FLOAT    rw = rot.a31*u+rot.a32*v+rot.a33*w;

        TYPE_FLOAT    angle_sign_determinator = r*ru+s*rv+t*rw;

        if        (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())
        {
            q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, +x*sin_theta_sur_2, +y*sin_theta_sur_2, +z*sin_theta_sur_2);
        }
        else if    (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())
        {
            q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, -x*sin_theta_sur_2, -y*sin_theta_sur_2, -z*sin_theta_sur_2);
        }
        else
        {
            TYPE_FLOAT    desambiguator = u*ru+v*rv+w*rw;

            if    (desambiguator >= static_cast<TYPE_FLOAT>(1))
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
            }
            else
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
            }
        }
    }

    if    ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
    {
        return(-q);
    }

    return(q);
}

#endif /* TEST_HSO3_HPP */


/**
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <tclap/CmdLine.h>

#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <memory>
#include <numeric>

#include "Applications/FileIO/AsciiRasterInterface.h"
#include "GeoLib/AABB.h"
#include "GeoLib/Point.h"
#include "GeoLib/Raster.h"

double compute2DGaussBellCurveValues(GeoLib::Point const& point,
                                     GeoLib::AABB const& aabb)
{
    auto const sigma_x = (aabb.getMaxPoint() - aabb.getMinPoint())[0] / 3;
    auto const sigma_y = (aabb.getMaxPoint() - aabb.getMinPoint())[1] / 3;

    auto const mid_point = (aabb.getMaxPoint() + aabb.getMinPoint()) / 2;

    return std::exp(
        -0.5 * std::pow((point[0] - mid_point[0]), 2) / std::pow(sigma_x, 2) -
        0.5 * std::pow((point[1] - mid_point[1]), 2) / std::pow(sigma_y, 2));
}

double computeSinXSinY(GeoLib::Point const& point, GeoLib::AABB const& aabb)
{
    auto const aabb_size = aabb.getMaxPoint() - aabb.getMinPoint();
    auto const offset = aabb.getMinPoint();

    return std::sin((point[0] - offset[0]) / aabb_size[0] *
                    boost::math::double_constants::pi) *
           std::sin((point[1] - offset[1]) / aabb_size[1] *
                    boost::math::double_constants::pi);
}

int main(int argc, char* argv[])
{
    TCLAP::CmdLine cmd("Add values to raster.", ' ', "0.1");

    TCLAP::ValueArg<std::string> out_raster_arg(
        "o",
        "output_raster",
        "the output raster is stored to a file of this name",
        true,
        "",
        "filename for raster output");
    cmd.add(out_raster_arg);

    TCLAP::ValueArg<double> scaling_arg(
        "",
        "scaling_value",
        "value the function sin(x pi) sin(y pi) will be scaled with",
        false,
        1,
        "double value");
    cmd.add(scaling_arg);

    TCLAP::ValueArg<double> offset_arg(
        "",
        "offset_value",
        "constant added to the function 'scaling * sin(x pi) * sin(y pi)'",
        false,
        0,
        "double value");
    cmd.add(offset_arg);

    TCLAP::ValueArg<double> ll_x_arg(
        "",
        "ll_x",
        "x coordinate of lower left point of axis aligned rectangular region",
        false,
        0,
        "double value");
    cmd.add(ll_x_arg);
    TCLAP::ValueArg<double> ll_y_arg(
        "",
        "ll_y",
        "y coordinate of lower left point of axis aligned rectangular region",
        false,
        0,
        "double value");
    cmd.add(ll_y_arg);
    TCLAP::ValueArg<double> ur_x_arg("",
                                     "ur_x",
                                     "x coordinate of the upper right point of "
                                     "axis aligned rectangular region",
                                     false,
                                     0,
                                     "double value");
    cmd.add(ur_x_arg);
    TCLAP::ValueArg<double> ur_y_arg("",
                                     "ur_y",
                                     "y coordinate of the upper right point of "
                                     "axis aligned rectangular region",
                                     false,
                                     0,
                                     "double value");

    cmd.add(ur_y_arg);
    std::vector<std::string> allowed_functions_vector{"sinxsiny", "exp"};
    TCLAP::ValuesConstraint<std::string> allowed_functions(
        allowed_functions_vector);
    TCLAP::ValueArg<std::string> function_arg(
        "f", "function", "Name of the function used to modify the raster", true,
        "", &allowed_functions);
    cmd.add(function_arg);
    TCLAP::ValueArg<std::string> input_arg("i", "input",
                                           "Name of the input raster (*.asc)",
                                           true, "", "input file name");
    cmd.add(input_arg);

    cmd.parse(argc, argv);

    std::array input_points = {
        GeoLib::Point{{ll_x_arg.getValue(), ll_y_arg.getValue(), 0}},
        GeoLib::Point{{ur_x_arg.getValue(), ur_y_arg.getValue(), 0}}};
    GeoLib::AABB const aabb{std::begin(input_points), std::end(input_points)};

    auto const s = scaling_arg.getValue();
    auto const offset = offset_arg.getValue();

    std::unique_ptr<GeoLib::Raster> const raster(
        FileIO::AsciiRasterInterface::getRasterFromASCFile(
            input_arg.getValue()));
    auto const& header = raster->getHeader();
    auto const& origin = header.origin;

    std::function<double(GeoLib::Point const& p, GeoLib::AABB const& aabb)>
        computeFunctionValue = function_arg.getValue() == "sinxsiny"
                                   ? computeSinXSinY
                                   : compute2DGaussBellCurveValues;

    for (std::size_t r = 0; r < header.n_rows; r++)
    {
        for (std::size_t c = 0; c < header.n_cols; c++)
        {
            GeoLib::Point const p{{origin[0] + header.cell_size * c,
                                   origin[1] + header.cell_size * r, 0.0}};
            if (!aabb.containsPoint(p, std::numeric_limits<double>::epsilon()))
            {
                continue;
            }

            (*raster)(r, c) += offset + s * computeFunctionValue(p, aabb);
        }
    }

    FileIO::AsciiRasterInterface::writeRasterAsASC(*raster,
                                                   out_raster_arg.getValue());
    return EXIT_SUCCESS;
}


/**
 * @file solvecauchyproblem.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 19.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "solvecauchyproblem.h"
#include "uniformcubicspline.h"

#include <cmath>

#include <Eigen/Core>

namespace CLEmpiricFlux {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d findSupport(const UniformCubicSpline &f,
                            Eigen::Vector2d initsupp, double t) {
  Eigen::Vector2d result;
#if SOLUTION
  Eigen::Vector2d speed = {f.derivative(-1.0), f.derivative(1.0)};
  result = initsupp + t * speed;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
template <typename FUNCTOR>
Eigen::VectorXd semiDiscreteRhs(const Eigen::VectorXd &mu0, double h,
                                FUNCTOR &&numFlux) {
  int m = mu0.size();
  Eigen::VectorXd mu1(m);
#if SOLUTION
  mu1(0) = -1.0 / h * (numFlux(mu0(0), mu0(1)) - numFlux(mu0(0), mu0(0)));
  for (int j = 1; j < m - 1; ++j) {
    mu1(j) =
        -1.0 / h * (numFlux(mu0(j), mu0(j + 1)) - numFlux(mu0(j - 1), mu0(j)));
  }
  mu1(m - 1) =
      -1.0 / h *
      (numFlux(mu0(m - 1), mu0(m - 1)) - numFlux(mu0(m - 2), mu0(m - 1)));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return mu1;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
template <typename FUNCTOR>
Eigen::VectorXd RalstonODESolver(FUNCTOR &&rhs, Eigen::VectorXd mu0, double tau,
                                 int n) {
#if SOLUTION
  for (int i = 0; i < n; ++i) {
    Eigen::VectorXd k1 = rhs(mu0);
    Eigen::VectorXd k2 = rhs(mu0 + tau * 2.0 / 3.0 * k1);
    mu0 = mu0 + 0.25 * tau * (k1 + 3.0 * k2);
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return mu0;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd solveCauchyProblem(const UniformCubicSpline &f,
                                   const Eigen::VectorXd &mu0, double h,
                                   double T) {
  Eigen::VectorXd muT(mu0.size());
#if SOLUTION
  double tau = std::min(h / std::abs(f.derivative(-1.0)),
                        h / std::abs(f.derivative(1.0)));
  double n = (int)std::floor(T / tau);
  GodunovFlux godunovFlux(f);
  auto rhs = [h, &godunovFlux](const Eigen::VectorXd &mu) {
    return semiDiscreteRhs(mu, h, godunovFlux);
  };
  muT = RalstonODESolver(rhs, mu0, tau, n);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return muT;
}
/* SAM_LISTING_END_4 */

}  // namespace CLEmpiricFlux


///////////////////////////////////////////////////////////////////////////////
// weighted_kurtosis.hpp
//
//  Copyright 2006 Olivier Gygi, Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_KURTOSIS_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_KURTOSIS_HPP_EAN_28_10_2005

#include <limits>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_kurtosis_impl
    /**
        @brief Kurtosis estimation for weighted samples

        The kurtosis of a sample distribution is defined as the ratio of the 4th central moment and the square of the 2nd central
        moment (the variance) of the samples, minus 3. The term \f$ -3 \f$ is added in order to ensure that the normal distribution
        has zero kurtosis. The kurtosis can also be expressed by the simple moments:

        \f[
            \hat{g}_2 =
                \frac
                {\widehat{m}_n^{(4)}-4\widehat{m}_n^{(3)}\hat{\mu}_n+6\widehat{m}_n^{(2)}\hat{\mu}_n^2-3\hat{\mu}_n^4}
                {\left(\widehat{m}_n^{(2)} - \hat{\mu}_n^{2}\right)^2} - 3,
        \f]

        where \f$ \widehat{m}_n^{(i)} \f$ are the \f$ i \f$-th moment and \f$ \hat{\mu}_n \f$ the mean (first moment) of the
        \f$ n \f$ samples.

        The kurtosis estimator for weighted samples is formally identical to the estimator for unweighted samples, except that
        the weighted counterparts of all measures it depends on are to be taken.
    */
    template<typename Sample, typename Weight>
    struct weighted_kurtosis_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, weighted_sample>::result_type result_type;

        weighted_kurtosis_impl(dont_care)
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::average(
                        accumulators::weighted_moment<4>(args)
                        - 4. * accumulators::weighted_moment<3>(args) * weighted_mean(args)
                        + 6. * accumulators::weighted_moment<2>(args) * weighted_mean(args) * weighted_mean(args)
                        - 3. * weighted_mean(args) * weighted_mean(args) * weighted_mean(args) * weighted_mean(args)
                      , ( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
                        * ( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
                   ) - 3.;
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_kurtosis
//
namespace tag
{
    struct weighted_kurtosis
      : depends_on<weighted_mean, weighted_moment<2>, weighted_moment<3>, weighted_moment<4> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_kurtosis_impl<mpl::_1, mpl::_2> impl;
    };
}

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

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_kurtosis)
}

using extract::weighted_kurtosis;

}} // namespace boost::accumulators

#endif


/**
 * @file debuggingfem_main.cc
 * @brief NPDE homework ParametricElementMatrices code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <memory>
#include <vector>

#include "locallaplaceqfe.h"
#include "qfeinterpolator.h"
#include "qfeprovidertester.h"

using size_type = lf::base::size_type;

int main() {
  // read mesh
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/square_64.msh");
  auto mesh = reader.mesh();

  // refine mesh
  const int reflevels = 4;
  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh, reflevels);
  lf::refinement::MeshHierarchy &multi_mesh{*multi_mesh_p};
  lf::base::size_type L = multi_mesh.NumLevels();

  // vector holding pointers to the different element matrix providers
  std::vector<std::unique_ptr<DebuggingFEM::EntityMatrixProvider>>
      element_matrix_provider;
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE1>());
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE2>());
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE3>());
  const int num_emp = element_matrix_provider.size();

  // vectors accumulating the computed errors
  Eigen::MatrixXd H1SMerr(L, num_emp);
  Eigen::VectorXi N(L);

  for (int level = 0; level < L; ++level) {
    // set up fespace and dof handler for the mesh at the current level
    auto mesh_p = multi_mesh.getMesh(level);
    lf::uscalfe::FeSpaceLagrangeO2<double> fespace(mesh_p);
    const auto &dofh = fespace.LocGlobMap();

    // Dimension of finite element space
    N[level] = dofh.NumDofs();

    // compute error for each element matrix provider
    for (int i = 0; i < num_emp; ++i) {
      // function to interpolateOntoQuadFE
      auto f = [](const Eigen::VectorXd &x) -> double {
        return std::exp(x(1) * x(1) + x(0) * x(0));
      };
      double energy = 0.0;
      // Matrix in triplet format holding Galerkin for LocalLaplaceQFEX matrix,
      // zero initially.
      DebuggingFEM::QFEProviderTester qfe_provider_tester(
          dofh, *element_matrix_provider[i]);
      // compute the energy and error
      energy = qfe_provider_tester.energyOfInterpolant(f);
      H1SMerr(level, i) = std::abs(23.76088 - energy);
    }
  }

  // Tabular output of the results
  std::cout << std::left << std::setw(10) << "N" << std::setw(20)
            << "Assember 1" << std::setw(20) << "Assembler 2" << std::setw(20)
            << "Assembler 3" << std::endl;
  for (int level = 0; level < L; ++level) {
    std::cout << std::left << std::setw(10) << N[level] << std::setw(20)
              << H1SMerr(level, 0) << std::setw(20) << H1SMerr(level, 1)
              << std::setw(20) << H1SMerr(level, 2) << std::endl;
  }

  // Write .csv file and plot it by a python script
  Eigen::MatrixXd data(L, num_emp + 1);
  data.col(0) = N.cast<double>();
  data.rightCols(num_emp) = H1SMerr;
  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream error_file;
  error_file.open("error.csv");
  error_file << data.format(CSVFormat) << std::endl;
  error_file.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/error.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/error.csv " CURRENT_BINARY_DIR "/error.eps");

  return 0;
}


#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/simple_hierarchical_clustering.hpp>
#include <common_robotics_utilities/simple_kmeans_clustering.hpp>

int main(int argc, char** argv)
{
  const size_t num_points
      = (argc >= 2) ? static_cast<size_t>(atoi(argv[1])) : 1000;
  std::cout << "Generating " << num_points << " points..." << std::endl;
  const int64_t seed = 42;
  std::mt19937_64 prng(seed);
  std::uniform_real_distribution<double> dist(0.0, 10.0);
  std::vector<Eigen::VectorXd> random_points(num_points);
  std::vector<size_t> indices(num_points);
  for (size_t idx = 0; idx < num_points; idx++)
  {
    const double x = dist(prng);
    const double y = dist(prng);
    Eigen::VectorXd random_point(3);
    random_point << x, y, 0.0;
    random_points[idx] = random_point;
    indices[idx] = idx;
  }
  const std::vector<bool> use_parallel_options = {false, true};
  for (const bool use_parallel : use_parallel_options)
  {
    std::cout << "Complete-link hierarchical clustering " << num_points
              << " points..." << std::endl;
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)>
        distance_fn = [] (const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
    {
      return common_robotics_utilities::math::Distance(v1, v2);
    };
    const Eigen::MatrixXd distance_matrix
        = common_robotics_utilities::math
            ::BuildPairwiseDistanceMatrixParallel(random_points, distance_fn);
    const std::vector<std::vector<size_t>> hierarchical_index_clusters
        = common_robotics_utilities::simple_hierarchical_clustering
            ::ClusterWithDistanceMatrix<int64_t>(
                indices, distance_matrix, 1.0,
                common_robotics_utilities::simple_hierarchical_clustering
                    ::ClusterStrategy::COMPLETE_LINK, use_parallel).first;
    std::vector<Eigen::VectorXd> hierarchical_clustered_points = random_points;
    for (size_t cluster_idx = 0;
         cluster_idx < hierarchical_index_clusters.size(); cluster_idx++)
    {
      const std::vector<size_t>& current_cluster
          = hierarchical_index_clusters[cluster_idx];
      const double cluster_num = 1.0 + (double)cluster_idx;
      for (size_t element_idx = 0; element_idx < current_cluster.size();
           element_idx++)
      {
        const size_t index = current_cluster[element_idx];
        hierarchical_clustered_points.at(index)(2) = cluster_num;
      }
    }
    std::cout << "K-means clustering " << num_points << " points..."
              << std::endl;
    std::function<Eigen::VectorXd(const std::vector<Eigen::VectorXd>&)>
        average_fn = [] (const std::vector<Eigen::VectorXd>& cluster)
    {
      return common_robotics_utilities::math::AverageEigenVectorXd(cluster);
    };
    const std::vector<int32_t> kmeans_labels
        = common_robotics_utilities::simple_kmeans_clustering::Cluster(
            random_points, distance_fn, average_fn, 50, 42, true, use_parallel);
    std::vector<Eigen::VectorXd> kmeans_clustered_points = random_points;
    for (size_t idx = 0; idx < kmeans_clustered_points.size(); idx++)
    {
      kmeans_clustered_points.at(idx)(2) = kmeans_labels.at(idx);
    }
    std::cout << "Saving to CSV..." << std::endl;
    const std::string hierarchical_log_file_name
        = (use_parallel) ? "/tmp/test_parallel_hierarchical_clustering.csv"
                         : "/tmp/test_hierarchical_clustering.csv";
    std::ofstream hierarchical_log_file(
        hierarchical_log_file_name, std::ios_base::out);
    if (!hierarchical_log_file.is_open())
    {
      std::cerr << "\x1b[31;1m Unable to create folder/file to log to: "
                << hierarchical_log_file_name << "\x1b[0m \n";
      throw std::invalid_argument("Log filename must be write-openable");
    }
    for (size_t idx = 0; idx < num_points; idx++)
    {
      const Eigen::VectorXd& point = hierarchical_clustered_points.at(idx);
      hierarchical_log_file << point(0) << "," << point(1) << "," << point(2)
                            << std::endl;
    }
    hierarchical_log_file.close();
    const std::string kmeans_log_file_name
        = (use_parallel) ? "/tmp/test_parallel_kmeans_clustering.csv"
                         : "/tmp/test_kmeans_clustering.csv";
    std::ofstream kmeans_log_file(kmeans_log_file_name, std::ios_base::out);
    if (!kmeans_log_file.is_open())
    {
      std::cerr << "\x1b[31;1m Unable to create folder/file to log to: "
                << kmeans_log_file_name << "\x1b[0m \n";
      throw std::invalid_argument("Log filename must be write-openable");
    }
    for (size_t idx = 0; idx < num_points; idx++)
    {
      const Eigen::VectorXd& point = kmeans_clustered_points.at(idx);
      kmeans_log_file << point(0) << "," << point(1) << "," << point(2)
                      << std::endl;
    }
    kmeans_log_file.close();
  }
  std::cout << "Done saving, you can plot as a 3d scatterplot to see clustering"
            << std::endl;
  return 0;
}


#include "triangle.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <complex>
namespace
{
    ComplexType validate(PASTNode astnode, ParsersHelper& ph, const std::string& fnname)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error(fnname+" can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
          throw std::runtime_error("The argument of "+fnname+" must be complex");
        return boost::get<ComplexType>(secondCh->token.info).toinexact();
    }
}

namespace HT
{
    void sin(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "sin");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::sin(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void cos(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "cos");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::cos(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void tan(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "tan");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::tan(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void asin(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "asin");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::asin(a);
        if (a.imag() == 0.0 && a.real()>1.0) ans = std::complex<long double>(ans.real(), -ans.imag());
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void acos(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "acos");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        LOG("arg of acos:"<<a)
        auto ans = std::acos(a);
        LOG("acos="<<ans)
        if (a.imag() == 0.0 && a.real()>1.0) ans = std::complex<long double>(ans.real(), -ans.imag());
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void atan(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "atan");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::atan(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }

}


/*******************************************************************************
*
*  Filename    : parameter_arithmatics.cc
*  Description : Testing the arithmatics of paramters
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include <iostream>
#include <vector>

#include <boost/format.hpp>
using namespace std;
using namespace mgr;

void
AddTest( const vector<Parameter>& list )
{
    for( size_t i = 0; i < list.size(); ++i ){
        cout << list[ i ].CentralValue();

        if( i != list.size() - 1 ){
            cout << "+";
        }
        else{
            cout << " & ";
        }
    }

    const Parameter sum = SumUncorrelated( list );
    cout << sum.CentralValue() << "&" << sum.AbsUpperError() << "&" << sum.AbsLowerError() << endl;
}

void
ProdTest( const vector<double> chain )
{
    Parameter original  = Poisson::Minos( chain.front() );
    Parameter finalpass = Poisson::Minos( chain.back() );
    vector<Parameter> prodlist;
    prodlist.push_back( original );
    cout << chain.front() << flush;

    for( size_t i = 0; i < chain.size() - 1; ++i ){
        cout << boost::format( "\\times\\frac{%lg}{%lg}" ) % chain[ i + 1 ] % chain[ i ] << flush;
        prodlist.push_back( Efficiency::Minos( chain[ i + 1 ], chain[ i ] ) );
    }

    cout << "&" << flush;
    const Parameter product = ProdUncorrelated( prodlist );
    cout << product.CentralValue() << "&" << product.AbsUpperError() << "&" << product.AbsLowerError() << endl;
}

int
main( int argc, char const* argv[] )
{
    cout << ">>>> Addition testing: Sum to 100" << endl;
    {
        Parameter a = Poisson::Minos( 10 );
        Parameter b = Poisson::Minos( 20 );
        Parameter c = Poisson::Minos( 30 );
        Parameter d = Poisson::Minos( 40 );
        Parameter e = Poisson::Minos( 50 );
        Parameter f = Poisson::Minos( 60 );
        Parameter g = Poisson::Minos( 70 );
        Parameter h = Poisson::Minos( 80 );
        Parameter i = Poisson::Minos( 90 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 100 ), 4 ) << endl;
        AddTest( { e, e } );
        AddTest( { d, f } );
        AddTest( { c, g } );
        AddTest( { b, h } );
        AddTest( { a, i } );
        AddTest( { a, c, c, c } );
        AddTest( { b, b, b, b, b } );
        AddTest( { a, a, a, a, a, a, a, a, a, a } );
        cout << endl;
    }
    cout << ">>>> Addition testing: Sum to 1000" << endl;
    {
        Parameter a = Poisson::Minos( 100 );
        Parameter b = Poisson::Minos( 200 );
        Parameter c = Poisson::Minos( 300 );
        Parameter d = Poisson::Minos( 400 );
        Parameter e = Poisson::Minos( 500 );
        Parameter f = Poisson::Minos( 600 );
        Parameter g = Poisson::Minos( 700 );
        Parameter h = Poisson::Minos( 800 );
        Parameter i = Poisson::Minos( 900 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 1000 ), 4 ) << endl;
        AddTest( { e, e } );
        AddTest( { d, f } );
        AddTest( { c, g } );
        AddTest( { b, h } );
        AddTest( { a, i } );
        AddTest( { a, c, c, c } );
        AddTest( { b, b, b, b, b } );
        AddTest( { a, a, a, a, a, a, a, a, a, a } );
        cout << endl;
    }
    cout << ">>>> Addition testing: Partial addition to 100" << endl;
    {
        Parameter a = Poisson::Minos( 20 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 100 ), 4 ) << endl;
        cout << "(20+20+20+20+20) & " << FloatingPoint( Sum( a, a, a, a, a ), 4 ) << endl;
        cout << "(20+20)+(20+20+20) & " << FloatingPoint( Sum( Sum( a, a ), Sum( a, a, a ) ), 4 ) << endl;
        cout << "(20+20)+(20+20)+20 & " << FloatingPoint( Sum( Sum( a, a ), Sum( a, a ), a ), 4 ) << endl;
        cout << "(((20+20)+20)+20)+20 )& " << FloatingPoint( Sum( Sum( Sum( Sum( a, a ), a ), a ), a ), 4 ) << endl;
        cout << endl;
    }
    cout << ">>>> Product testing: Product to 50" << endl;
    {
        cout << "Direct value : " << FloatingPoint( Poisson::Minos( 50 ), 4 ) << endl;
        ProdTest( { 100, 50 } );
        ProdTest( { 100, 75, 50 } );
        ProdTest( { 100, 75, 60, 50 } );
        ProdTest( { 200, 100, 75, 60, 50 } );
        cout << endl;
    }
    cout << ">>> Product test: Partial product " << endl;
    {
        Parameter a  = Poisson::Minos( 200 );
        Parameter e1 = Efficiency::Minos( 100, 200 );
        Parameter e2 = Efficiency::Minos( 75, 100 );
        Parameter e3 = Efficiency::Minos( 60, 75 );
        Parameter e4 = Efficiency::Minos( 50, 60 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 50 ), 4 ) << endl;
        cout << "(a*b*c*d*e)" << FloatingPoint( Prod( a, e1, e2, e3, e4 ), 4 ) << endl;
        cout << "(a*b)*(c*d*e)" << FloatingPoint( Prod( Prod( a, e1 ), Prod( e2, e3, e4 ) ), 4 ) << endl;
        cout << "(a*b)*(c*d)*e" << FloatingPoint( Prod( Prod( a, e1 ), Prod( e2, e3 ), e4 ), 4 ) << endl;
        cout << "((((a*b)*c)*d)*e)" << FloatingPoint( Prod( Prod( Prod( Prod( a, e1 ), e2 ), e3 ), e4 ), 4 ) << endl;
    }
    cout << "\n>>> Another product test: Scale factor tests" << endl;
    {
        Parameter s( 1, 0, 0 );
        Parameter a( 0.9867, 0.0060, 0.0060 );
        Parameter b( 0.9791, 0.0132, 0.0132 );
        Parameter c( 0.9384, 0.0022, 0.0023 );
        cout << FloatingPoint( a, 3 ) << endl;
        cout << FloatingPoint( b, 3 ) << endl;
        cout << FloatingPoint( c, 3 ) << endl;
        cout << FloatingPoint( Prod( a, b, c ), 3 ) << endl;
        cout << FloatingPoint( s *= a, 3 ) << endl;
        cout << FloatingPoint( s *= b, 3 ) << endl;
        cout << FloatingPoint( s *= c, 3 ) << endl;
    }
    cout << "\n>> Specical case testing " << endl;
    {
        Parameter a( 11394.14521, +3455.26185, +2904.08486 );
        Parameter b( 1.65585, +1.88848, +0.83634 );
        cout << FloatingPoint( a, 5 ) << endl;
        cout << FloatingPoint( b, 5 ) << endl;
        a += b;
        cout << FloatingPoint( a, 5 ) << endl;
    }
    cout << "\n>>> Scale case testing" << endl;
    {
        Parameter a( 0, 0, 0 );
        Parameter b = Poisson::Minos( 10 );
        cout << FloatingPoint( b, 5 ) << endl;
        cout << FloatingPoint( a + b, 5 ) << endl;
        cout << FloatingPoint( b + a, 5 ) << endl;
    }
    cout << "\n>>> Scale product testing" << endl;
    {
        Parameter a( 1, 0.001, 0.2 );
        Parameter b( 100, 0.1, 0.2 );
        Parameter c( 1000, 100, 200 );
        cout << FloatingPoint( Prod( a, b ), 3 ) << endl;
        cout << FloatingPoint( Prod( b, c ), 3 ) << endl;
        // cout << FloatingPoint( Prod(c,b), 3 ) << endl;
        // cout << FloatingPoint( Prod(a,b,c), 3 ) << endl;
    }
    cout << "\n>>> Product test " << endl;
    {
        Parameter results = Prod(
            Parameter( 1, 0.0107266, 0.0107266 ),
            Parameter( 1, 0.0329748, 0.0279871 ),
            Parameter( 1,         0,  0.170623 ),
            Parameter( 1,  0.133084,   0.13232 ),
            Parameter( 1, 0.0243757, 0.0240791 ),
            Parameter( 1, 0.0257803, 0.0254051 ),
            Parameter( 1, 0.0249153, 0.0249153 ),
            Parameter( 1,  0.163553,  0.163553 ),
            Parameter( 1, 0.0547323, 0.0542217 ),
            Parameter( 1,     0.046,     0.046 ),
            Parameter( 1,      0.03,      0.03 )
            );
        cout << FloatingPoint( results, 3 ) << endl;
    }
    cout << "\n>>> Product test" << endl;
    {
        Parameter a( 1, 1.46, 0.787 );
        Parameter b( 1, 0.046, 0.046 );
        Parameter c( 1, 0.03, 0.03 );
        cout << FloatingPoint( Prod( a, b, c ), 5 ) << endl;
        cout << FloatingPoint( Prod( a, b ), 5 ) << endl;
        cout << FloatingPoint( Prod( b, c ), 5 ) << endl;
        cout << FloatingPoint( Prod( a, c ), 5 ) << endl;
    }
    return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp

 [begin_description]
 Definition of the coefficients for the Adams-Bashforth method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< class Value , size_t Steps >
class adams_bashforth_coefficients ;

template< class Value >
class adams_bashforth_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 1 >()
      {
        (*this)[0] = static_cast< Value >( 1 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 2 >()
      {
        (*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 2 );
        (*this)[1] = -static_cast< Value >( 1 ) / static_cast< Value >( 2 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 3 >()
      {
        (*this)[0] = static_cast< Value >( 23 ) / static_cast< Value >( 12 );
        (*this)[1] = -static_cast< Value >( 4 ) / static_cast< Value >( 3 );
        (*this)[2] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 4 >()
      {
        (*this)[0] = static_cast< Value >( 55 ) / static_cast< Value >( 24 );
        (*this)[1] = -static_cast< Value >( 59 ) / static_cast< Value >( 24 );
        (*this)[2] = static_cast< Value >( 37 ) / static_cast< Value >( 24 );
        (*this)[3] = -static_cast< Value >( 3 ) / static_cast< Value >( 8 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 5 >()
      {
        (*this)[0] = static_cast< Value >( 1901 ) / static_cast< Value >( 720 );
        (*this)[1] = -static_cast< Value >( 1387 ) / static_cast< Value >( 360 );
        (*this)[2] = static_cast< Value >( 109 ) / static_cast< Value >( 30 );
        (*this)[3] = -static_cast< Value >( 637 ) / static_cast< Value >( 360 );
        (*this)[4] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 6 >()
      {
        (*this)[0] = static_cast< Value >( 4277 ) / static_cast< Value >( 1440 );
        (*this)[1] = -static_cast< Value >( 2641 ) / static_cast< Value >( 480 );
        (*this)[2] = static_cast< Value >( 4991 ) / static_cast< Value >( 720 );
        (*this)[3] = -static_cast< Value >( 3649 ) / static_cast< Value >( 720 );
        (*this)[4] = static_cast< Value >( 959 ) / static_cast< Value >( 480 );
        (*this)[5] = -static_cast< Value >( 95 ) / static_cast< Value >( 288 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 7 >()
      {
        (*this)[0] = static_cast< Value >( 198721 ) / static_cast< Value >( 60480 );
        (*this)[1] = -static_cast< Value >( 18637 ) / static_cast< Value >( 2520 );
        (*this)[2] = static_cast< Value >( 235183 ) / static_cast< Value >( 20160 );
        (*this)[3] = -static_cast< Value >( 10754 ) / static_cast< Value >( 945 );
        (*this)[4] = static_cast< Value >( 135713 ) / static_cast< Value >( 20160 );
        (*this)[5] = -static_cast< Value >( 5603 ) / static_cast< Value >( 2520 );
        (*this)[6] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 8 >()
      {
        (*this)[0] = static_cast< Value >( 16083 ) / static_cast< Value >( 4480 );
        (*this)[1] = -static_cast< Value >( 1152169 ) / static_cast< Value >( 120960 );
        (*this)[2] = static_cast< Value >( 242653 ) / static_cast< Value >( 13440 );
        (*this)[3] = -static_cast< Value >( 296053 ) / static_cast< Value >( 13440 );
        (*this)[4] = static_cast< Value >( 2102243 ) / static_cast< Value >( 120960 );
        (*this)[5] = -static_cast< Value >( 115747 ) / static_cast< Value >( 13440 );
        (*this)[6] = static_cast< Value >( 32863 ) / static_cast< Value >( 13440 );
        (*this)[7] = -static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
      }
};

} // detail
} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019, The University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
//
// This file was originally part of Exotica, cf.
// https://github.com/ipab-slmc/exotica/blob/master/exotica_core/include/exotica_core/tools/box_qp.h
//
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <vector>

namespace crocoddyl {
struct BoxQPSolution {
  BoxQPSolution(const Eigen::MatrixXd& Hff_inv_in, const Eigen::VectorXd& x_in, const std::vector<size_t>& free_idx_in,
                const std::vector<size_t>& clamped_idx_in)
      : Hff_inv(Hff_inv_in), x(x_in), free_idx(free_idx_in), clamped_idx(clamped_idx_in) {}

  Eigen::MatrixXd Hff_inv;
  Eigen::MatrixXd x;
  std::vector<size_t> free_idx;
  std::vector<size_t> clamped_idx;
};

// Based on Yuval Tassa's BoxQP
// Cf. https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization
inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& b_low,
                           const Eigen::VectorXd& b_high, const Eigen::VectorXd& x_init, const double gamma,
                           const int max_iterations, const double epsilon, const double lambda) {
  if (max_iterations < 0) {
    throw std::runtime_error("Max iterations needs to be positive.");
  }

  int it = 0;
  Eigen::VectorXd delta_xf(x_init.size()), x = x_init;
  std::vector<size_t> clamped_idx, free_idx;
  Eigen::VectorXd grad = q + H * x_init;
  Eigen::MatrixXd Hff, Hfc, Hff_inv(H.rows(), H.cols());
  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt = Eigen::LLT<Eigen::MatrixXd>(H.rows());

  Hff_inv_llt.compute(Eigen::MatrixXd::Identity(H.rows(), H.cols()) * lambda + H);
  Hff_inv_llt.solveInPlace(Hff_inv);

  if (grad.lpNorm<Eigen::Infinity>() <= epsilon) {
    return BoxQPSolution(Hff_inv, x_init, free_idx, clamped_idx);
  }

  while (grad.lpNorm<Eigen::Infinity>() > epsilon && it < max_iterations) {
    ++it;
    grad.noalias() = q + H * x;
    clamped_idx.clear();
    free_idx.clear();

    for (int i = 0; i < grad.size(); ++i) {
      if ((x(i) == b_low(i) && grad(i) > 0) || (x(i) == b_high(i) && grad(i) < 0)) {
        clamped_idx.push_back(i);
      } else {
        free_idx.push_back(i);
      }
    }

    if (free_idx.size() == 0) {
      return BoxQPSolution(Hff_inv, x, free_idx, clamped_idx);
    }

    Hff.resize(free_idx.size(), free_idx.size());
    Hfc.resize(free_idx.size(), clamped_idx.size());

    if (clamped_idx.size() == 0) {
      Hff = H;
    } else {
      for (size_t i = 0; i < free_idx.size(); ++i) {
        for (size_t j = 0; j < free_idx.size(); ++j) {
          Hff(i, j) = H(free_idx[i], free_idx[j]);
        }
      }

      for (size_t i = 0; i < free_idx.size(); ++i) {
        for (size_t j = 0; j < clamped_idx.size(); ++j) {
          Hfc(i, j) = H(free_idx[i], clamped_idx[j]);
        }
      }
    }

    // NOTE: Array indexing not supported in current eigen version
    Eigen::VectorXd q_free(free_idx.size()), x_free(free_idx.size()), x_clamped(clamped_idx.size());
    for (size_t i = 0; i < free_idx.size(); ++i) {
      q_free(i) = q(free_idx[i]);
      x_free(i) = x(free_idx[i]);
    }

    for (size_t j = 0; j < clamped_idx.size(); ++j) {
      x_clamped(j) = x(clamped_idx[j]);
    }

    // The dimension of Hff has changed - reinitialise LLT
    // Hff_inv_llt = Eigen::LLT<Eigen::MatrixXd>(Hff.rows());
    // Hff_inv_llt.compute(Eigen::MatrixXd::Identity(Hff.rows(), Hff.cols()) * lambda + Hff);
    // Hff_inv_llt.solveInPlace(Hff_inv);
    // TODO: Use Cholesky, however, often unstable without adapting lambda.
    Hff_inv = (Eigen::MatrixXd::Identity(Hff.rows(), Hff.cols()) * lambda + Hff).inverse();

    if (clamped_idx.size() == 0) {
      // std::cout << "Hff_inv=" << Hff_inv.rows() << "x" << Hff_inv.cols() << ", q_free=" << q_free.size() << ",
      // x_free=" << x_free.size() << std::endl;
      delta_xf.noalias() = -Hff_inv * (q_free)-x_free;
    } else {
      delta_xf.noalias() = -Hff_inv * (q_free + Hfc * x_clamped) - x_free;
    }

    double f_old = (0.5 * x.transpose() * H * x + q.transpose() * x)(0);
    static const Eigen::VectorXd alpha_space = Eigen::VectorXd::LinSpaced(10, 1.0, 0.1);

    bool armijo_reached = false;
    Eigen::VectorXd x_new(x.size()), x_diff(x.size());
    for (int ai = 0; ai < alpha_space.rows(); ++ai) {
      x_new = x;
      for (size_t i = 0; i < free_idx.size(); ++i) {
        x_new(free_idx[i]) = std::max(std::min(x(free_idx[i]) + alpha_space[ai] * delta_xf(i), b_high(i)), b_low(i));
      }

      double f_new = (0.5 * x_new.transpose() * H * x_new + q.transpose() * x_new)(0);
      x_diff.noalias() = x - x_new;

      // armijo criterion>
      double armijo_coef = (f_old - f_new) / (grad.transpose() * x_diff + 1e-5);
      if (armijo_coef > gamma) {
        armijo_reached = true;
        x = x_new;
        break;
      }
    }

    // break if no step made
    if (!armijo_reached) break;
  }

  return BoxQPSolution(Hff_inv, x, free_idx, clamped_idx);
}

inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& b_low,
                           const Eigen::VectorXd& b_high, const Eigen::VectorXd& x_init) {
  const double epsilon = 1e-5;
  const double gamma = 0.1;
  const int max_iterations = 100;
  const double lambda = 1e-5;
  return BoxQP(H, q, b_low, b_high, x_init, gamma, max_iterations, epsilon, lambda);
}
}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbLinearInterpolation.h"

#include <boost/make_shared.hpp>

namespace rttb
{
	namespace interpolation
	{

		DoseTypeGy LinearInterpolation::trilinear(std::array<double, 3> target,
		        boost::shared_ptr<DoseTypeGy[]> values) const
		{
			//4 linear interpolation in x direction
			DoseTypeGy c_00 = values[0] * (1.0 - target[0]) + values[1] * target[0];
			DoseTypeGy c_10 = values[2] * (1.0 - target[0]) + values[3] * target[0];
			DoseTypeGy c_01 = values[4] * (1.0 - target[0]) + values[5] * target[0];
			DoseTypeGy c_11 = values[6] * (1.0 - target[0]) + values[7] * target[0];

			//combine result in y direction
			DoseTypeGy c_0 = c_00 * (1.0 - target[1]) + c_10 * target[1];
			DoseTypeGy c_1 = c_01 * (1.0 - target[1]) + c_11 * target[1];

			//finally incorporate z direction
			return (c_0 * (1.0 - target[2]) + c_1 * target[2]);
		}

		DoseTypeGy LinearInterpolation::getValue(const WorldCoordinate3D& aWorldCoordinate) const
		{
			//proper initialization of target and values
			std::array<double, 3> target = {0.0, 0.0, 0.0};
      auto values = boost::make_shared<DoseTypeGy[]>(8);
			getNeighborhoodVoxelValues(aWorldCoordinate, 8, target, values);

			return trilinear(target, values);
		}

	}
}


#include <mex.h>
#include <math.h>
#include <iostream>
#include <vector>

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

#include <igl/PI.h>
#include <igl/forward_kinematics.h>
#include <igl/directed_edge_parents.h>

#include <Eigen/Core>

using namespace Eigen;
using namespace std;

void euler_to_quat(const Eigen::Vector3d& euler, Eigen::Quaterniond& q) {

  q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
}

void euler_to_quat(const Eigen::Vector3d& euler, const Eigen::Affine3d& a, Eigen::Quaterniond& q) {
  q = Eigen::AngleAxisd(euler(2), a.rotation().col(2))
      * Eigen::AngleAxisd(euler(1), a.rotation().col(1))
      * Eigen::AngleAxisd(euler(0), a.rotation().col(0));
  // q = Eigen::AngleAxisd(euler(0), a.rotation().col(0))
  //     * Eigen::AngleAxisd(euler(1), a.rotation().col(1))
  //     * Eigen::AngleAxisd(euler(2), a.rotation().col(2));
}

void read_pnt_anim(std::string anim_file,
    const Eigen::MatrixXd& C,
    const Eigen::VectorXd& center,
    const double& scale,
    std::vector<Eigen::MatrixXd>& T_list)
{
  FILE* file;
  file= fopen(anim_file.c_str(), "rb");

  int dim = 3;//= C.cols();

  double degree2radian = igl::PI/180.0;

  int num_bone, num_frame;
  double val = 0;

  typedef std::vector<Eigen::Quaterniond,
      Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

  fscanf(file, "%d %d\n", &num_bone, &num_frame);
  T_list.resize(num_frame);
  //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

  std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
  std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

  for(int i=0; i<num_bone ; i++){
    Eigen::Vector3d rest_tran;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      rest_tran(j) = val;
    }
    rest_tran_list[i] = (rest_tran-center)/scale;

    Eigen::Vector3d euler;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      euler(j) = val;
    }
    euler = euler.array() * degree2radian;
    Eigen::Quaterniond q;
    euler_to_quat(euler, q);
    Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
    root_affine.rotate(q);
    rest_affine_list[i] = root_affine;
    //std::cout<<"i: "<<i<<", mat: "<<root_affine.matrix()<<std::endl;
  }
  RotationList rot_list(num_bone);
  std::vector<Eigen::Vector3d> tran_list(num_bone);

  for (int k = 0; k < num_frame; k++) {
    for (int i = 0; i < num_bone; i++) {
      Eigen::Vector3d tran;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        tran(j) = val;
      }
      tran = (tran - center)/scale;
      Eigen::Vector3d euler;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        euler(j) = val;
      }
      euler = euler.array() * degree2radian;
      Eigen::Quaterniond q;
      euler_to_quat(euler, rest_affine_list[i], q);
      //std::cout<<"1 i: "<<i<<", mat: "<<rest_affine_list[i].rotation()<<std::endl;

      q = rest_affine_list[i].rotation().transpose()*q;

      //euler_to_quat(euler, q);

      //std::cout<<"q w: "<<q.w()<<"< vec: "<<q.vec()<<std::endl;

      const Eigen::Vector3d cn = C.row(i).transpose();

      tran_list[i] = tran - rest_tran_list[i] + cn-q*cn;
      //tran_list[i] = tran - rest_tran_list[i];
      rot_list[i] = q;
    }
    RotationList vQ = rot_list;
    std::vector<Eigen::Vector3d> vT= tran_list;
    //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

    Eigen::MatrixXd T(num_bone * (dim+1), dim);
    for (int i = 0; i < num_bone; i++) {
      Eigen::Affine3d a = Eigen::Affine3d::Identity();
      a.translate(vT[i]);
      a.rotate(vQ[i]);
      T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
    }
    T_list[k] = T;
  }
  fclose(file);

}

// void read_pnt_anim(std::string anim_file,
//     std::vector<Eigen::MatrixXd>& T_list)
// {
//   FILE* file;
//   file= fopen(anim_file.c_str(), "rb");

//   int dim = 3;//= C.cols();

//   double degree2radian = igl::PI/180.0;

//   int num_bone, num_frame;
//   double val = 0;

//   typedef std::vector<Eigen::Quaterniond,
//       Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

//   fscanf(file, "%d %d\n", &num_bone, &num_frame);
//   T_list.resize(num_frame);
//   //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

//   std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
//   std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

//   for(int i=0; i<num_bone ; i++){
//     Eigen::Vector3d rest_tran;
//     for (int j = 0; j < dim; j++) {
//       fscanf(file, "%lf", &val);
//       rest_tran(j) = val;
//     }
//     rest_tran_list[i] = rest_tran;

//     Eigen::Vector3d euler;
//     for (int j = 0; j < dim; j++) {
//       fscanf(file, "%lf", &val);
//       euler(j) = val;
//     }
//     euler = euler.array() * degree2radian;
//     Eigen::Quaterniond q;
//     euler_to_quat(euler, q);
//     Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
//     root_affine.rotate(q);
//     rest_affine_list[i] = root_affine;
//   }
//   RotationList rot_list(num_bone);
//   std::vector<Eigen::Vector3d> tran_list(num_bone);

//   for (int k = 0; k < num_frame; k++) {
//     for (int i = 0; i < num_bone; i++) {
//       Eigen::Vector3d tran;
//       for (int j = 0; j < dim; j++) {
//         fscanf(file, "%lf", &val);
//         tran(j) = val;
//       }
//       Eigen::Vector3d euler;
//       for (int j = 0; j < dim; j++) {
//         fscanf(file, "%lf", &val);
//         euler(j) = val;
//       }
//       euler = euler.array() * degree2radian;
//       Eigen::Quaterniond q;
//       euler_to_quat(euler, rest_affine_list[i], q);

//       tran_list[i] = tran - rest_tran_list[i];
//       rot_list[i] = q;
//     }
//     RotationList vQ = rot_list;
//     std::vector<Eigen::Vector3d> vT= tran_list;
//     //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

//     Eigen::MatrixXd T(num_bone * (dim+1), dim);
//     for (int i = 0; i < num_bone; i++) {
//       Eigen::Affine3d a = Eigen::Affine3d::Identity();
//       a.translate(vT[i]);
//       a.rotate(vQ[i]);
//       T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
//     }
//     T_list[k] = T;
//   }
//   fclose(file);
// }

void read_pnt_anim(std::string anim_file,
    Eigen::MatrixXd C,
    std::vector<Eigen::MatrixXd>& T_list)
{
  FILE* file;
  file= fopen(anim_file.c_str(), "rb");

  int dim = 3;//= C.cols();

  double degree2radian = igl::PI/180.0;

  int num_bone, num_frame;
  double val = 0;

  typedef std::vector<Eigen::Quaterniond,
      Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

  fscanf(file, "%d %d\n", &num_bone, &num_frame);
  T_list.resize(num_frame);
  //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

  std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
  std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

  for(int i=0; i<num_bone ; i++){
    Eigen::Vector3d rest_tran;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      rest_tran(j) = val;
    }
    rest_tran_list[i] = rest_tran;

    Eigen::Vector3d euler;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      euler(j) = val;
    }
    euler = euler.array() * degree2radian;
    Eigen::Quaterniond q;
    euler_to_quat(euler, q);
    Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
    root_affine.rotate(q);
    rest_affine_list[i] = root_affine;
  }
  RotationList rot_list(num_bone);
  std::vector<Eigen::Vector3d> tran_list(num_bone);

  for (int k = 0; k < num_frame; k++) {
    for (int i = 0; i < num_bone; i++) {
      Eigen::Vector3d tran;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        tran(j) = val;
      }
      Eigen::Vector3d euler;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        euler(j) = val;
      }
      euler = euler.array() * degree2radian;
      Eigen::Quaterniond q;
      euler_to_quat(euler, rest_affine_list[i], q);

      q = rest_affine_list[i].rotation().transpose()*q;

      const Eigen::Vector3d cn = C.row(i).transpose();

      tran_list[i] = tran - rest_tran_list[i] + cn-q*cn;
      rot_list[i] = q;//cn-q*cn;//q;
    }
    RotationList vQ = rot_list;
    std::vector<Eigen::Vector3d> vT= tran_list;
    //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

    Eigen::MatrixXd T(num_bone * (dim+1), dim);
    for (int i = 0; i < num_bone; i++) {
      Eigen::Affine3d a = Eigen::Affine3d::Identity();
      a.translate(vT[i]);
      a.rotate(vQ[i]);
      T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
    }
    T_list[k] = T;
  }
  fclose(file);
}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[])
{
    Eigen::MatrixXd C;
    Eigen::MatrixXi BE;
    Eigen::VectorXi P;
    Eigen::VectorXd center;
    double scale;
    char* anim_file = mxArrayToString(prhs[0]);

    igl::matlab::parse_rhs_double(prhs+1, C);
    igl::matlab::parse_rhs_double(prhs+2, center);
    scale = (double) *mxGetPr(prhs[3]);

    std::vector<Eigen::MatrixXd> T_list;
    //read_pnt_anim(anim_file, center, scale, T_list);
    //read_pnt_anim(anim_file, C, T_list);
    read_pnt_anim(anim_file, C, center, scale, T_list);

    plhs[0] = mxCreateCellMatrix(T_list.size(),1);

    mxArray *x;
    for(int i=0; i<T_list.size(); i++){
        const int m = T_list[i].rows();
        const int n = T_list[i].cols();
        x = mxCreateDoubleMatrix(m,n, mxREAL);
        Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> > map(mxGetPr(x),m,n);
        map = T_list[i].template cast<double>();
        mxSetCell(plhs[0], i, x);
    }
    return;
}