//==============================================================================
//         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
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNINV_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_LOGNINV_HPP_INCLUDED
#include <nt2/statistics/functions/logninv.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/sqrt_2.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/globalall.hpp>
#include <nt2/include/functions/simd/erfcinv.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/is_nltz.hpp>
#include <nt2/include/functions/norminv.hpp>
#include <nt2/include/functions/sqrt.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      return  exp(-Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)(A1)
                            , (generic_<floating_<A0> > )
                              (generic_<floating_<A1> >)
                             )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {

      return  exp(-Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*a0)+a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv0_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A1> >)
                              (generic_< floating_<A2> >)
                            )
  {
    typedef A0 result_type;

    NT2_FUNCTOR_CALL(3)
    {
      BOOST_ASSERT_MSG(nt2::globalall(nt2::is_gtz(a2)), "sigma(s) must be positive");
      return  exp(-Sqrt_2<A0>()*a2*erfcinv( nt2::Two<A0>()*a0)+a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( logninv_, tag::cpu_
                              , (A0)(N0)(A1)(N1)
                              , ((node_<A0, nt2::tag::logninv_, N0, nt2::container::domain>))
                                ((node_<A1, nt2::tag::tie_ , N1, nt2::container::domain>))
                            )
  {
    typedef void                                                    result_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type         Out0;
    typedef typename boost::proto::result_of::child_c<A0&,0>::type          In0;
    typedef typename boost::proto::result_of::child_c<A0&,1>::type          In1;
    typedef typename boost::proto::result_of::child_c<A0&,2>::type          In2;
    typedef typename A0::value_type                                  value_type;
    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      doit(a0, a1, N0(), N1());
    }
    ////////////////////////////////////////////
    // No enough inputs to computes all ouputs
    ////////////////////////////////////////////
    BOOST_FORCEINLINE static void doit(const A0&, A1&,
                                       boost::mpl::long_<1> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");
    }
    BOOST_FORCEINLINE static void doit(const A0& a0,  A1& a1,
                                       boost::mpl::long_<2> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0));
    }
    BOOST_FORCEINLINE static void doit(const A0&,  A1&,
                                       boost::mpl::long_<3> const &, boost::mpl::long_<3> const & )
    {
      BOOST_ASSERT_MSG(false, "Must provide parameter variance to compute confidence bounds.");

    }
    ////////////////////////////////////////////
    // No enough output to computes all ouputs
    ////////////////////////////////////////////
    template < class T >
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<4> const &, T const & )
    {
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0),
                                                   boost::proto::child_c<2>(a0));
    }
    template < class T >
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<5> const &, T const & )
    {
      boost::proto::child_c<0>(a1) =  nt2::logninv(boost::proto::child_c<0>(a0),
                                                   boost::proto::child_c<1>(a0),
                                                   boost::proto::child_c<2>(a0));
    }
    ////////////////////////////////////////////
    // Regular cases
    ////////////////////////////////////////////
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<4> const &, boost::mpl::long_<3> const & )
    {
      conf_bounds(a0, a1, value_type(0.05));
    }
    BOOST_FORCEINLINE static void doit(const A0& a0, A1& a1,
                                       boost::mpl::long_<5> const &, boost::mpl::long_<3> const & )
    {
      conf_bounds(a0, a1,boost::proto::child_c<4>(a0));
    }

    BOOST_FORCEINLINE static void conf_bounds(const A0& a0, A1& a1,
                                              const value_type& alpha )
    {
      typedef nt2::memory::container<tag::table_, value_type, nt2::_2D>  semantic;
      NT2_AS_TERMINAL_IN(semantic, pcov, boost::proto::child_c<3>(a0));
      const In0& p  = boost::proto::child_c<0>(a0);
      const In1& mu = boost::proto::child_c<1>(a0);
      const In2& sigma = boost::proto::child_c<2>(a0);
      auto logx0 = -Sqrt_2<A0>()*erfcinv( nt2::Two<A0>()*p);
      auto xvar =   fma(fma(pcov(2,2), logx0, Two<value_type>()*pcov(1,2)), logx0, pcov(1,1));
      BOOST_ASSERT_MSG(nt2::globalall(nt2::is_nltz(xvar)), "Covariance matrix must be positive");
      value_type normz = -nt2::norminv(alpha*nt2::Half<value_type>());
      auto halfwidth = normz*nt2::sqrt(xvar);
      boost::proto::child_c<0>(a1) = exp(fma(sigma, logx0, mu));
      auto coef = exp(-halfwidth);
      boost::proto::child_c<1>(a1) = boost::proto::child_c<0>(a1)*coef;
      boost::proto::child_c<2>(a1) = boost::proto::child_c<0>(a1)/coef;
    }

  };

} }

