//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_CUMTRAPZ_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_CUMTRAPZ_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/constant/constants/one.hpp>

/*!
 * \ingroup boost_simd_swar
 * \defgroup boost_simd_swar_cumtrapz cumtrapz
 *
 * \par Description
 * compute the cumulate trapz of the vector elements using the abscissae differences
 * is they are given
 *  z = cumtrapz(y) computes an approximation of the cumulative
 *  integral of y via the trapezoidal method (with unit spacing).  to
 *  compute the integral for spacing different from one, multiply z by
 *  the spacing incrementor use cumtrapz(dx, y) where dx is the abscisae
 *  constant and SCALAR increment.
 *
 *  for vectors, cumtrapz(y) is a vector containing the cumulative
 *  integral of y. for matrices, cumtrapz(y) is a matrix the same size as
 *  x with the cumulative integral over each column. for n-d arrays,
 *  cumtrapz(y) works along the first non-singleton dimension.
 *
 *  z = cumtrapz(x,y) computes the cumulative integral of y with respect
 *  to x using trapezoidal integration.  x and y must be vectors of the
 *  same length, or x must be a column vector and y an array whose first
 *  non-singleton dimension is length(x).  cumtrapz operates across this
 *  dimension.
 *  if x is scalar the increment is considered constant and of value x.
 *  (A 1x1 matrix expression is not a scalar)
 *
 *  z = cumtrapz(x,y,dim) or cumtrapz(y,dim) integrates along dimension
 *  dim of y. the length of x must be the same as size(y,dim)).
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/cumtrapz.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::cumtrapz_(A0)>::type
 *     cumtrapz(const A0 & x, const A1 & y, const A2 & dim);
 * }
 * \endcode
 *
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag cumtrapz_ of functor cumtrapz
     *        in namespace boost::simd::tag for toolbox boost.simd.swar
    **/
    struct cumtrapz_ : tag::formal_
    {
      typedef tag::formal_ parent;
    };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumtrapz_, cumtrapz, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumtrapz_, cumtrapz, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumtrapz_, cumtrapz, 3)
} }

#endif

// modified by jt the 25/12/2010


//
// Copyright (c) 2019-2020 INRIA
//

#include "pinocchio/autodiff/casadi.hpp"

#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <casadi/casadi.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_integrate)
{
  typedef double Scalar;
  typedef casadi::SX ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVector q(model.nq);
  q = pinocchio::randomConfiguration(model);
  TangentVector v(TangentVector::Random(model.nv));
  TangentVector a(TangentVector::Random(model.nv));

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  pinocchio::rnea(model,data,q,v,a);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);

  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);

  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
  casadi::SX cs_q_int(model.nq,1);
  pinocchio::casadi::copy(q_int_ad,cs_q_int);

  std::cout << "cs_q_int:" << cs_q_int << std::endl;
  casadi::Function eval_integrate("eval_integrate",
                                  casadi::SXVector {cs_q,cs_v_int},
                                  casadi::SXVector {cs_q_int});
  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

  std::vector<double> v_int_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();
  casadi::DM q_int_res = eval_integrate(casadi::DMVector {q_vec,v_int_vec})[0];

  Data::ConfigVectorType q_int_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(q_int_res).data(),model.nq,1);

  ConfigVector q_plus(model.nq);
  pinocchio::integrate(model,q,TangentVector::Zero(model.nv),q_plus);

  std::cout << "q_int_vec: " << q_int_vec.transpose() << std::endl;
  BOOST_CHECK(q_plus.isApprox(q_int_vec));
}

BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
{
  typedef double Scalar;
  typedef casadi::SX ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;
  typedef Model::Data Data;

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::Data ADData;

  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  Data data(model);

  typedef Model::ConfigVectorType ConfigVector;
  typedef Model::TangentVectorType TangentVector;
  ConfigVector q(model.nq);
  q = pinocchio::randomConfiguration(model);
  TangentVector v(TangentVector::Random(model.nv));
  TangentVector a(TangentVector::Random(model.nv));

  typedef ADModel::ConfigVectorType ConfigVectorAD;
  typedef ADModel::TangentVectorType TangentVectorAD;
  ADModel ad_model = model.cast<ADScalar>();
  ADData ad_data(ad_model);

  pinocchio::rnea(model,data,q,v,a);

  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);

  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);

  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
  casadi::SX cs_q_int(model.nq,1);
  pinocchio::casadi::copy(q_int_ad,cs_q_int);
  std::vector<double> q_vec((size_t)model.nq);
  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

  std::vector<double> v_int_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();

  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
  TangentVectorAD v_ad(model.nv);
  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);

  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
  TangentVectorAD a_ad(model.nv);
  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);

  rnea(ad_model,ad_data,q_int_ad,v_ad,a_ad);
  casadi::SX cs_tau(model.nv,1);
  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    cs_tau(k) = ad_data.tau[k];
  }
  casadi::Function eval_rnea("eval_rnea",
                             casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
                             casadi::SXVector {cs_tau});

  std::vector<double> v_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

  std::vector<double> a_vec((size_t)model.nv);
  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;

  // check return value
  casadi::DM tau_res = eval_rnea(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
  std::cout << "tau_res = " << tau_res << std::endl;
  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(tau_res).data(),model.nv,1);

  BOOST_CHECK(data.tau.isApprox(tau_vec));

  // compute references
  Data::MatrixXs dtau_dq_ref(model.nv,model.nv), dtau_dv_ref(model.nv,model.nv), dtau_da_ref(model.nv,model.nv);
  dtau_dq_ref.setZero(); dtau_dv_ref.setZero(); dtau_da_ref.setZero();

  pinocchio::computeRNEADerivatives(model,data,q,v,a,dtau_dq_ref,dtau_dv_ref,dtau_da_ref);
  dtau_da_ref.triangularView<Eigen::StrictlyLower>() = dtau_da_ref.transpose().triangularView<Eigen::StrictlyLower>();

  // check with respect to q+dq
  casadi::SX dtau_dq = jacobian(cs_tau, cs_v_int);
  casadi::Function eval_dtau_dq("eval_dtau_dq",
                                casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
                                casadi::SXVector {dtau_dq});

  casadi::DM dtau_dq_res = eval_dtau_dq(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
  std::vector<double> dtau_dq_vec(static_cast< std::vector<double> >(dtau_dq_res));
  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_dq_vec.data(),model.nv,model.nv).isApprox(dtau_dq_ref));

  // check with respect to v+dv
  casadi::SX dtau_dv = jacobian(cs_tau, cs_v);
  casadi::Function eval_dtau_dv("eval_dtau_dv",
                                casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
                                casadi::SXVector {dtau_dv});

  casadi::DM dtau_dv_res = eval_dtau_dv(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
  std::vector<double> dtau_dv_vec(static_cast< std::vector<double> >(dtau_dv_res));
  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_dv_vec.data(),model.nv,model.nv).isApprox(dtau_dv_ref));

  // check with respect to a+da
  casadi::SX dtau_da = jacobian(cs_tau, cs_a);
  casadi::Function eval_dtau_da("eval_dtau_da",
                                casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
                                casadi::SXVector {dtau_da});

  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
  std::vector<double> dtau_da_vec(static_cast< std::vector<double> >(dtau_da_res));
  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_da_vec.data(),model.nv,model.nv).isApprox(dtau_da_ref));

  // call RNEA derivatives in Casadi
  casadi::SX cs_dtau_dq(model.nv,model.nv);
  casadi::SX cs_dtau_dv(model.nv,model.nv);
  casadi::SX cs_dtau_da(model.nv,model.nv);

  computeRNEADerivatives(ad_model,ad_data,q_ad,v_ad,a_ad);
  ad_data.M.triangularView<Eigen::StrictlyLower>()
  = ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();

  pinocchio::casadi::copy(ad_data.dtau_dq,cs_dtau_dq);
  pinocchio::casadi::copy(ad_data.dtau_dv,cs_dtau_dv);
  pinocchio::casadi::copy(ad_data.M,cs_dtau_da);

  casadi::Function eval_rnea_derivatives_dq("eval_rnea_derivatives_dq",
                                            casadi::SXVector {cs_q, cs_v, cs_a},
                                            casadi::SXVector {cs_dtau_dq});

  casadi::DM dtau_dq_res_direct = eval_rnea_derivatives_dq(casadi::DMVector {q_vec,v_vec,a_vec})[0];
  Data::MatrixXs dtau_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_dq_res_direct).data(),model.nv,model.nv);
  BOOST_CHECK(dtau_dq_ref.isApprox(dtau_dq_res_direct_map));

  casadi::Function eval_rnea_derivatives_dv("eval_rnea_derivatives_dv",
                                            casadi::SXVector {cs_q, cs_v, cs_a},
                                            casadi::SXVector {cs_dtau_dv});

  casadi::DM dtau_dv_res_direct = eval_rnea_derivatives_dv(casadi::DMVector {q_vec,v_vec,a_vec})[0];
  Data::MatrixXs dtau_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_dv_res_direct).data(),model.nv,model.nv);
  BOOST_CHECK(dtau_dv_ref.isApprox(dtau_dv_res_direct_map));

  casadi::Function eval_rnea_derivatives_da("eval_rnea_derivatives_da",
                                            casadi::SXVector {cs_q, cs_v, cs_a},
                                            casadi::SXVector {cs_dtau_da});

  casadi::DM dtau_da_res_direct = eval_rnea_derivatives_da(casadi::DMVector {q_vec,v_vec,a_vec})[0];
  Data::MatrixXs dtau_da_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_da_res_direct).data(),model.nv,model.nv);
  BOOST_CHECK(dtau_da_ref.isApprox(dtau_da_res_direct_map));
}

  BOOST_AUTO_TEST_CASE(test_aba)
  {
    typedef double Scalar;
    typedef casadi::SX ADScalar;

    typedef pinocchio::ModelTpl<Scalar> Model;
    typedef Model::Data Data;

    typedef pinocchio::ModelTpl<ADScalar> ADModel;
    typedef ADModel::Data ADData;

    Model model;
    pinocchio::buildModels::humanoidRandom(model);
    model.lowerPositionLimit.head<3>().fill(-1.);
    model.upperPositionLimit.head<3>().fill(1.);
    Data data(model);

    typedef Model::ConfigVectorType ConfigVector;
    typedef Model::TangentVectorType TangentVector;
    ConfigVector q(model.nq);
    q = pinocchio::randomConfiguration(model);
    TangentVector v(TangentVector::Random(model.nv));
    TangentVector tau(TangentVector::Random(model.nv));

    typedef ADModel::ConfigVectorType ConfigVectorAD;
    typedef ADModel::TangentVectorType TangentVectorAD;
    ADModel ad_model = model.cast<ADScalar>();
    ADData ad_data(ad_model);

    pinocchio::aba(model,data,q,v,tau);

    casadi::SX cs_q = casadi::SX::sym("q", model.nq);
    casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
    ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
    q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
    v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);

    pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
    casadi::SX cs_q_int(model.nq,1);
    pinocchio::casadi::copy(q_int_ad,cs_q_int);
    std::vector<double> q_vec((size_t)model.nq);
    Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;

    std::vector<double> v_int_vec((size_t)model.nv);
    Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();

    casadi::SX cs_v = casadi::SX::sym("v", model.nv);
    TangentVectorAD v_ad(model.nv);
    v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);

    casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
    TangentVectorAD tau_ad(model.nv);
    tau_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_tau).data(),model.nv,1);

    // ABA
    aba(ad_model,ad_data,q_int_ad,v_ad,tau_ad);
    casadi::SX cs_ddq(model.nv,1);
    for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
      cs_ddq(k) = ad_data.ddq[k];
    casadi::Function eval_aba("eval_aba",
                              casadi::SXVector {cs_q, cs_v_int, cs_v, cs_tau},
                              casadi::SXVector {cs_ddq});

    std::vector<double> v_vec((size_t)model.nv);
    Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;

    std::vector<double> tau_vec((size_t)model.nv);
    Eigen::Map<TangentVector>(tau_vec.data(),model.nv,1) = tau;

    casadi::DM ddq_res = eval_aba(casadi::DMVector {q_vec, v_int_vec, v_vec, tau_vec})[0];
    Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(ddq_res).data(),
                                                            model.nv,1);

    BOOST_CHECK(ddq_mat.isApprox(data.ddq));

    // compute references
    Data::MatrixXs ddq_dq_ref(model.nv,model.nv), ddq_dv_ref(model.nv,model.nv), ddq_dtau_ref(model.nv,model.nv);
    ddq_dq_ref.setZero(); ddq_dv_ref.setZero(); ddq_dtau_ref.setZero();

    pinocchio::computeABADerivatives(model,data,q,v,tau,ddq_dq_ref,ddq_dv_ref,ddq_dtau_ref);
    ddq_dtau_ref.triangularView<Eigen::StrictlyLower>()
    = ddq_dtau_ref.transpose().triangularView<Eigen::StrictlyLower>();

    // check with respect to q+dq
    casadi::SX ddq_dq = jacobian(cs_ddq, cs_v_int);
    casadi::Function eval_ddq_dq("eval_ddq_dq",
                                  casadi::SXVector {cs_q,cs_v_int,cs_v,cs_tau},
                                  casadi::SXVector {ddq_dq});

    casadi::DM ddq_dq_res = eval_ddq_dq(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
    std::vector<double> ddq_dq_vec(static_cast< std::vector<double> >(ddq_dq_res));
    BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dq_vec.data(),model.nv,model.nv).isApprox(ddq_dq_ref));

    // check with respect to v+dv
    casadi::SX ddq_dv = jacobian(cs_ddq, cs_v);
    casadi::Function eval_ddq_dv("eval_ddq_dv",
                                  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_tau},
                                  casadi::SXVector {ddq_dv});

    casadi::DM ddq_dv_res = eval_ddq_dv(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
    std::vector<double> ddq_dv_vec(static_cast< std::vector<double> >(ddq_dv_res));
    BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dv_vec.data(),model.nv,model.nv).isApprox(ddq_dv_ref));

    // check with respect to a+da
    casadi::SX ddq_dtau = jacobian(cs_ddq, cs_tau);
    casadi::Function eval_ddq_da("eval_ddq_da",
                                  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_tau},
                                  casadi::SXVector {ddq_dtau});

    casadi::DM ddq_dtau_res = eval_ddq_da(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
    std::vector<double> ddq_dtau_vec(static_cast< std::vector<double> >(ddq_dtau_res));
    BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dtau_vec.data(),model.nv,model.nv).isApprox(ddq_dtau_ref));

    // call ABA derivatives in Casadi
    casadi::SX cs_ddq_dq(model.nv,model.nv);
    casadi::SX cs_ddq_dv(model.nv,model.nv);
    casadi::SX cs_ddq_dtau(model.nv,model.nv);

    computeABADerivatives(ad_model,ad_data,q_ad,v_ad,tau_ad);
    ad_data.Minv.triangularView<Eigen::StrictlyLower>()
    = ad_data.Minv.transpose().triangularView<Eigen::StrictlyLower>();

    pinocchio::casadi::copy(ad_data.ddq_dq,cs_ddq_dq);
    pinocchio::casadi::copy(ad_data.ddq_dv,cs_ddq_dv);
    pinocchio::casadi::copy(ad_data.Minv,cs_ddq_dtau);

    casadi::Function eval_aba_derivatives_dq("eval_aba_derivatives_dq",
                                              casadi::SXVector {cs_q, cs_v, cs_tau},
                                              casadi::SXVector {cs_ddq_dq});

    casadi::DM ddq_dq_res_direct = eval_aba_derivatives_dq(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
    Data::MatrixXs ddq_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dq_res_direct).data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dq_ref.isApprox(ddq_dq_res_direct_map));

    casadi::Function eval_aba_derivatives_dv("eval_aba_derivatives_dv",
                                              casadi::SXVector {cs_q, cs_v, cs_tau},
                                              casadi::SXVector {cs_ddq_dv});

    casadi::DM ddq_dv_res_direct = eval_aba_derivatives_dv(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
    Data::MatrixXs ddq_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dv_res_direct).data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dv_ref.isApprox(ddq_dv_res_direct_map));

    casadi::Function eval_aba_derivatives_dtau("eval_aba_derivatives_dtau",
                                              casadi::SXVector {cs_q, cs_v, cs_tau},
                                              casadi::SXVector {cs_ddq_dtau});

    casadi::DM ddq_dtau_res_direct = eval_aba_derivatives_dtau(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
    Data::MatrixXs ddq_dtau_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dtau_res_direct).data(),model.nv,model.nv);
    BOOST_CHECK(ddq_dtau_ref.isApprox(ddq_dtau_res_direct_map));
  }

BOOST_AUTO_TEST_SUITE_END()


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------

 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/symbolic_function.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_dgp.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_values_extractors.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>

using namespace dealii;

template <int dim>
class Step6
{
public:
  Step6();

  void
  run();

private:
  void
  setup_system();
  void
  assemble_system();
  void
  solve();
  void
  refine_grid();
  void
  output_results(const unsigned int cycle) const;

  Triangulation<dim> triangulation;

  FE_Q<dim>     fe_velocity;
  FE_DGP<dim>   fe_pressure;
  FESystem<dim> fe;

  FEValuesExtractors::Vector velocity;
  FEValuesExtractors::Scalar pressure;

  DoFHandler<dim> dof_handler;

  AffineConstraints<double> constraints;

  SparseMatrix<double> system_matrix;
  SparsityPattern      sparsity_pattern;

  Vector<double> solution;
  Vector<double> system_rhs;
};

template <int dim>
Step6<dim>::Step6()
  : fe_velocity(2)
  , fe_pressure(1)
  , fe(fe_velocity, dim, fe_pressure, 1)
  , velocity(0)
  , pressure(dim)
  , dof_handler(triangulation)
{}

template <int dim>
void
Step6<dim>::setup_system()
{
  dof_handler.distribute_dofs(fe);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());

  constraints.clear();
  DoFTools::make_hanging_node_constraints(dof_handler, constraints);

  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<dim>(dim +
                                                                        1),
                                           constraints,
                                           fe.component_mask(velocity));

  constraints.close();

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler,
                                  dsp,
                                  constraints,
                                  /*keep_constrained_dofs = */ false);

  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);
}