#endif


////////////////////////////////////////////////////////////////////////////////
#include <polyfem/RBFWithLinear.hpp>
#include <polyfem/Types.hpp>
#include <polyfem/Logger.hpp>

#include <igl/Timer.h>

#include <Eigen/Dense>

#include <iostream>
#include <fstream>
#include <array>
////////////////////////////////////////////////////////////////////////////////

using namespace polyfem;

namespace {

// Harmonic kernel
double kernel(const bool is_volume, const double r) {
	if (r < 1e-8) { return 0; }

	if (is_volume) {
		return 1/r;
	} else {
		return log(r);
	}
}

double kernel_prime(const bool is_volume, const double r) {
	if (r < 1e-8) { return 0; }

	if(is_volume) {
		return -1/(r*r);
	} else {
		return 1/r;
	}
}

} // anonymous namespace

////////////////////////////////////////////////////////////////////////////////

RBFWithLinear::RBFWithLinear(
		const Eigen::MatrixXd &centers,
		const Eigen::MatrixXd &samples,
		const Eigen::MatrixXd &local_basis_integral,
		const Quadrature &quadr,
		Eigen::MatrixXd &rhs,
		bool with_constraints)
	: centers_(centers)
{
	compute_weights(samples, local_basis_integral, quadr, rhs, with_constraints);
}

// -----------------------------------------------------------------------------

void RBFWithLinear::basis(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	Eigen::MatrixXd tmp;
	bases_values(samples, tmp);
	val = tmp.col(local_index);
}

// -----------------------------------------------------------------------------

void RBFWithLinear::grad(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	Eigen::MatrixXd tmp;
	const int dim = centers_.cols();
	val.resize(samples.rows(), dim);
	for (int d = 0; d < dim; ++d) {
		bases_grads(d, samples, tmp);
		val.col(d) = tmp.col(local_index);
	}
}

////////////////////////////////////////////////////////////////////////////////

void RBFWithLinear::bases_values(const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	// Multiply by the weights
	val = A * weights_;
}

// -----------------------------------------------------------------------------

void RBFWithLinear::bases_grads(const int axis, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const {
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();

	// Compute ∇xA
	Eigen::MatrixXd A_prime(samples.rows(), num_kernels + 1 + dim);
	A_prime.setZero();

	for (int j = 0; j < num_kernels; ++j) {
		A_prime.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x)
			{ return kernel_prime(is_volume(), x) / x; });
		A_prime.col(j) = (samples.col(axis).array() - centers_(j, axis)) * A_prime.col(j).array();
	}
	// Linear terms
	A_prime.middleCols(num_kernels + 1 + axis, 1).setOnes();

	// Apply weights
	val = A_prime * weights_;
}

////////////////////////////////////////////////////////////////////////////////

void RBFWithLinear::compute_kernels_matrix(const Eigen::MatrixXd &samples, Eigen::MatrixXd &A) const {
	// Compute A
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();

	A.resize(samples.rows(), num_kernels + 1 + dim);
	for (int j = 0; j < num_kernels; ++j) {
		A.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x)
			{ return kernel(is_volume(), x); });
	}
	A.col(num_kernels).setOnes(); // constant term
	A.rightCols(dim) = samples; // linear terms
}