template <int dim>
void
Step6<dim>::assemble_system()
{
  const QGauss<dim> quadrature_formula(fe.degree + 1);

  FEValues<dim> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe.dofs_per_cell;

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  Functions::SymbolicFunction<dim> rhs_function(
    dim == 2 ?
      "x^2*y^2*(x - 1)^2*(2*y - 2) + 2*x^2*y*(x - 1)^2*(y - 1)^2;-x^2*y^2*(2*x - 2)*(y - 1)^2 - 2*x*y^2*(x - 1)^2*(y - 1)^2; 0" :
      "x^2*y^2*(x - 1)^2*(2*y - 2) + 2*x^2*y*(x - 1)^2*(y - 1)^2;-x^2*y^2*(2*x - 2)*(y - 1)^2 - 2*x*y^2*(x - 1)^2*(y - 1)^2; 0; 0");

  Vector<double> rhs_values(dim + 1);
  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      cell_matrix = 0;
      cell_rhs    = 0;

      fe_values.reinit(cell);

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          rhs_function.vector_value(fe_values.quadrature_point(q_index),
                                    rhs_values);
          for (const unsigned int i : fe_values.dof_indices())
            {
              auto v      = fe_values[velocity].value(i, q_index);
              auto div_v  = fe_values[velocity].divergence(i, q_index);
              auto grad_v = fe_values[velocity].gradient(i, q_index);
              auto q      = fe_values[pressure].value(i, q_index);

              for (const unsigned int j : fe_values.dof_indices())
                {
                  auto div_u  = fe_values[velocity].divergence(j, q_index);
                  auto grad_u = fe_values[velocity].gradient(j, q_index);
                  auto p      = fe_values[pressure].value(j, q_index);

                  cell_matrix(i, j) +=
                    (scalar_product(grad_u, grad_v) - div_v * p - div_u * q) *
                    fe_values.JxW(q_index); // dx
                }
              for (unsigned int d = 0; d < dim; ++d)
                cell_rhs(i) += (1000 * rhs_values[d] *   // f(x)
                                v[d] *                   // phi_i(x_q)
                                fe_values.JxW(q_index)); // dx
            }
        }

      cell->get_dof_indices(local_dof_indices);
      constraints.distribute_local_to_global(
        cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
    }
}

template <int dim>
void
Step6<dim>::solve()
{
  SparseDirectUMFPACK inverse;
  inverse.initialize(system_matrix);

  // SolverControl            solver_control(1000, 1e-12);
  // SolverCG<Vector<double>> solver(solver_control);

  // PreconditionSSOR<SparseMatrix<double>> preconditioner;
  // preconditioner.initialize(system_matrix, 1.2);

  // solver.solve(system_matrix, solution, system_rhs, preconditioner);

  inverse.vmult(solution, system_rhs);
  constraints.distribute(solution);
}

template <int dim>
void
Step6<dim>::refine_grid()
{
  Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

  KellyErrorEstimator<dim>::estimate(dof_handler,
                                     QGauss<dim - 1>(fe.degree + 1),
                                     {},
                                     solution,
                                     estimated_error_per_cell,
                                     fe.component_mask(velocity));

  GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                  estimated_error_per_cell,
                                                  0.3,
                                                  0.03);

  triangulation.execute_coarsening_and_refinement();
}

template <int dim>
void
Step6<dim>::output_results(const unsigned int cycle) const
{
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");
    DataOutBase::VtkFlags flags;
    flags.write_higher_order_cells = true;

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);

    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);
    DataOut<dim> data_out;
    data_out.set_flags(flags);

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches(fe.degree);

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
    data_out.write_vtu(output);
  }
}

template <int dim>
void
Step6<dim>::run()
{
  for (unsigned int cycle = 0; cycle < 4; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle == 0)
        {
          GridGenerator::hyper_cube(triangulation);
          triangulation.refine_global(4);
        }
      else
        refine_grid();

      std::cout << "   Number of active cells:       "
                << triangulation.n_active_cells() << std::endl;

      setup_system();

      std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                << std::endl;

      assemble_system();
      solve();
      output_results(cycle);
    }
}