// -----------------------------------------------------------------------------

void RBFWithLinear::compute_constraints_matrix(
	const int num_bases,
	const Quadrature &quadr,
	const Eigen::MatrixXd &local_basis_integral,
	Eigen::MatrixXd &L,
	Eigen::MatrixXd &t) const
{
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();

	// Compute KI
	Eigen::MatrixXd KI(num_kernels, dim);
	for (int j = 0; j < num_kernels; ++j) {
		// ∫∇x(φj)(p) = Σ_q (xq - xk) * 1/r * h'(r) * wq
		// - xq is the x coordinate of the q-th quadrature point
		// - wq is the q-th quadrature weight
		// - r is the distance from pq to the kernel center
		// - h is the RBFWithLinear RBF kernel (scalar function)
		const Eigen::MatrixXd drdp = quadr.points.rowwise() - centers_.row(j);
		const Eigen::VectorXd r = drdp.rowwise().norm();
		KI.row(j) = (drdp.array().colwise() * (quadr.weights.array() * r.unaryExpr([this](double x)
			{ return kernel_prime(is_volume(), x); }).array() / r.array())).colwise().sum();
	}
	KI /= quadr.weights.sum();

	// Compute L
	L.resize(num_kernels + dim + 1, num_kernels + 1);
	L.setZero();
	L.diagonal().setOnes();
	L.block(num_kernels + 1, 0, dim, num_kernels) = -KI.transpose();
	// std::cout << L.bottomRightCorner(10, 10) << std::endl;

	// Compute t
	t.resize(num_kernels + 1 + dim, num_bases);
	t.setZero();
	t.bottomRows(dim) = local_basis_integral.transpose().topRows(dim) / quadr.weights.sum();
}

// -----------------------------------------------------------------------------

void RBFWithLinear::compute_weights(const Eigen::MatrixXd &samples,
	const Eigen::MatrixXd &local_basis_integral, const Quadrature &quadr,
	Eigen::MatrixXd &rhs, bool with_constraints)
{
	logger().trace("#kernel centers: {}", centers_.rows());
	logger().trace("#collocation points: {}", samples.rows());
	logger().trace("#quadrature points: {}", quadr.weights.size());
	logger().trace("#non-vanishing bases: {}", rhs.cols());

	if (!with_constraints) {
		// Compute A
		Eigen::MatrixXd A;
		compute_kernels_matrix(samples, A);

		// Solve the system
		const int num_kernels = centers_.rows();
		logger().trace("-- Solving system of size {}x{}", num_kernels, num_kernels);
		weights_ = (A.transpose() * A).ldlt().solve(A.transpose() * rhs);
		logger().trace("-- Solved!");

		return;
	}

	// For each basis function f that is nonzero on the element E, we want to
	// solve the least square system A w = rhs, where:
	//     ┏                    ┓
	//     ┃ φj(pi) ... 1 xi yi ┃
	// A = ┃   ┊        ┊  ┊  ┊ ┃ ∊ ℝ^{#S x (#K+1+dim)}
	//     ┃   ┊        ┊  ┊  ┊ ┃
	//     ┗                    ┛
	//     ┏                    ┓^⊤
	// w = ┃ wj ... a00 a10 a01 ┃   ∊ ℝ^{#K+1+dim}
	//     ┗                    ┛
	// - A is the RBF kernels evaluated over the collocation points (#S)
	// - b is the expected value of the basis sampled on the boundary (#S)
	// - w is the weight of the kernels defining the basis
	// - pi = (xi, yi) is the i-th collocation point
	//
	// Moreover, we want to impose a constraint on the gradients of the kernels
	// so that the integral of the gradients over the polytope must be equal to
	// the value specified in the argument `local_basis_integral` (#K)
	//
	// Let `lb` be the precomputed expected value of ∫f over the rest of the mesh.
	// We write down the constraint as:
	//
	// ∫_{p ∊ E} Σ_j wj ∇x(φj)(p) + ∇x(a^⊤·p + c) dp = lb       (1)
	// (1) ⇔ ∫_{p ∊ E} Σ_j wj ∇x(φj)(p) + ax dp = lb
	//     ⇔ lb - Σ_j wj ∫_{p ∊ E} ∇x(φj)(p) dp = ax Vol(E)
	//
	// We now have a relationship w = Lv + t, where the weights (and esp. the
	// linear terms in the weight vector w), are expressed as an affine
	// combination of unknowns v = [wj ... c] ∊ ℝ^{#K+1} and a translation t
	//
	// After solving the new least square system A L v = rhs - A t, we can retrieve
	// w = L v

	//
	//     ┏                      ┓^⊤
	// t = ┃ 0  ┈  ┈  0 0 lbx lby ┃   / Vol(E) ∊ ℝ^{#K+1+dim}
	//     ┗                      ┛
	//
	//     ┏                  ┓
	//     ┃   1              ┃
	//     ┃       1          ┃
	//     ┃          ·       ┃
	// L = ┃             ·    ┃ ∊ ℝ^{ (#K+1+dim) x (#K+1}) }
	//     ┃                1 ┃
	//     ┃ Lx_j  ┈        0 ┃
	//     ┃ Ly_j  ┈        0 ┃
	//     ┗                  ┛
	// Where Lx_j = -∫∇xφj / Vol(E) = -∫_{p ∊ E} ∇x(φj)(p) / Vol(E) is integrated numerically
	//

	const int num_bases = rhs.cols();

	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	// Compute L and t
	// Note that t is stored into `weights_` for memory efficiency reasons
	Eigen::MatrixXd L;
	compute_constraints_matrix(num_bases, quadr, local_basis_integral, L, weights_);

	// Compute b = rhs - A t
	rhs -= A * weights_;

	// Solve the system
	logger().trace("-- Solving system of size {}x{}", L.cols(), L.cols());
	weights_ += L * (L.transpose() * A.transpose() * A * L).ldlt().solve(L.transpose() * A.transpose() * rhs);
	logger().trace("-- Solved!");

	// std::cout << weights_.bottomRows(10) << std::endl;

	// Eigen::MatrixXd M, x, dx;
	// grad(0, quadr.points, M);
	// for (int d = 0; d < dim; ++d) {
	// 	basis(0, quadr.points, x);
	// 	auto asd = quadr.points;
	// 	asd.col(d).array() += 1e-7;
	// 	basis(0, asd, dx);
	// 	std::cout << (dx - x) / 1e-7 - M.col(d) << std::endl;
	// 	std::cout << (M.col(d).array() * quadr.weights.array()).sum() - local_basis_integral(0, d) << std::endl;
	// }
}


/**
 *  Copyright (C) 2012
 *    Ekaterina Potapova
 *    Automation and Control Institute
 *    Vienna University of Technology
 *    Gusshausstraße 25-29
 *    1040 Vienna, Austria
 *    potapova(at)acin.tuwien.ac.at
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/
 */

#include "v4r/attention_segmentation/PCA.h"
#include <Eigen/Dense>