int
main()
{
  try
    {
      Step6<2> laplace_problem_2d;
      laplace_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


 #include <boost/spirit.hpp>
 #include <boost/spirit/tree/ast.hpp>
 #include <string>
 #include <cassert>
 #include <iostream>
 #include <istream>
 #include <ostream>

 using boost::spirit::rule;
 using boost::spirit::parser_tag;
 using boost::spirit::ch_p;
 using boost::spirit::real_p;

 using boost::spirit::tree_node;
 using boost::spirit::node_val_data;

 // The grammar
 struct parser: public boost::spirit::grammar<parser>
 {
   enum rule_ids { addsub_id, multdiv_id, value_id, real_id };

   struct set_value
   {
     set_value(parser const& p): self(p) {}
     void operator()(tree_node<node_val_data<std::string::iterator,
                                             double> >& node,
                     std::string::iterator begin,
                     std::string::iterator end) const
     {
       node.value.value(self.tmp);
     }
     parser const& self;
   };

   mutable double tmp;

   template<typename Scanner> struct definition
   {
     rule<Scanner, parser_tag<addsub_id> > addsub;
     rule<Scanner, parser_tag<multdiv_id> > multdiv;
     rule<Scanner, parser_tag<value_id> > value;
     rule<Scanner, parser_tag<real_id> > real;

     definition(parser const& self)
     {
       using namespace boost::spirit;
       addsub = multdiv
         >> *((root_node_d[ch_p('+')] | root_node_d[ch_p('-')]) >> multdiv);
       multdiv = value
         >> *((root_node_d[ch_p('*')] | root_node_d[ch_p('/')]) >> value);
       value = real | inner_node_d[('(' >> addsub >> ')')];
       real = leaf_node_d[access_node_d[real_p[assign_a(self.tmp)]][set_value(self)]];
     }

     rule<Scanner, parser_tag<addsub_id> > const& start() const
     {
       return addsub;
     }
   };
 };

 template<typename TreeIter>
 double evaluate(TreeIter const& i)
 {
   double op1, op2;
   switch (i->value.id().to_long())
   {
   case parser::real_id:
     return i->value.value();
   case parser::value_id:
   case parser::addsub_id:
   case parser::multdiv_id:
     op1 = evaluate(i->children.begin());
     op2 = evaluate(i->children.begin()+1);
     switch(*i->value.begin())
     {
     case '+':
       return op1 + op2;
     case '-':
       return op1 - op2;
     case '*':
       return op1 * op2;
     case '/':
       return op1 / op2;
     default:
       assert(!"Should not happen");
     }
   default:
     assert(!"Should not happen");
   }
   return 0;
 }

 // the read/eval/write loop
 int main()
 {
   parser eval;
   std::string line;
   while (std::cout << "Expression: "
          && std::getline(std::cin, line)
          && !line.empty())
   {
     typedef boost::spirit::node_val_data_factory<double> factory_t;
     boost::spirit::tree_parse_info<std::string::iterator, factory_t> info =
       boost::spirit::ast_parse<factory_t>(line.begin(), line.end(),
                                           eval, boost::spirit::space_p);
     if (info.full)
     {
       std::cout << "Result: " << evaluate(info.trees.begin()) << std::endl;
     }
     else
     {
       std::cout << "Error in expression." << std::endl;
     }
   }
 };


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/timer.hpp>
#include <iostream>
#include <list>

#include <boost/numeric/linear_algebra/accumulate.hpp>
#include <boost/numeric/linear_algebra/concept_maps.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>

template <typename Element>
void test_accumulate(const char* name)
{
    const int   array_size= 10;
    Element     array[array_size];
    for (int i= 0; i < array_size; i++)
    	array[i]= Element(i);

    std::list<Element> l;
    for (int i= 0; i < array_size; i++)
    	l.push_back(Element(i));

    std::cout << '\n' << name << '\n' << " Add: ";
    math::accumulate(&array[0], array+array_size, Element(0), math::add<Element>());
    std::cout << "Mult: ";
    math::accumulate(array, array+array_size, Element(1), math::mult<Element>());
    std::cout << "Mult [with a list]: ";
    math::accumulate(l.begin(), l.end(), Element(1), math::mult<Element>());
    std::cout << " Min: ";
    math::accumulate(array, array+array_size, Element(1000), math::min<Element>());
    std::cout << " Max: ";
    math::accumulate(array, array+array_size, Element(-1000), math::max<Element>());
}

int main(int, char* [])
{
    test_accumulate<int>("int");
    test_accumulate<float>("float");
    test_accumulate<double>("double");
    std::cout << '\n';

    return 0;
}


// Copyright (c) 2020 Sabar Nimmagadda. All rights reserved.

#include "matrix_app.h"

#include <cinder/app/App.h>
#include <Eigen/Dense>
#include <cinder/gl/gl.h>
#include <cinder/Font.h>
#include <cinder/Text.h>
#include <cinder/Vector.h>
#include <cinder/gl/draw.h>
#include <Linear Algebra/computations.h>
#include <iostream>
#include <utility>
#include "CinderImGui.h"
using Eigen::MatrixXd;
namespace matrixapp {
using cinder::app::KeyEvent;
using cinder::Color;
using cinder::ColorA;
using cinder::Rectf;
using cinder::TextBox;
using std::string;
const char kNormalFont[] = "Arial";

MatrixApp::MatrixApp()
    : state_{AppState::kSelecting} {}

void MatrixApp::setup() {
    ui::initialize();
    test_mat << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;
}

void MatrixApp::update() {
    if (state_ == AppState::kInputtingData) {
        InputMatrix();
        String_To_Matrix();
    }
}

void MatrixApp::draw() {
    DrawBackground();
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 50};
    const Color color = Color::white();
    if (state_ == AppState::kSelecting) {
        PrintText("WELCOME", color, size, {100, 50});
        CreateMenu();
    }
    if (state_ == AppState::kSolved) {
        DrawAnswer();
    }
}

void MatrixApp::CreateMenu() {
    ui::ScopedWindow window( "Choose problem", ImGuiWindowFlags_MenuBar );
    if( ui::BeginMenuBar() ){
        if( ui::BeginMenu( "Problem Type" )) {
            if (ui::MenuItem( "RREF" )) {
                problemType = ProblemType::RREF;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Row Space" )) {
                problemType = ProblemType::RowSpace;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Column Space")) {
                problemType = ProblemType::ColumnSpace;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem("LU Decomposition")) {
                problemType = ProblemType::LUDecomposition;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Permutation Matrix")) {
                problemType = ProblemType::PermutationMatrix;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Inverse")) {
                problemType = ProblemType::Inverse;
                state_= AppState::kInputtingData;
            }
            if (ui::MenuItem( "Matrix Multiplication")) {
                problemType = ProblemType::MatrixMultiplication;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("QR Decomposition")) {
                problemType = ProblemType::QRDecomposition;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Dot Product")) {
                problemType = ProblemType::DotProduct;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Eigen Vectors")) {
                problemType = ProblemType::EigenVectors;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Eigen Values")) {
                problemType = ProblemType::EigenValues;
                state_ = AppState::kInputtingData;
            }
            if (ui::MenuItem("Determinant")) {
                problemType = ProblemType::Determinant;
                state_ = AppState::kInputtingData;
            }
            ui::EndMenu();
        }
        ui::EndMenuBar();
    }
    const ImVec2 vec2(500, 500);
    ui::SetWindowSize("Choose problem", vec2);
}

void MatrixApp::DrawAnswer() {
    if (problemType == ProblemType::QRDecomposition) {
        DrawQRAnswer(in_mat1);
    }
    if (problemType == ProblemType::LUDecomposition) {
        DrawLUAnswer(in_mat1);
    }
    if (problemType == ProblemType::PermutationMatrix) {
        DrawPermutationAnswer(in_mat1);
    }
    if (problemType == ProblemType::RREF) {
        DrawRREFAnswer(in_mat1);
    }
    if (problemType == ProblemType::EigenValues) {
        DrawEigenValuesAnswer(in_mat1);
    }
    if (problemType == ProblemType::Inverse) {
        DrawInverseAnswer(in_mat1);
    }
    if (problemType == ProblemType::EigenVectors) {
        DrawEigenVectorsAnswer(in_mat1);
    }
    if (problemType == ProblemType::DotProduct) {
        DrawDotProductAnswer(in_mat1, in_mat2);
    }
    if (problemType == ProblemType::MatrixMultiplication) {
        DrawMultiplicationAnswer(in_mat1, in_mat2);
    }
    if (problemType == ProblemType::Determinant) {
        DrawDeterminantAnswer(in_mat1);
    }
    if (problemType == ProblemType::RowSpace) {
        DrawRowSpaceAnswer(in_mat1);
    }
    if (problemType == ProblemType::ColumnSpace) {
        DrawColSpaceAnswer(in_mat1);
    }
}

void MatrixApp::keyDown(KeyEvent event) {
}

void MatrixApp::PrintText(const string& text, const Color color, const cinder::ivec2& size,
               const cinder::vec2& loc) {
    cinder::gl::color(color);
    auto box = TextBox()
            .alignment(TextBox::CENTER)
            .font(cinder::Font(kNormalFont, 30))
            .size(size)
            .color(color)
            .backgroundColor(ColorA(0, 0, 1, 0))
            .text(text);

    const auto box_size = box.getSize();
    const cinder::vec2 locp = {loc.x - box_size.x / 2, loc.y - box_size.y / 2};
    const auto surface = box.render();
    const auto texture = cinder::gl::Texture::create(surface);
    cinder::gl::draw(texture, locp);
}
void MatrixApp::DrawBackground() const {
    cinder::gl::clear(Color(0, 0, 0));
}
void MatrixApp::DrawLUAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeL(matrix);
    PrintText("Your L Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    std::stringstream st;
    st << Computations::ComputeU(matrix);
    PrintText("Your U Matrix is",color,{500,500},{center.x-50,center.y + 150});
    PrintText(st.str(), color, size, {center.x, center.y + 200});
    BackToMenu();
}

void MatrixApp::DrawDotProductAnswer(MatrixXd matrix1, MatrixXd matrix2) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeDotProduct(std::move(matrix1), std::move(matrix2), kDimension);
    PrintText("Your Dot Product is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawPermutationAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputePermutationMatrix(std::move(matrix));
    PrintText("Your Permutation Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawRREFAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeRREF(matrix);
    PrintText("Your Row Reduced Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawMultiplicationAnswer(MatrixXd matrix1, MatrixXd matrix2) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeMultiply(std::move(matrix1), std::move(matrix2));
    PrintText("The product Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawInverseAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeInverse(matrix);
    PrintText("The Inverse Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawQRAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeQ(matrix);
    PrintText("Your Q Matrix is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    std::stringstream st;
    st << Computations::ComputeR(matrix);
    PrintText("Your R Matrix is",color,{500,500},{center.x-50,center.y + 150});
    PrintText(st.str(), color, size, {center.x, center.y + 200});
    BackToMenu();
}

void MatrixApp::DrawEigenVectorsAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix);
    PrintText("The Matrix of EigenVectors is",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawEigenValuesAnswer(MatrixXd matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenValues(matrix);
    PrintText("The Eigenvalues are",color,{500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size , center);
    BackToMenu();
}

void MatrixApp::DrawDeterminantAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeDeterminant(matrix);
    PrintText("Your determinant is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawColSpaceAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix.inverse());
    PrintText("Your Column Space Matrix is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::DrawRowSpaceAnswer(const MatrixXd& matrix) {
    const cinder::vec2 center = getWindowCenter();
    const cinder::ivec2 size = {500, 500};
    const Color color = Color::white();
    std::stringstream ss;
    ss << Computations::ComputeEigenVectors(matrix);
    PrintText("Your Row Space Matrix is", color, {500,500},{center.x-50,center.y - 50});
    PrintText(ss.str(), color, size, center);
    BackToMenu();
}

void MatrixApp::InputMatrix() {
    if (problemType != ProblemType::DotProduct && problemType != ProblemType::MatrixMultiplication) {
        ui::InputInt("Enter dimension",  &kDimension);
        ui::InputText("Input matrix", &input_string);
    } else {
        ui::InputInt("Enter dimension",  &kDimension);
        ui::InputText("Input first matrix", &input_string);
        ui::InputText("Input second matrix", &input_string2);
        str_mat2 = input_string2;
    }
    str_mat = input_string;
}

void MatrixApp::String_To_Matrix() {
    int mat_size = kDimension *kDimension;
    if ( problemType != ProblemType::DotProduct
     && problemType != ProblemType::MatrixMultiplication
     && str_mat.size() == mat_size * 2 && mat_size != 0) { // <= size * 2
        //When the computation only needs one matrix.
        std::istringstream ss(str_mat);
        for (int r = 0; r < kDimension; r++) {
            for (int c = 0; c < kDimension; c++) {
                int elem;
                ss >> elem;
                in_mat1(r, c) = elem;
            }
        }
        state_ = AppState::kSolved;
    } else if (str_mat.size() == mat_size * 2 && str_mat2.size() == mat_size * 2 && mat_size != 0){
        //Made else if instead of else, because size cannot be zero ever.
        std::istringstream ss1(str_mat);
        std::istringstream ss2(str_mat2);
        for (int r = 0; r < kDimension; r++) {
            for (int c = 0; c < kDimension; c++) {
                int elem1;
                int elem2;
                ss1 >> elem1;
                ss2 >> elem2;
                in_mat1(r, c) = elem1;
                in_mat2(r, c) = elem2;
            }
        }
        state_ = AppState::kSolved;
    }
}

void MatrixApp::BackToMenu() {
    const cinder::ivec2 button_size = {500, 50};
    if (ui::Button("BACK TO MAIN MENU", button_size)) {
        state_ = AppState::kSelecting;
        input_string = "";
        input_string2 = "";
        kDimension = 0;
    }
}

}  // namespace myapp


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/geometry/geometry_plane_box.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_geometry_util_plane_box);

BOOST_AUTO_TEST_CASE(case_by_case_testing)
{

  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::real_type                            real_type;

  typedef OpenTissue::geometry::PlaneBox<math_types>  plane_box_type;
  plane_box_type plane_box;

  real_type tol = 0.0001;

  vector3_type min_coord(0,0,0);
  vector3_type max_coord(1,1,1);

  plane_box.init(min_coord,max_coord);
  BOOST_CHECK( plane_box.box().min().is_equal( min_coord, tol ) );
  BOOST_CHECK( plane_box.box().max().is_equal( max_coord, tol ) );

  BOOST_CHECK( plane_box.n() == vector3_type(1,0,0) );
  BOOST_CHECK( plane_box.plane().n() == vector3_type(1,0,0) );

  plane_box.set_y_axis();
  BOOST_CHECK( plane_box.n() == vector3_type(0,1,0) );
  BOOST_CHECK( plane_box.plane().n() == vector3_type(0,1,0) );

  plane_box.set_z_axis();
  BOOST_CHECK( plane_box.n() == vector3_type(0,0,1) );
  BOOST_CHECK( plane_box.plane().n() == vector3_type(0,0,1) );

  plane_box.set_x_axis();
  BOOST_CHECK( plane_box.n() == vector3_type(1,0,0) );
  BOOST_CHECK( plane_box.plane().n() == vector3_type(1,0,0) );

  real_type w1 = plane_box.plane().w();
  plane_box.decrement();
  real_type w2 = plane_box.plane().w();
  BOOST_CHECK( w2 < w1 );
  plane_box.increment();
  real_type w3 = plane_box.plane().w();
  BOOST_CHECK( w3 > w2 );

  plane_box.set_x_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.increment();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

  plane_box.set_y_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.increment();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

  plane_box.set_z_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.increment();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

  plane_box.set_x_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.decrement();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

  plane_box.set_y_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.decrement();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

  plane_box.set_z_axis();
  for(size_t i=0;i<500;++i)
  {
    plane_box.decrement();
    BOOST_CHECK( plane_box.p0() <= max_coord );
    BOOST_CHECK( plane_box.p0() >= min_coord );
    BOOST_CHECK( plane_box.p1() <= max_coord );
    BOOST_CHECK( plane_box.p1() >= min_coord );
    BOOST_CHECK( plane_box.p2() <= max_coord );
    BOOST_CHECK( plane_box.p2() >= min_coord );
    BOOST_CHECK( plane_box.p3() <= max_coord );
    BOOST_CHECK( plane_box.p3() >= min_coord );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <cstdlib>
#include <iostream>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/triangular.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef ublas::triangular_matrix<double, ublas::lower, ublas::column_major> matrix_l;
    typedef ublas::triangular_matrix<double, ublas::upper, ublas::column_major> matrix_u;
    typedef typename vector::size_type size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix_l A_l(n, n);
    matrix_u A_u(n, n);
    for (size_type j=0; j<n; ++j) {
      A_u(j, j)=rand_normal<double>::get();
      A_l(j, j)=A_u(j, j);
      for (size_type i=0; i<j; ++i) {
        A_u(i, j)=rand_normal<double>::get();
        A_l(j, i)=A_u(i, j);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    matrix P;
    {
      P=ublas::outer_prod(alpha*x, y);
      P+=ublas::outer_prod(y, alpha*x);
      for (size_type j=0; j<n; ++j)
        for (size_type i=0; i<j; ++i)
          P(i, j)=0;
      matrix A1(P+A_l);
      matrix_l A2(A_l);
      blas::spr2(alpha, x, y, A2);
      std::cout << print_mat(A1) << '\n'
                << print_mat(A2) << '\n';
    }
    {
      P=ublas::outer_prod(alpha*x, y);
      P+=ublas::outer_prod(y, alpha*x);
      for (size_type j=0; j<n; ++j)
        for (size_type i=j+1; i<n; ++i)
          P(i, j)=0;
      matrix A1(P+A_u);
      matrix_u A2(A_u);
      blas::spr2(alpha, x, y, A2);
      std::cout << print_mat(A1) << '\n'
                << print_mat(A2) << '\n';
    }
  }
  return EXIT_SUCCESS;
}


#include "vc.h"

#include "cropper.h"
#include "fft_plan.h"
#include "tensorOps.h"

#include <Eigen/SVD>

Cx4 VBC(Cx4 &maps, Log &log)
{
  Index const nc = maps.dimension(0);
  Index const nx = maps.dimension(1);
  Index const ny = maps.dimension(2);
  Index const nz = maps.dimension(3);

  Eigen::Map<Eigen::MatrixXcf const> mat(maps.data(), nc, nx * ny * nz);
  log.info("VBC SVD size {}x{}", mat.rows(), mat.cols());
  auto const svd = mat.bdcSvd(Eigen::ComputeThinV);
  Cx4 body(maps.dimensions());
  Eigen::Map<Eigen::MatrixXcf> bodymat(body.data(), nc, nx * ny * nz);
  bodymat = svd.matrixV().transpose();
  log.image(maps, "vbc-maps.nii");
  log.image(body, "vbc-body.nii");
  bodymat = bodymat.array().conjugate() / bodymat.array().abs();
  return body;
}

void VCC(Cx4 &data, Log &log)
{
  Index const nc = data.dimension(0);
  Index const nx = data.dimension(1);
  Index const ny = data.dimension(2);
  Index const nz = data.dimension(3);

  // Assemble our virtual conjugate channels
  Cx4 cdata(nc, nx, ny, nz);
  FFT::Planned<5, 3> fft(cdata, log);
  cdata = data;
  log.image(cdata, "vcc-cdata.nii");
  fft.forward(cdata);
  log.image(cdata, "vcc-cdata-ks.nii");
  Cx4 rdata = cdata.slice(Sz4{0, 1, 1, 1}, Sz4{nc, nx - 1, ny - 1, nz - 1})
                .reverse(Eigen::array<bool, 4>({false, true, true, true}))
                .conjugate();
  cdata.setZero();
  cdata.slice(Sz4{0, 1, 1, 1}, Sz4{nc, nx - 1, ny - 1, nz - 1}) = rdata;
  log.image(cdata, "vcc-cdata-conj-ks.nii");
  fft.reverse(cdata);
  log.image(cdata, "vcc-cdata-conj.nii");

  Cx3 phase(nx, ny, nz);
  phase.setZero();
  for (Index iz = 1; iz < nz; iz++) {
    for (Index iy = 1; iy < ny; iy++) {
      for (Index ix = 1; ix < nx; ix++) {
        Cx1 const vals = data.chip(iz, 3).chip(iy, 2).chip(ix, 1);
        Cx1 const cvals = cdata.chip(iz, 3).chip(iy, 2).chip(ix, 1).conjugate(); // Dot has a conj
        float const p = std::log(Dot(cvals, vals)).imag() / 2.f;
        phase(ix, iy, iz) = std::polar(1.f, -p);
      }
    }
  }
  log.image(phase, "vcc-correction.nii");
  log.info("Applying Virtual Conjugate Coil phase correction");
  data = data * Tile(phase, nc);
  log.image(data, "vcc-corrected.nii");
}

Cx3 Hammond(Cx4 const &maps, Log &log)
{
  Index const nc = maps.dimension(0);
  Index const nx = maps.dimension(1);
  Index const ny = maps.dimension(2);
  Index const nz = maps.dimension(3);
  log.info("Combining images via the Hammond method");

  Index const refSz = 9;
  Cropper refCrop(Sz3{nx, ny, nz}, Sz3{refSz, refSz, refSz}, log);
  Cx1 const ref =
    refCrop.crop4(maps).sum(Sz3{1, 2, 3}).conjugate() / refCrop.crop4(maps).sum(Sz3{1, 2, 3}).abs();

  using FixedOne = Eigen::type2index<1>;
  Eigen::IndexList<int, FixedOne, FixedOne, FixedOne> rsh;
  rsh.set(0, nc);
  Eigen::IndexList<FixedOne, int, int, int> brd;
  brd.set(1, nx);
  brd.set(2, ny);
  brd.set(3, nz);
  auto const broadcasted = ref.reshape(rsh).broadcast(brd);
  Cx3 const combined = (maps * broadcasted).sum(Sz1{0});
  Cx3 const rss = maps.square().sum(Sz1{0}).sqrt();
  log.image(combined, "hammond-combined.nii");
  log.image(rss, "hammond-rss.nii");

  return combined;
}


// Copyright 2008 Chung-Lin Wen.
// 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 GIL_LAB_H
#define GIL_LAB_H

////////////////////////////////////////////////////////////////////////////////////////
/// \file
/// \brief Support for CIE Lab color space
/// \author Chung-Lin Wen \n
////////////////////////////////////////////////////////////////////////////////////////

#include <boost/cast.hpp>
#include <boost/gil/gil_all.hpp>

namespace boost { namespace gil {

/// \addtogroup ColorNameModel
/// \{
namespace lab_color_space
{
/// \brief Luminance
struct luminance_t {};
/// \brief a Color Component
struct a_color_opponent_t {};
/// \brief b Color Component
struct b_color_opponent_t {};
}
/// \}

/// \ingroup ColorSpaceModel
typedef mpl::vector3< lab_color_space::luminance_t
                    , lab_color_space::a_color_opponent_t
                    , lab_color_space::b_color_opponent_t
                    > lab_t;

/// \ingroup LayoutModel
typedef layout<lab_t> lab_layout_t;

GIL_DEFINE_ALL_TYPEDEFS( 32f, lab );

/// \ingroup ColorConvert
/// \brief RGB to LAB
template <>
struct default_color_converter_impl< rgb_t, lab_t >
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst ) const
   {
      using namespace lab_color_space;

      // only bits32f for lab is supported
      bits32f temp_red   = channel_convert<bits32f>( get_color( src, red_t()   ));
      bits32f temp_green = channel_convert<bits32f>( get_color( src, green_t() ));
      bits32f temp_blue  = channel_convert<bits32f>( get_color( src, blue_t()  ));

      // first, transfer to xyz color space
      bits32f normalized_r = temp_red / 255.f;
      bits32f normalized_g = temp_green / 255.f;
      bits32f normalized_b = temp_blue / 255.f;

      if( normalized_r > 0.04045f )
      {
	  	   normalized_r = pow( (( normalized_r + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_r /= 12.92f;
      }

      if( normalized_g > 0.04045f )
      {
	  	   normalized_g = pow((( normalized_g + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_g /= 12.92f;
      }

      if( normalized_b > 0.04045f )
      {
	  	   normalized_b = pow( (( normalized_b + 0.055f ) / 1.055f ), 2.4f );
      }
	   else
	   {
	  	   normalized_b /= 12.92f;
      }

	  normalized_r *= 100.f;
	  normalized_g *= 100.f;
	  normalized_b *= 100.f;

      bits32f x, y, z;
      x = normalized_r * 0.4124f + normalized_g * 0.3576f + normalized_b * 0.1805f;
      y = normalized_r * 0.2126f + normalized_g * 0.7152f + normalized_b * 0.0722f;
      z = normalized_r * 0.0193f + normalized_g * 0.1192f + normalized_b * 0.9505f;

      // then, transfer to lab color space
      bits32f ref_x = 95.047f;
      bits32f ref_y = 100.000f;
      bits32f ref_z = 108.883f;
      bits32f normalized_x = x / ref_x;
      bits32f normalized_y = y / ref_y;
      bits32f normalized_z = z / ref_z;

      if( normalized_x > 0.008856f )
      {
         normalized_x = pow( normalized_x, 0.333f );
      }
	   else
	   {
	  	   normalized_x = (7.787f * normalized_x) + ( 16.f/116.f );
      }

      if( normalized_y > 0.008856f )
      {
	  	   normalized_y = pow( normalized_y, 0.333f );
      }
	   else
	   {
	  	   normalized_y = (7.787f * normalized_y) + ( 16.f/116.f );
      }

	   if( normalized_z > 0.008856f )
	   {
	  	   normalized_z = pow( normalized_z, 0.333f );
      }
	   else
	   {
	  	   normalized_z = ( 7.787f * normalized_z ) + ( 16.f/116.f );
      }

      bits32f luminance, a_color_opponent, b_color_opponent;
      luminance = ( 116.f * normalized_y ) - 16.f;
      a_color_opponent = 500.f * ( normalized_x - normalized_y );
      b_color_opponent = 200.f * ( normalized_y - normalized_z );

      get_color( dst, luminance_t() ) = luminance;
      get_color( dst, a_color_opponent_t() ) = a_color_opponent;
      get_color( dst, b_color_opponent_t() ) = b_color_opponent;
   }
};

/// \ingroup ColorConvert
/// \brief LAB to RGB
template <>
struct default_color_converter_impl<lab_t,rgb_t>
{
   template <typename P1, typename P2>
   void operator()( const P1& src, P2& dst) const
   {
      using namespace lab_color_space;

      bits32f luminance = get_color( src, luminance_t() );
      bits32f a_color_opponent = get_color( src, a_color_opponent_t() );
      bits32f b_color_opponent = get_color( src, b_color_opponent_t() );

      // first, transfer to xyz color space
      bits32f normalized_y = ( luminance + 16.f ) / 116.f;
      bits32f normalized_x = ( a_color_opponent / 500.f ) + normalized_y;
      bits32f normalized_z = normalized_y - ( b_color_opponent / 200.f );

      if( pow( normalized_y, 3.f ) > 0.008856f )
      {
         normalized_y = pow( normalized_y, 3.f );
      }
      else
      {
         normalized_y = ( normalized_y - 16.f / 116.f ) / 7.787f;
      }

      if( pow( normalized_x, 3.f ) > 0.008856f )
      {
         normalized_x = pow( normalized_x, 3.f );
      }
      else
      {
         normalized_x = ( normalized_x - 16.f / 116.f ) / 7.787f;
      }

      if( pow( normalized_z, 3.f ) > 0.008856f )
      {
         normalized_z = pow( normalized_z, 3.f );
      }
      else
      {
         normalized_z = ( normalized_z - 16.f / 116.f ) / 7.787f;
      }

      bits32f reference_x = 95.047f;
      bits32f reference_y = 100.000f;
      bits32f reference_z = 108.883f;
      bits32f x, y, z;
      x = reference_x * normalized_x;
      y = reference_y * normalized_y;
      z = reference_z * normalized_z;

      // then, transfer to rgb color space
      normalized_x = x / 100.f;
      normalized_y = y / 100.f;
      normalized_z = z / 100.f;

      bits32f result_r = normalized_x *  3.2406f + normalized_y * -1.5372f + normalized_z * -0.4986f;
      bits32f result_g = normalized_x * -0.9689f + normalized_y *  1.8758f + normalized_z *  0.0415f;
      bits32f result_b = normalized_x *  0.0557f + normalized_y * -0.2040f + normalized_z *  1.0570f;

      if( result_r > 0.0031308f )
      {
         result_r = 1.055f * pow( result_r, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_r = 12.92f * result_r;
      }

      if( result_g > 0.0031308f )
      {
         result_g = 1.055f * pow( result_g, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_g = 12.92f * result_g;
      }

      if( result_b > 0.0031308f )
      {
         result_b = 1.055f * pow( result_b, 1.f/2.4f ) - 0.055f;
      }
      else
      {
         result_b = 12.92f * result_b;
      }

      bits32f red, green, blue;
      red   = result_r * 255.f;
      green = result_g * 255.f;
      blue  = result_b * 255.f;

      get_color(dst,red_t())  =
         channel_convert<typename color_element_type<P2, red_t>::type>( red );
      get_color(dst,green_t())=
         channel_convert<typename color_element_type<P2, green_t>::type>( green );
      get_color(dst,blue_t()) =
         channel_convert<typename color_element_type<P2, blue_t>::type>( blue );
   }
};

} }  // namespace boost::gil

#endif // GIL_LAB_H


// Implementation for kc_test
//

#include "stdafx.h"
#include <cmath>
#include "funcs.h"
#include "ck_sdk.h"
#include "KCSdkUtilities.h"
#include "SMask.h"
#include "TestUtil.h"
#include "splines.h"
#include "SplineHelper.h"
#include <fstream>
#include <chrono>
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

const double MIN_TOL = .00005;
using namespace Eigen;
using timer = std::chrono::steady_clock;
using std::chrono::time_point;
using std::chrono::duration_cast;
using std::chrono::milliseconds;
typedef Matrix<double, -1, -1> Points;
int TestSplineLibrary() {
	int status = CKNoError;
	CKPart part = CKGetActivePart();
	if (!part.IsValid()) {
		return CK_NO_PART;
	}
	std::vector<CKSCoord> control_points;   // Holds the 4 control points for entire segment
  std::vector <double> coeffs;
  CKSMatrix worldMat;
  CKSEntityArray curves = CurvesSelect(part);
  status = GetCoeffFromCurves(part, curves, coeffs, .0000001);
  CKSEntity spline = part.AddSpline(true, false, coeffs, NULL, &worldMat);
  //CKSEntity spline = SplineSelect(part);
  if (!spline.IsValid()) {
		MessageBox(nullptr, _T("Error with spline selection"), _T("Spline Data"), MB_OK_STOP);
		status = CKError;
	}
	else {
		bool is3D = false;
		bool isClosed = false;

    HPMatrix splineMatrix;
		CKSCoordArray nodePoints;
		CKSCoord startVector, endVector;
		part.GetSpline(spline, NULL, coeffs, is3D, isClosed, NULL, &splineMatrix);
		part.GetSpline(spline, NULL, nodePoints, &startVector, &endVector, is3D, isClosed, NULL, &splineMatrix);
		size_t blockSize = 0;
		if (is3D) {
			blockSize = 12;
		}
		else {
			blockSize = 8;
		}
		double param[3][4] = { 0.0 };   // Array to hold current segment coefficients
		for (size_t i = 0; i < coeffs.size(); i += blockSize) {
			if (is3D) {
				for (size_t j = 0; j < 3; ++j) {
					for (size_t k = 0; k < 4; ++k) {
						param[j][k] = coeffs[i + (j * 4) + k];
					}
				}
			}
			else {  // 2D Spline
				for (size_t j = 0; j < 2; ++j) {
					for (size_t k = 0; k < 4; ++k) {
						param[j][k] = coeffs[i + (j * 4) + k];
					}
				}
			}
			GetSplineControlPoints(param, control_points);
		}

		//// Test old normal function using 2nd derivative
		//CKSCoord old_normal;
		//CKSCoord old_tangent;
		//CKSCoord old_position;

		//CKSMath::Evaluate(spline, NULL, false, false, false, 1, .0625, &splineMatrix, &old_position,
		//	&old_normal, &old_tangent);
		////GetSplineNormal(coeffs, 1, .5, old_normal);
		//if (old_normal.Magnitude() > .001) {
		//	old_normal.Normalize();
		//}
		//if (old_tangent.Magnitude() > .001) {
		//	old_tangent.Normalize();
		//}

		////GetSplineCoord(coeffs, 1, .5, old_position);
		//old_normal = old_position + old_normal;
		//old_tangent = old_position + old_tangent;
		//part.AddPoint(old_position);
		//part.AddPoint(old_normal);
		//part.AddPoint(old_tangent);
		//part.NoteState();

		//// Test 2D spline points
		//CKSCoord2D cp_2d;
		//std::vector<CKSCoord2D> points_2d;
		//for (auto i : control_points) {
		//	cp_2d.m_dX = i.m_dX;
		//	cp_2d.m_dY = i.m_dY;
		//	points_2d.push_back(cp_2d);
		//}
		const size_t degree = 3;

		//// Test elevate degree 2d. Works
		//std::vector<CKSCoord2D> elevated_points = bezier::ElevateDegree<double>(points_2d, bezier::Dimension::k2d, degree);
		//for (auto i: elevated_points) {
		//	part.AddPoint(i.m_dX, i.m_dY, 0.0);
		//}
		//part.NoteState();

		//// Test segment split 2d. Works.
		//std::vector<CKSCoord2D> split_points;
		//for (size_t i = 0; i < points_2d.size() / (degree + 1); ++i) {
		//	split_points = bezier::SplitSegment<double>(points_2d, .5, i, degree, 2);
		//	for (auto j : split_points) {
		//		part.AddPoint(j.m_dX, j.m_dY, 0.0);
		//	}
		//}
		//part.NoteState();

		//// Create spline from calculated coefficients 2d
		//for (size_t i = 0; i < 2; ++i) {
		//	std::vector<double> new_coeff = bezier::GetCoefficients<double>(split_points, i, degree, bezier::k2d);
		//	part.AddSpline(false, false, kc_coeff);
		//}
		//part.NoteState();

		//// Test tangent and normal functions 2d. Works
		//CKSCoord2D coordinate;
		//CKSCoord2D tangent;
		//CKSCoord2D normal;
		//for (size_t i = 0; i < points_2d.size() / 4; ++i) {
  //    for (size_t j = 0; j < 4; ++j) {
  //      double t = j / 4.0;
  //      coordinate = bezier::GetPosition<double>(points_2d, t, i, degree, bezier::k2d);
  //      part.AddPoint(coordinate.m_dX, coordinate.m_dY, 0.0);
  //      tangent = bezier::GetFirstDerivative<double>(points_2d, t, i, degree, bezier::k2d);
  //      normal = bezier::GetNormal<double>(points_2d, t, i, degree, bezier::k2d);
  //      CKSMatrix temp;
  //      CKSCoord v1(coordinate.m_dX, coordinate.m_dY, 0.0);
  //      CKSCoord v2(tangent.m_dX, tangent.m_dY, 0.0);
  //      CKSCoord v3(normal.m_dX, normal.m_dY, 0.0);
  //      CKSMath::MatrixVector(v1, v1 + v2, temp);
  //      CKEntityAttrib attrib;
  //      attrib.m_ucColorNumber = 7;
  //      part.AddVector(.25, &temp, &attrib);
  //      attrib.m_ucColorNumber = 10;
  //      CKSMath::MatrixVector(v1, v1 + v3, temp);
  //      part.AddVector(.25, &temp, &attrib);
  //    }
		//}
		//part.NoteState();

		//// Test elevate degree 3d. Works
		//CKSCoordArray elevated_points = bezier::ElevateDegree<double>(control_points, bezier::Dimension::k3d, 3);
		//for (size_t i = 0; i < elevated_points.size(); ++i) {
		//	part.AddPoint(elevated_points[i]);
		//}
		//part.NoteState();

		// Test segment split 3d. Works.
		//const size_t degree = 3;
		//std::vector<CKSCoord> split_points;
		//for (size_t i = 0; i < control_points.size() / (degree + 1); ++i) {
		//	split_points = bezier::SplitSegment<double>(control_points, .5, i, degree, 3);
		//	for (auto j : split_points) {
		//		part.AddPoint(j);
		//	}
		//}
		//part.NoteState();

		// Create spline from calculated coefficients 3d. Works
		//for (size_t i = 0; i < 2; ++i) {
		//	std::vector<double> new_coeff = bezier::GetCoefficients<double>(split_points, i);
		//	part.AddSpline(true, false, kc_coeff);
		//}
		//part.NoteState();

		//// Test tangent and normal functions 3d. Works
		//CKSCoord coordinate;
		//CKSCoord tangent;
		//CKSCoord normal;
		//CKSCoord curvature;
		//for (size_t i = 0; i < control_points.size() / 4; ++i) {
		//	for (auto j = 0; j < 5; ++j) {
		//		double t = j / 4.0;
		//		coordinate = bezier::GetPosition<double>(control_points, t, i, 3, bezier::k3d);
		//		part.AddPoint(coordinate);
		//		tangent = bezier::GetFirstDerivative<double>(control_points, t, i, 3, bezier::k3d);
		//		normal = bezier::GetNormal<double>(control_points, t, i, 3, bezier::k3d);
		//		curvature = bezier::GetSecondDerivative<double>(control_points, t, i, 3, bezier::k3d);
		//		tangent.Normalize();
		//		normal.Normalize();
		//		curvature.Normalize();
		//		CKSMatrix temp;
		//		CKSMath::MatrixVector(coordinate, coordinate + tangent, temp);
		//		CKEntityAttrib attrib;
		//		attrib.m_ucColorNumber = 7;
		//		part.AddVector(1.0, &temp, &attrib);
		//		attrib.m_ucColorNumber = 10;
		//		CKSMath::MatrixVector(coordinate, coordinate + normal, temp);
		//		part.AddVector(1.0, &temp, &attrib);
		//		//attrib.m_ucColorNumber = 2;
		//		//CKSMath::MatrixVector(coordinate, coordinate + curvature, temp);
		//		//part.AddVector(1.0, &temp, &attrib);
		//	}
		//}
    //part.DeleteEntity(spline);
    //part.NoteState();

    coeffs.clear();
    coeffs = bezier::GetCoefficients<double>(control_points);
    size_t coeffs_size = coeffs.size();
    size_t control_point_set = degree + 1;
    size_t segment_size = control_point_set * 3;
    size_t segment_count = coeffs_size / segment_size;
    std::vector<double> quad_coeffs(3);
    std::vector<double> linear_coeffs(2);
    std::vector<double> t_values;
    for (size_t i = 0; i < segment_count; ++i) {
      for (size_t j = 0; j < 3; ++j) {
        for (size_t k = 0; k < 3; ++k) {
          quad_coeffs[k] = coeffs[(i * segment_size) + (j * 4) + k] * (3 - k);
          if (k < 2) {
            linear_coeffs[k] = quad_coeffs[k] * (2 - k);
          }
        }
        bezier::SolveQuadratic(quad_coeffs, t_values);
        t_values.push_back(bezier::SolveLinear(linear_coeffs));
      }
    }
    //auto t_end = std::remove_if(t_values.begin(), t_values.end(), [](double a) {
    //  return (a <= 0.0 || a >= 1.0);
    //});
    //t_values.erase(t_end, t_values.end());
    t_values.push_back(0.0);
    t_values.push_back(1.0);
    for (auto it : t_values) {
      if (it >= 0.0 && it <= 1.0) {
        CKSCoord position = bezier::GetPosition(control_points, it);
        part.AddPoint(position);
      }
    }
    part.NoteState();
    WriteData("nodes.dat", nodePoints);
		WriteCoefficients("coeff.dat", coeffs);
    WriteCoefficients("derivatives.dat", t_values);
		WriteControlPoints("ctrl.dat", control_points);
		CKSCoordArray fitPoints;
		SplineToPoints(part, spline, fitPoints);
		WriteData("spline.dat", fitPoints);
		fitPoints.clear();
	}
	return status;
}

int SplineHelix()
{
  CWnd* pWnd = AfxGetMainWnd();
  int status = CKNoError;
  CKPart part = CKGetActivePart();
  if (!part.IsValid())
  {
    return CK_NO_PART;
  }

  CKSMatrix worldMat;
  CKSMask mask;
  mask.AddEntity(CKMaskLine);
  mask.AddEntity(CKMaskArc);
  mask.AddEntity(CKMaskSpline);
  mask.AddEntity(CKMaskNURBSpline);
  mask.AddEntity(CKMaskPolyline);
  mask.AddEntity(CKMaskEllipse);
  mask.AddEntity(CKMaskParabola);
  mask.AddEntity(CKMaskHyperbola);

  // Test create helical spline along a 3D curve
  Events keyCheck;
  double diameter = 1.0;
  double pitch = 0.25;
  CRegistry reg;
  if (reg.KeyExists(_T("Software\\HPM\\HPMTools\\SplineHelix")))
  {
    reg.SetKey(_T("Software\\HPM\\HPMTools\\SplineHelix"), FALSE);
    diameter = reg.ReadFloat(_T("Diameter"), 1.0);
    pitch = reg.ReadFloat(_T("Pitch"), .25);
  }
  else
  {
    reg.CreateKey(_T("Software\\HPM\\HPMTools\\SplineHelix"));
    reg.SetKey(_T("Software\\HPM\\HPMTools\\SplineHelix"), FALSE);
    reg.WriteFloat(_T("Diameter"), 1.0);
    reg.WriteFloat(_T("Pitch"), .25);
  }
  int step = 0;
  while (true)
  {
    switch (step)
    {
    case 0:
    {
      keyCheck = ck_get_input(_T("Enter diameter: "), _T(""), diameter, true, 0, CKS::GreaterThan, 0.0);
      switch (keyCheck)
      {
      case CKBackup:
      case CKEscape:
        return keyCheck;
      case CKNoError:
      {
        reg.WriteFloat(_T("Diameter"), diameter);
        step++;
        break;
      }
      default:
        return keyCheck;
      }
    }
    case 1:
    {
      keyCheck = ck_get_input(_T("Enter pitch: "), _T(""), pitch);
      switch (keyCheck)
      {
      case CKBackup:
      {
        step--;
        continue;
      }
      case CKEscape:
        return keyCheck;
      case CKNoError:
      {
        reg.WriteFloat(_T("Pitch"), pitch);
        step++;
        break;
      }
      default:
        return keyCheck;
      }
      if (CKSMath::CompareToZero(pitch, .01) <= 0)
      {
        pWnd->MessageBox(_T("The pitch entered is too small"), MB_TITLE, MB_OK_INFO);
        step--;
        continue;
      }
    }
    case 2:
    {
      CKSEntityArray driveCurves;
      status = part.GenSel(_T("Select the sweep path chain of curves"), driveCurves);
      switch (status)
      {
      case CKNoError:
        break;
      case CKBackup:
      {
        step--;
        continue;
      }
      default:
        if ((status < CKMenu1) || (status >= CKEscape))
          return status;
        //case CKEscape:
        //case CK_NO_PART:
        //  return status;
      }
      CKSCoordArray helixPnts;
      CKSCoord startVec, endVec;
      status = GetHelicalSplinePoints(part, driveCurves, helixPnts, startVec, endVec, diameter, pitch, .0001);
      if (helixPnts.size())
      {
        std::ofstream out_file("helix.points");
        for (auto p : helixPnts) {
          out_file << p.m_dX << '\t' << p.m_dY << '\t' << p.m_dZ << '\n';
        }
        time_point<timer> start_time;
        time_point<timer> end_time;
        milliseconds elapsed;
        CString eigen_time;
        CString kc_time;
        CKSEntity helicalSpline;
        CKEntityAttrib attrib;

        start_time = timer::now();
        size_t point_count = helixPnts.size();
        Points points(3, point_count);
        for (size_t i = 0; i < point_count; ++i) {
          points(0, i) = helixPnts[i][0];
          points(1, i) = helixPnts[i][1];
          points(2, i) = helixPnts[i][2];
        }
        Spline3d testspline = SplineFitting<Spline3d>::Interpolate(points, 3);
        size_t knots_size = testspline.knots().size();
        std::vector<double> knots(knots_size);
        for (size_t j = 0; j < knots_size; ++j) {
          knots[j] = testspline.knots().data()[j];
        }
        Matrix<double, -1, -1> control_points = testspline.ctrls();
        size_t ctrl_size = control_points.cols();
        std::vector<CKSCoord> ctrl_points(ctrl_size);
        for (size_t j = 0; j < ctrl_size; ++j) {
          ctrl_points[j].m_dX = control_points(0, j);
          ctrl_points[j].m_dY = control_points(1, j);
          ctrl_points[j].m_dZ = control_points(2, j);
        }
        attrib.m_ucColorNumber = 7;
        std::vector<double> weights(ctrl_size, 1.0);
        helicalSpline = part.AddNURBSpline(3, true, false, knots, ctrl_points, weights, &attrib);
        end_time = timer::now();
        elapsed = duration_cast<milliseconds>(end_time - start_time);
        eigen_time.Format(_T("Eigen Spline creation time: %d ms\n"), elapsed.count());
        part.NoteState();
        helicalSpline = part.AddSpline(true, false, true, true, startVec, endVec, helixPnts, NULL, &worldMat);
        part.NoteState();
        //helicalSpline = part.AddSpline(true, false, false, false, startVec, endVec, helixPnts, NULL, &worldMat);
        //start_time = timer::now();
        //attrib.m_ucColorNumber = 9;
        //helicalSpline = part.AddNURBSpline(3, true, false, helixPnts, &attrib, &worldMat);
        //end_time = timer::now();
        //elapsed = duration_cast<milliseconds>(end_time - start_time);
        //kc_time.Format(_T("KC spline time: %d ms\n"), elapsed.count());
        //part.NoteState();
        if (!helicalSpline.IsValid()) {
          pWnd->MessageBox(_T("Error creating helical spline"), MB_TITLE, MB_OK_STOP);
          return CKError;
        }
        CString all_time = eigen_time + kc_time;
        //CString all_time = eigen_time;
        pWnd->MessageBox(all_time, MB_TITLE, MB_OK_STOP);
      }
    }
    }
  }
  return status;
}


/**
This file is part of Deformable Shape Tracking (DEST).

Copyright(C) 2015/2016 Christoph Heindl
All rights reserved.

This software may be modified and distributed under the terms
of the BSD license.See the LICENSE file for details.
*/

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include <dest/core/shape.h>
#include <Eigen/Geometry>

TEST_CASE("similarity-transform-translate")
{
    dest::core::Shape to(2, 4);
    to << 0.f, 2.f, 2.f, 0.f,
          0.f, 0.f, 2.f, 2.f;

    Eigen::AffineCompact2f t;
    t = Eigen::Translation2f(1.f, 1.f);

    dest::core::Shape from = t.matrix() * to.colwise().homogeneous();

    Eigen::AffineCompact3f s = dest::core::estimateSimilarityTransform(from, to);

    Eigen::AffineCompact2f expected;
    expected = Eigen::Translation2f(-1.f, -1.f);

    REQUIRE(s.isApprox(expected));

}

TEST_CASE("similarity-transform-compound")
{
    dest::core::Shape to(2, 4);
    to << 0.f, 2.f, 2.f, 0.f,
    0.f, 0.f, 2.f, 2.f;

    Eigen::AffineCompact2f t;
    t = Eigen::Translation2f(1.f, 1.f) * Eigen::Rotation2Df(0.17f) * Eigen::Scaling(1.8f);

    dest::core::Shape from = t.matrix() * to.colwise().homogeneous();

    Eigen::AffineCompact3f s = dest::core::estimateSimilarityTransform(from, to);

    Eigen::AffineCompact2f expected = t.inverse();

    REQUIRE(s.isApprox(expected));
}

TEST_CASE("similarity-transform-between-rects")
{
    dest::core::Rect r = dest::core::createRectangle(Eigen::Vector2f(-2.f, -2.f), Eigen::Vector2f(2.f, 2.f));

    Eigen::AffineCompact2f t;
    t = Eigen::Rotation2Df(0.17f);

    r = t.matrix() * r.colwise().homogeneous();

    dest::core::Rect n = dest::core::unitRectangle();
    Eigen::AffineCompact3f s = dest::core::estimateSimilarityTransform(r, n);

    r = s.matrix() * r.colwise().homogeneous();
    REQUIRE(r.isApprox(n));
}


#include <blitzml/sparse_linear/lasso_solver.h>

#include <blitzml/base/math_util.h>
#include <blitzml/base/vector_util.h>
#include <blitzml/base/subproblem_controller.h>

using std::vector;

namespace BlitzML {

value_t LassoSolver::compute_dual_obj() const {
  value_t loss = 0.5 * l2_norm_sq(x);
  return -(loss + l1_penalty * l1_norm(omega));
}

value_t LassoSolver::compute_primal_obj_x() const {
  value_t ip = inner_product(data->b_values(), &x[0], num_examples);
  return 0.5 * sq(kappa_x) * l2_norm_sq(x) + kappa_x * ip;
}

value_t LassoSolver::compute_primal_obj_y() const {
  value_t ip = inner_product(data->b_values(), &y[0], num_examples);
  return 0.5 * l2_norm_sq(y) + ip;
}

value_t LassoSolver::update_coordinates_in_working_set() {
  value_t ret = 0.;
  for (const_index_itr i = ws.begin(); i != ws.end(); ++i) {
    value_t subgrad = update_feature_lasso(*i);
    ret += sq(subgrad);
  }
  ws.shuffle();
  return ret;
}

inline value_t LassoSolver::update_feature_lasso(index_t i) {
  value_t inv_L = inv_lipschitz_cache[i];
  if (inv_L < 0) {
    return 0.;
  }

  const Column& col = *A_cols[i];
  value_t current_value = omega[i];
  value_t grad = col.inner_product(x)
                          + num_examples * Delta_bias * col_means_cache[i];
  if (current_value == 0. && fabs(grad) < l1_penalty) {
    return 0.;
  }

  value_t pre_shrink = current_value - grad * inv_L;
  value_t new_value = soft_threshold(pre_shrink, l1_penalty * inv_L);
  value_t delta = new_value - current_value;
  if (delta == 0.) {
    return 0.;
  }
  col.add_multiple(x, delta);
  omega[i] = new_value;
  if (use_bias) {
    Delta_bias -= col_means_cache[i] * delta;
  }

  return grad + sign(current_value) * l1_penalty;
}

void LassoSolver::update_bias(int max_newton_itr) {
  if (!use_bias) {
    return;
  }
  value_t grad = sum_vector(x);
  value_t delta = -grad / x.size();
  bias += delta;
  add_scalar_to_vector(x, delta);
}

void LassoSolver::perform_backtracking() {
  if (use_bias) {
    add_scalar_to_vector(x, Delta_bias);
    bias += Delta_bias;
  }
}

void LassoSolver::setup_proximal_newton_problem() {
  Delta_bias = 0.;
}

}


#include <boost/math/distributions/hypergeometric.hpp>