namespace v4r
{

Eigen::Vector4f getMean(pcl::PointCloud<pcl::Normal>::ConstPtr cloud)
{
  Eigen::Vector4f mean;
  mean.setZero();

  for (unsigned int pi=0; pi<cloud->size(); pi++)
  {
    mean[0] += cloud->points.at(pi).normal[0];
    mean[1] += cloud->points.at(pi).normal[1];
    mean[2] += cloud->points.at(pi).normal[2];
  }

  mean /= (float)cloud->size();

  return(mean);

}

bool computeCovarianceMatrix(pcl::PointCloud<pcl::Normal>::ConstPtr cloud, const Eigen::Vector4f &mean, Eigen::Matrix3f &cov)
{
  bool done = false;
  cov.setZero ();

  for (unsigned pi = 0; pi < cloud->size (); ++pi)
  {
    float x = cloud->points.at(pi).normal[0] - mean[0];
    float y = cloud->points.at(pi).normal[1] - mean[1];
    float z = cloud->points.at(pi).normal[2] - mean[2];

    cov(0,0) += x*x;
    cov(0,1) += x*y;
    cov(0,2) += x*z;

    cov(1,0) += y*x;
    cov(1,1) += y*y;
    cov(1,2) += y*z;

    cov(2,0) += z*x;
    cov(2,1) += z*y;
    cov(2,2) += z*z;

    done = true;
  }

  return(done);
}

void principleAxis(pcl::PointCloud<pcl::Normal>::ConstPtr cloud, std::vector<pcl::Normal> &axis)
{
  Eigen::Vector4f mean;
  EIGEN_ALIGN16 Eigen::Matrix3f cov;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;

  axis.clear();
  axis.resize(3);

  mean = getMean(cloud);

  if(computeCovarianceMatrix(cloud,mean,cov))
  {
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(cov);
    if (eigensolver.info() != Eigen::Success)
      return;

    eigen_values = eigensolver.eigenvalues();
    eigen_vectors = eigensolver.eigenvectors();

    //pcl::eigen33 (cov, eigen_vectors, eigen_values);

    //std::cerr << "inside principleAxis" << std::endl;

    axis.at(0).normal[0] = eigen_vectors (0,0);
    axis.at(0).normal[1] = eigen_vectors (1,0);
    axis.at(0).normal[2] = eigen_vectors (2,0);

    axis.at(1).normal[0] = eigen_vectors (0,1);
    axis.at(1).normal[1] = eigen_vectors (1,1);
    axis.at(1).normal[2] = eigen_vectors (2,1);

    axis.at(2).normal[0] = eigen_vectors (0,2);
    axis.at(2).normal[1] = eigen_vectors (1,2);
    axis.at(2).normal[2] = eigen_vectors (2,2);
  }

}

} //namespace v4r


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/simplicial_map.h>
#include <Eigen/Dense>

namespace cinolib
{

CINO_INLINE
void affine_simplicial_map(const vec3d & A0,
                           const vec3d & A1,
                           const vec3d & A2,
                           const vec3d & A3,
                           const vec3d & B0,
                           const vec3d & B1,
                           const vec3d & B2,
                           const vec3d & B3,
                           double        m[3][3],
                           double        t[3])
{
    Eigen::VectorXd rhs(12);
    Eigen::MatrixXd M;
    M.setZero(12,12);

    M.coeffRef( 0, 0) = A0.x();
    M.coeffRef( 0, 1) = A0.y();
    M.coeffRef( 0, 2) = A0.z();
    M.coeffRef( 0, 3) = 1.0;
    M.coeffRef( 1, 4) = A0.x();
    M.coeffRef( 1, 5) = A0.y();
    M.coeffRef( 1, 6) = A0.z();
    M.coeffRef( 1, 7) = 1.0;
    M.coeffRef( 2, 8) = A0.x();
    M.coeffRef( 2, 9) = A0.y();
    M.coeffRef( 2,10) = A0.z();
    M.coeffRef( 2,11) = 1.0;

    M.coeffRef( 3, 0) = A1.x();
    M.coeffRef( 3, 1) = A1.y();
    M.coeffRef( 3, 2) = A1.z();
    M.coeffRef( 3, 3) = 1.0;
    M.coeffRef( 4, 4) = A1.x();
    M.coeffRef( 4, 5) = A1.y();
    M.coeffRef( 4, 6) = A1.z();
    M.coeffRef( 4, 7) = 1.0;
    M.coeffRef( 5, 8) = A1.x();
    M.coeffRef( 5, 9) = A1.y();
    M.coeffRef( 5,10) = A1.z();
    M.coeffRef( 5,11) = 1.0;

    M.coeffRef( 6, 0) = A2.x();
    M.coeffRef( 6, 1) = A2.y();
    M.coeffRef( 6, 2) = A2.z();
    M.coeffRef( 6, 3) = 1.0;
    M.coeffRef( 7, 4) = A2.x();
    M.coeffRef( 7, 5) = A2.y();
    M.coeffRef( 7, 6) = A2.z();
    M.coeffRef( 7, 7) = 1.0;
    M.coeffRef( 8, 8) = A2.x();
    M.coeffRef( 8, 9) = A2.y();
    M.coeffRef( 8,10) = A2.z();
    M.coeffRef( 8,11) = 1.0;

    M.coeffRef( 9, 0) = A3.x();
    M.coeffRef( 9, 1) = A3.y();
    M.coeffRef( 9, 2) = A3.z();
    M.coeffRef( 9, 3) = 1.0;
    M.coeffRef(10, 4) = A3.x();
    M.coeffRef(10, 5) = A3.y();
    M.coeffRef(10, 6) = A3.z();
    M.coeffRef(10, 7) = 1.0;
    M.coeffRef(11, 8) = A3.x();
    M.coeffRef(11, 9) = A3.y();
    M.coeffRef(11,10) = A3.z();
    M.coeffRef(11,11) = 1.0;

    rhs( 0) = B0.x();
    rhs( 1) = B0.y();
    rhs( 2) = B0.z();
    rhs( 3) = B1.x();
    rhs( 4) = B1.y();
    rhs( 5) = B1.z();
    rhs( 6) = B2.x();
    rhs( 7) = B2.y();
    rhs( 8) = B2.z();
    rhs( 9) = B3.x();
    rhs(10) = B3.y();
    rhs(11) = B3.z();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd x = svd.solve(rhs);

    m[0][0]=x(0);   m[0][1]=x(1);   m[0][2]=x(2);   t[0]=x(3);
    m[1][0]=x(4);   m[1][1]=x(5);   m[1][2]=x(6);   t[1]=x(7);
    m[2][0]=x(8);   m[2][1]=x(9);   m[2][2]=x(10);  t[2]=x(11);

    // safety checks
    //
    //#ifndef NDEBUG
    //vec3d test0 = A0;
    //vec3d test1 = A1;
    //vec3d test2 = A2;
    //vec3d test3 = A3;
    //transform(test0, m); test0 += vec3d(t[0], t[1], t[2]);
    //transform(test1, m); test1 += vec3d(t[0], t[1], t[2]);
    //transform(test2, m); test2 += vec3d(t[0], t[1], t[2]);
    //transform(test3, m); test3 += vec3d(t[0], t[1], t[2]);
    //if((B0 - test0).length() > 1e-10) std::cout << (B0 - test0).length() << "\t" << tet_scaled_jacobian(A0,A1,A2,A3) << "\t" << tet_scaled_jacobian(B0,B1,B2,B3) << std::endl;
    //if((B1 - test1).length() > 1e-10) std::cout << (B1 - test1).length() << "\t" << tet_scaled_jacobian(A0,A1,A2,A3) << "\t" << tet_scaled_jacobian(B0,B1,B2,B3) << std::endl;
    //if((B2 - test2).length() > 1e-10) std::cout << (B2 - test2).length() << "\t" << tet_scaled_jacobian(A0,A1,A2,A3) << "\t" << tet_scaled_jacobian(B0,B1,B2,B3) << std::endl;
    //if((B3 - test3).length() > 1e-10) std::cout << (B3 - test3).length() << "\t" << tet_scaled_jacobian(A0,A1,A2,A3) << "\t" << tet_scaled_jacobian(B0,B1,B2,B3) << std::endl;
    //assert((B0 - test0).length() < 1e-10);
    //assert((B1 - test1).length() < 1e-10);
    //assert((B2 - test2).length() < 1e-10);
    //assert((B3 - test3).length() < 1e-10);
    //#endif
}

}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_NEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEARBYINT_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object computes the rounded to even value of its parameter.

    @par Header <boost/simd/function/nearbyint.hpp>

    @par Notes:
    - If x is \f$\pm\infty\f$, it is returned, unmodified
    - If x is \f$\pm0\f$, it is returned, unmodified
    - If x is NaN, NaN is returned

    - to even means that half integer values are rounded to the nearest
    even value.

    -  This function is in general faster than @ref round which do the rouding on
        half integer values away from zero.

    @see round, ceil, floor, trunc
  **/
  Value nearbyint(Value const& x);
} }
#endif

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

#endif


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include "gurobi_c++.h"
#include <sstream>
#include <Eigen/Dense>
#include <type_traits>
using namespace std;

template <typename T>
GRBQuadExpr GetNorm2(const std::vector<T>& x)  // Return the squared norm of a vector
{
  GRBQuadExpr result = 0;
  for (int i = 0; i < x.size(); i++)
  {
    result = result + x[i] * x[i];
  }
  return result;
}

std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBVar>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

template <typename T>  // Overload + to sum Elementwise std::vectors
std::vector<T> operator+(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::plus<T>());
  return result;
}

template <typename T>  // Overload - to substract Elementwise std::vectors
std::vector<T> operator-(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::minus<T>());
  return result;
}

std::vector<GRBLinExpr> operator-(const std::vector<GRBVar>& x, const std::vector<double>& b)
{
  std::vector<GRBLinExpr> result;
  for (int i = 0; i < x.size(); i++)
  {
    GRBLinExpr tmp = x[i] - b[i];
    result.push_back(tmp);
  }
  return result;
}

template <typename T>
std::vector<T> eigenVector2std(const Eigen::Matrix<T, -1, 1>& x)  // Return the squared norm of a vector
{
  std::vector<T> result = 0;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

template <typename T>
std::vector<T> GetColumn(std::vector<std::vector<T>> x, int column)
{
  std::vector<T> result;

  for (int i = 0; i < x.size(); i++)
  {
    result.push_back(x[i][column]);
  }
  return result;
}

int main(int argc, char* argv[])
{
  GRBEnv* env = 0;
  GRBVar* open = 0;
  GRBVar** transport = 0;
  int transportCt = 0;
  try
  {
    // Model
    env = new GRBEnv();
    GRBModel model = GRBModel(*env);
    model.set(GRB_StringAttr_ModelName, "planning");

    int N = 20;
    double umax = 5;
    double q = 20000000000;
    double dt = 0.4;
    double dt2 = dt * dt / 2.0;
    double dt3 = dt * dt * dt / 6.0;
    std::vector<std::string> states = { "x", "y", "z", "vx", "vy", "vz", "ax", "ay", "az" };
    std::vector<std::string> inputs = { "jx", "jy", "jz" };

    std::vector<double> x0 = { 5, 11.5, 0.5, 0, 0, 0, 0, 0, 0 };

    std::vector<double> xf = { 14, 5, 2.5, 0, 0, 0, 0, 0, 0 };

    std::vector<std::vector<double>> As = { { 1, 0, 0, dt, 0, 0, dt2, 0, 0 },  /////////////////////////////////
                                            { 0, 1, 0, 0, dt, 0, 0, dt2, 0 },  /////////////////////////////////
                                            { 0, 0, 1, 0, 0, dt, 0, 0, dt2 },  /////////////////////////////////
                                            { 0, 0, 0, 1, 0, 0, dt, 0, 0 },    /////////////////////////////////
                                            { 0, 0, 0, 0, 1, 0, 0, dt, 0 },    /////////////////////////////////
                                            { 0, 0, 0, 0, 0, 1, 0, 0, dt },    /////////////////////////////////
                                            { 0, 0, 0, 0, 0, 0, 1, 0, 0 },     /////////////////////////////////
                                            { 0, 0, 0, 0, 0, 0, 0, 1, 0 },     /////////////////////////////////
                                            { 0, 0, 0, 0, 0, 0, 0, 0, 1 } };

    std::vector<std::vector<double>> Bs = { { dt3, 0, 0 },   /////////////////////////////////
                                            { 0, dt3, 0 },   /////////////////////////////////
                                            { 0, 0, dt3 },   /////////////////////////////////
                                            { dt2, 0, 0 },   /////////////////////////////////
                                            { 0, dt2, 0 },   /////////////////////////////////
                                            { 0, 0, dt2 },   /////////////////////////////////
                                            { dt, 0, 0 },    /////////////////////////////////
                                            { 0, dt, 0 },    /////////////////////////////////
                                            { 0, 0, dt } };  /////////////////////////////////

    std::cout << "here1" << std::endl;

    std::vector<std::vector<GRBVar>> x;
    std::vector<std::vector<GRBVar>> u;
    for (int i = 0; i < 9; i++)
    {
      std::vector<GRBVar> row_i;
      for (int t = 0; t < N + 1; t++)
      {
        row_i.push_back(model.addVar(-GRB_INFINITY, GRB_INFINITY, 0, GRB_CONTINUOUS, states[i] + std::to_string(t)));
      }
      x.push_back(row_i);
    }

    for (int i = 0; i < 3; i++)
    {
      std::vector<GRBVar> row_i;
      for (int t = 0; t < N; t++)
      {
        row_i.push_back(model.addVar(-umax, umax, 0, GRB_CONTINUOUS, inputs[i] + std::to_string(t)));
      }
      u.push_back(row_i);
    }

    // Constraints x_t+1=Ax_t+Bu_t
    for (int t = 0; t < N; t++)
    {
      std::vector<GRBVar> xt = GetColumn(x, t);
      std::vector<GRBVar> ut = GetColumn(u, t);
      std::vector<GRBLinExpr> Ax_tm1 = MatrixMultiply(As, xt) + MatrixMultiply(Bs, ut);  //  x_t+1=Ax_t+Bu_t

      for (int i = 0; i < 9; i++)
      {
        std::cout << "i=" << i << "  t=" << t << std::endl;
        // std::cout << "x[i][t]" << x[i][t] << std::endl;
        model.addConstr(x[i][t + 1] == Ax_tm1[i]);
        // std::cout << "Abajo" << std::endl;
      }
    }
    // model.update();

    // Constraints x_0=x_initial
    for (int i = 0; i < 9; i++)
    {
      model.addConstr(x[i][0] == x0[i]);
    }

    GRBQuadExpr control_cost = 0;
    for (int t = 0; t < N; t++)
    {
      std::vector<GRBVar> ut = GetColumn(u, t);
      control_cost = control_cost + GetNorm2(ut);
    }

    GRBQuadExpr final_state_cost = 0;
    std::vector<GRBVar> xFinal = GetColumn(x, N);
    // std::vector<GRBLinExpr> prueba = xFinal - xf;
    final_state_cost = GetNorm2(xFinal - xf);
    final_state_cost = q * final_state_cost;

    model.setObjective(control_cost + final_state_cost, GRB_MINIMIZE);

    // Solve*/
    model.update();
    model.write("debug.lp");
    model.optimize();

    std::cout << "\nOBJECTIVE: " << model.get(GRB_DoubleAttr_ObjVal) << std::endl;
    std::cout << "Positions X:" << std::endl;
    for (int t = 0; t < N + 1; t++)
    {
      std::cout << x[0][t].get(GRB_DoubleAttr_X) << std::endl;
    }
  }

  catch (GRBException e)
  {
    cout << "Error code = " << e.getErrorCode() << endl;
    cout << e.getMessage() << endl;
  }
  /*catch (...)
  {
    cout << "Exception during optimization" << endl;
  }*/

  delete env;
  return 0;
}