#pragma once
#include "vector.hpp"
#include <boost/operators.hpp>
#include <iostream>

namespace dmc
{
	template <class Scalar, int Dimension>
	class dual
		: boost::addable<dual<Scalar, Dimension>
		, boost::addable<dual<Scalar, Dimension>, Scalar
		, boost::subtractable<dual<Scalar, Dimension>
		, boost::subtractable<dual<Scalar, Dimension>, Scalar
		, boost::multipliable<dual<Scalar, Dimension>
		, boost::multipliable<dual<Scalar, Dimension>, Scalar
		, boost::dividable<dual<Scalar, Dimension>
		, boost::dividable<dual<Scalar, Dimension>, Scalar
		, boost::equality_comparable<dual<Scalar, Dimension>
		, boost::equality_comparable<dual<Scalar, Dimension>, Scalar
		, boost::less_than_comparable<dual<Scalar, Dimension>
		, boost::less_than_comparable<dual<Scalar, Dimension>, Scalar
		>>>>>>>>>>>>
	{
	public:
		typedef Scalar scalar_type;
		static const int dimension = Dimension;
		typedef vector<scalar_type, dimension> vector_type;

		dual() = default;

		dual(scalar_type value)
			: value_(value)
		{
		}

		dual(scalar_type value, const vector_type& grad)
			: value_(value)
			, grad_(grad)
		{
		}

		scalar_type& value()
		{
			return value_;
		}

		const scalar_type& value() const
		{
			return value_;
		}

		vector_type& grad()
		{
			return grad_;
		}

		const vector_type& grad() const
		{
			return grad_;
		}

		const dual& operator+() const
		{
			return *this;
		}

		dual operator-() const
		{
			return dual(-value_, -grad_);
		}

		dual& operator+=(const dual& rhs)
		{
			value_ += rhs.value_;
			grad_ += rhs.grad_;
			return *this;
		}

		dual& operator-=(const dual& rhs)
		{
			value_ -= rhs.value_;
			grad_ -= rhs.grad_;
			return *this;
		}

		dual& operator*=(const dual& rhs)
		{
			grad_ = value_ * rhs.grad_ + rhs.value_ * grad_;
			value_ *= rhs.value_;
			return *this;
		}

		dual& operator/=(const dual& rhs)
		{
			grad_ = (grad_ * rhs.value_ - value_ * rhs.grad_) / (rhs.value_ * rhs.value_);
			value_ /= rhs.value_;
			return *this;
		}

		friend bool operator==(const dual& lhs, const dual& rhs)
		{
			return lhs.value_ == rhs.value_;
		}

		friend bool operator<(const dual& lhs, const dual& rhs)
		{
			return lhs.value_ < rhs.value_;
		}

		friend dual abs(const dual& d)
		{
			return d.value() < static_cast<scalar_type>(0.0) ? -d : d;
		}

		friend dual sqrt(const dual& d)
		{
			auto value = std::sqrt(d.value());
			auto grad = value * static_cast<scalar_type>(0.5) * d.grad() / d.value();
			return dual(value, grad);
		}

		friend dual sin(const dual& d)
		{
			auto value = std::sin(d.value());
			auto grad = d.grad() * std::cos(d.value());
			return dual(value, grad);
		}

		friend dual cos(const dual& d)
		{
			auto value = std::cos(d.value());
			auto grad = -d.grad() * std::sin(d.value());
			return dual(value, grad);
		}

	private:
		scalar_type value_{};
		vector_type grad_;
	};

	template <class Scalar, int Dimension>
	Scalar value(const dual<Scalar, Dimension>& d)
	{
		return d.value();
	}

	template <class Scalar>
	Scalar value(const Scalar& value)
	{
		return value;
	}

	template <class Scalar, int Dimension>
	Scalar grad(const dual<Scalar, Dimension>& d)
	{
		return d.grad();
	}

	template <class Scalar, int Dimension>
	std::ostream& operator<<(std::ostream& os, const dual<Scalar, Dimension>& d)
	{
		os << "{ " << d.value() << ", " << d.grad().transpose() << " }";
		return os;
	}
}


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

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

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

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

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

/*
*
*   Tutorial: Algebraic multigrid preconditioner (only available with the OpenCL backend, experimental)
*
*/

#ifndef NDEBUG     //without NDEBUG the performance of sparse ublas matrices is poor.
 #define NDEBUG
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#define VIENNACL_WITH_UBLAS 1

#define SOLVER_ITERS 2500
//#define SCALAR float
#define SCALAR double

//#define SOLVER_TOLERANCE 1e-5
#define SOLVER_TOLERANCE 1e-9

#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/norm_2.hpp"

#include "viennacl/linalg/amg.hpp"

#include <iostream>
#include <vector>
#include <ctime>
#include "vector-io.hpp"

template<typename MatrixType, typename VectorType, typename SolverTag, typename PrecondTag>
void run_solver(MatrixType const & matrix, VectorType const & rhs, VectorType const & ref_result, SolverTag const & solver, PrecondTag const & precond)
{
  VectorType result(rhs);
  VectorType residual(rhs);

  result = viennacl::linalg::solve(matrix, rhs, solver, precond);
  residual -= viennacl::linalg::prod(matrix, result);
  std::cout << "  > Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(rhs) << std::endl;
  std::cout << "  > Iterations: " << solver.iters() << std::endl;
  result -= ref_result;
  std::cout << "  > Relative deviation from result: " << viennacl::linalg::norm_2(result) / viennacl::linalg::norm_2(ref_result) << std::endl;
}

template<typename ScalarType>
void run_amg(viennacl::linalg::cg_tag & cg_solver,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_vec*/,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_result*/,
             boost::numeric::ublas::compressed_matrix<ScalarType> & ublas_matrix,
             viennacl::vector<ScalarType> & vcl_vec,
             viennacl::vector<ScalarType> & vcl_result,
             viennacl::compressed_matrix<ScalarType> & vcl_compressed_matrix,
             std::string info,
             viennacl::linalg::amg_tag & amg_tag)
{

  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg = viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > (ublas_matrix, amg_tag);
  boost::numeric::ublas::vector<ScalarType> avgstencil;
  unsigned int coarselevels = amg_tag.get_coarselevels();

  std::cout << "-- CG with AMG preconditioner, " << info << " --" << std::endl;

  std::cout << " * Setup phase (ublas types)..." << std::endl;

  // Coarse level measure might have been changed during setup. Reload!
  ublas_amg.tag().set_coarselevels(coarselevels);
  ublas_amg.setup();

  std::cout << " * Operator complexity: " << ublas_amg.calc_complexity(avgstencil) << std::endl;

  amg_tag.set_coarselevels(coarselevels);
  viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > vcl_amg = viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > (vcl_compressed_matrix, amg_tag);
  std::cout << " * Setup phase (ViennaCL types)..." << std::endl;
  vcl_amg.tag().set_coarselevels(coarselevels);
  vcl_amg.setup();

  std::cout << " * CG solver (ublas types)..." << std::endl;
  //run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, ublas_amg);

  std::cout << " * CG solver (ViennaCL types)..." << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, vcl_amg);

}

int main()
{
  //
  // Print some device info
  //
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  // Optional: Customize OpenCL backend
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Optional: Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Optional: Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  std::cout << viennacl::ocl::current_device().info() << std::endl;
  viennacl::context ctx(viennacl::ocl::get_context(1));
#else
  viennacl::context ctx;
#endif

  typedef float    ScalarType;  // feel free to change this to double if supported by your device

  //
  // Set up the matrices and vectors for the iterative solvers (cf. iterative.cpp)
  //
  boost::numeric::ublas::vector<ScalarType> ublas_vec, ublas_result;
  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_matrix;

  viennacl::linalg::cg_tag cg_solver;
  viennacl::linalg::amg_tag amg_tag;
  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg;

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

  // Set up rhs and result vector
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", ublas_vec))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ublas_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  viennacl::vector<ScalarType> vcl_vec(ublas_vec.size(), ctx);
  viennacl::vector<ScalarType> vcl_result(ublas_vec.size(), ctx);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(ublas_vec.size(), ublas_vec.size(), ctx);

  // Copy to GPU
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_vec, vcl_vec);
  viennacl::copy(ublas_result, vcl_result);

  //
  // Run solver without preconditioner
  //
  std::cout << "-- CG solver (CPU, no preconditioner) --" << std::endl;
  run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, viennacl::linalg::no_precond());

  std::cout << "-- CG solver (GPU, no preconditioner) --" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, viennacl::linalg::no_precond());

  //
  // With AMG Preconditioner RS+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS,       // coarsening strategy
                                      VIENNACL_AMG_INTERPOL_DIRECT, // interpolation strategy
                                      0.25, // strength of dependence threshold
                                      0.2,  // interpolation weight
                                      0.67, // jacobi smoother weight
                                      3,    // presmoothing steps
                                      3,    // postsmoothing steps
                                      0);   // number of coarse levels to be used (0: automatically use as many as reasonable)
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS+CLASSIC
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS, VIENNACL_AMG_INTERPOL_CLASSIC, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg ( cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, CLASSIC INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner ONEPASS+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_ONEPASS, VIENNACL_AMG_INTERPOL_DIRECT,0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "ONEPASS COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS0+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS0, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS0 COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner RS3+DIRECT
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS3, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS3 COARSENING, DIRECT INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner AG
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_AG, 0.08, 0, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, AG INTERPOLATION", amg_tag);

  //
  // With AMG Preconditioner SA
  //
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_SA, 0.08, 0.67, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, SA INTERPOLATION",amg_tag);

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can 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 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef ANISOTROPIC_HPP
#define ANISOTROPIC_HPP

#include <iosfwd>

#include "Config.hpp"

#include <Eigen/Core>

#include "utils/MathUtils.hpp"

/*! \file Anisotropic.hpp
 *  \class Anisotropic
 *  \brief describes a medium with anisotropy, i.e. liquid crystal
 *  \author Roberto Di Remigio
 *  \date 2014
 */

class Anisotropic __final
{
private:
    /// Diagonal of the permittivity tensor in the lab-fixed frame
    Eigen::Vector3d epsilonLab_;
    /// Euler angles (in degrees) relating molecule-fixed and lab-fixed frames
    Eigen::Vector3d eulerAngles_;
    /// Permittivity tensor in molecule-fixed frame
    Eigen::Matrix3d epsilon_;
    /// Inverse of the permittivity tensor in molecule-fixed frame
    Eigen::Matrix3d epsilonInv_;
    /// molecule-fixed to lab-fixed frames rotation matrix
    Eigen::Matrix3d R_;
    /// Determinant of the permittivity tensor
    double detEps_;
    /*! Initializes some internals: molecule-fixed to lab-fixed frame rotation matrix,
     * permittivity tensor in molecule-fixed frame and its inverse
     */
    void build() {
        // 1. construct rotation matrix from Euler angles
	    eulerRotation(R_, eulerAngles_);
	    // 2. Apply the rotation matrix: epsilon_ = R_^t * epsilonLab_ * R_
	    epsilon_ = R_.transpose() * epsilonLab_.asDiagonal() * R_;
	    // 3. Obtain epsilonInv_ = R_ * epsilonLab_^-1 * R_^t
	    Eigen::Vector3d scratch;
	    scratch << (1.0/epsilonLab_(0)), (1.0/epsilonLab_(1)), (1.0/epsilonLab_(2));
	    epsilonInv_ = R_ * scratch.asDiagonal() * R_.transpose();
	    // 4. As a __final step, calculate the determinant
	    detEps_ = epsilonLab_(0) * epsilonLab_(1) * epsilonLab_(2);
    }
public:
    Anisotropic() :
	    epsilonLab_(Eigen::Vector3d::Ones()), eulerAngles_(Eigen::Vector3d::Zero()) { this->build(); }
    /*!
     * \param[in] eigen_eps eigenvalues of the permittivity tensors
     * \param[in] euler_ang Euler angles in degrees
     */
    Anisotropic(const Eigen::Vector3d & eigen_eps, const Eigen::Vector3d & euler_ang) :
	    epsilonLab_(eigen_eps), eulerAngles_(euler_ang) { this->build(); }
    const Eigen::Matrix3d & epsilon() const { return epsilon_; }
    const Eigen::Matrix3d & epsilonInv() const { return epsilonInv_; }
    double detEps() const { return detEps_; }
    friend std::ostream & operator<<(std::ostream & os, Anisotropic & arg) {
        os << "Permittivity tensor diagonal (lab frame)   = " << arg.epsilonLab_.transpose() << std::endl;
        os << "Euler angles (molecule-to-lab frame)       = " << arg.eulerAngles_.transpose() << std::endl;
        os << "Permittivity tensor (molecule-fixed frame) =\n" << arg.epsilon_;
        return os;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
};

#endif // ANISOTROPIC_HPP


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ctime>

#include <kv/qr.hpp>

namespace ub = boost::numeric::ublas;

int main()
{
	int i, j;
	ub::matrix<double> a, q, r;

	a.resize(2, 2);

	a(0, 0) = 1.;
	a(0, 1) = 2.;
	a(1, 0) = 3.;
	a(1, 1) = 4.;

	kv::qr(a, q, r);

	std::cout << q << "\n";
	std::cout << r << "\n";
	std::cout << a - prod(q, r) << "\n";
	std::cout << prod(q, trans(q)) << "\n";

	for (i=0; i<10; i++) {
		kv::qr(a, q, r);
		a = prod(r, q);
		std::cout << a << "\n";
	}

	a.resize(5, 5);
	srand(time(NULL));
	for (i=0; i<5; i++) {
		for (j=0; j<5; j++) {
			a(i, j) = (double)rand();
		}
	}

	kv::qr(a, q, r);

	// std::cout << q << "\n";
	// std::cout << r << "\n";
	std::cout << a - prod(q, r) << "\n";
	std::cout << prod(q, trans(q)) << "\n";
}


/*
 * ForceDihedral.cpp
 *
 *  Created on: 27 feb 2018
 *      Author: lorenzo
 */

#include "ForceDihedral.h"

#include <Eigen/Dense>

namespace ashell {

ForceDihedral::ForceDihedral(std::string name) :
				ForceComputer(name),
				_kb(100.),
				_theta0(0.) {

}

ForceDihedral::~ForceDihedral() {

}

void ForceDihedral::set_kb(double n_kb) {
	_kb = n_kb;
}

void ForceDihedral::set_theta0(double n_theta0) {
	_theta0 = n_theta0;
}

void ForceDihedral::_compute_forces(ullint step) {
	const vector_vec3 &poss = _particles->positions();

	for(auto &dihedral : _sys_props->dihedrals()) {
		uint i = dihedral->members[0];
		uint j = dihedral->members[1];
		uint k = dihedral->members[2];
		uint l = dihedral->members[3];

		vec3 i_pos = poss[i];
		vec3 j_pos = poss[j];
		vec3 k_pos = poss[k];
		vec3 l_pos = poss[l];

		// see Sofia Biagi's thesis, pag. 114
		vec3 r_ij = _sys_props->box()->minimum_image(j_pos, i_pos);
		vec3 r_kj = _sys_props->box()->minimum_image(j_pos, k_pos);
		vec3 r_kl = _sys_props->box()->minimum_image(l_pos, k_pos);

		double r_kj_sqr = r_kj.dot(r_kj);
		double r_kj_mod = sqrt(r_kj_sqr);

		vec3 m = r_ij.cross(r_kj);
		vec3 n = r_kj.cross(r_kl);

		double m_sqr = m.dot(m);
		double n_sqr = n.dot(n);

		double costheta = m.dot(n) / (sqrt(m_sqr) * sqrt(n_sqr));
		if(costheta > 1.) costheta = 1.;
		if(costheta < -1.) costheta = -1.;

		double theta = acos(costheta);
		if(r_ij.dot(n) < 0.) theta = -theta;
		// we use the so-called polymer convention, for which theta(polymer) = theta(IUPAC) +/- pi
		theta -= M_PI;

		double dVdtheta = _kb * sin(theta - _theta0);
		vec3 F_i = (-dVdtheta * r_kj_mod / m_sqr) * m;
		vec3 F_l = (dVdtheta * r_kj_mod / n_sqr) * n;

		// We compute Fj and Fk from Fi and Fl by knowing that  F_i + Fj + Fk + Fl == 0
		// and that the dihedral potential energy function is rotationally invariant
		vec3 S = (r_ij.dot(r_kj) / r_kj_sqr) * F_i - (r_kl.dot(r_kj) / r_kj_sqr) * F_l;

		_forces[i] += F_i;
		_forces[j] += -F_i + S;
		_forces[k] += -F_l - S;
		_forces[l] += F_l;

		double energy = _kb * (1. - cos(theta - _theta0));

//		BOOST_LOG_TRIVIAL(info) << costheta << " " << (theta*180/M_PI) << " " << energy;

		_energy += energy;
		_energies[i] += energy / 4.;
		_energies[j] += energy / 4.;
		_energies[k] += energy / 4.;
		_energies[l] += energy / 4.;
	}
}

} /* namespace ashell */


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef AVERAGESOURCEPARABOLICPDE_HPP_
#define AVERAGESOURCEPARABOLICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractCellPopulation.hpp"
#include "TetrahedralMesh.hpp"
#include "AbstractLinearParabolicPde.hpp"

/**
 * A parabolic PDE to be solved numerically using the finite element method, for
 * coupling to a cell-based simulation.
 *
 * The PDE takes the form
 *
 * c*du/dt = Grad.(D*Grad(u)) + k*u*rho(x),
 *
 * where the scalars c, D and k are specified by the members mDuDtCoefficient,
 * mDiffusionCoefficient and mSourceCoefficient, respectively. Their values must
 * be set in the constructor.
 *
 * The function rho(x) denotes the local density of non-apoptotic cells. This
 * quantity is computed for each element of a 'coarse' finite element mesh that is
 * passed to the method SetupSourceTerms() and stored in the member mCellDensityOnCoarseElements.
 * For a point x, rho(x) is defined to be the number of non-apoptotic cells whose
 * centres lie in each finite element containing that point, scaled by the area of
 * that element.
 */
template<unsigned DIM>
class AveragedSourceParabolicPde : public AbstractLinearParabolicPde<DIM,DIM>
{
    friend class TestCellBasedParabolicPdes;

private:

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<AbstractLinearParabolicPde<DIM, DIM> >(*this);
       archive & mDuDtCoefficient;
       archive & mDiffusionCoefficient;
       archive & mSourceCoefficient;
       archive & mCellDensityOnCoarseElements;
    }

protected:

    /** The cell population member. */
    AbstractCellPopulation<DIM, DIM>& mrCellPopulation;

    /** Coefficient of rate of change term.  */
    double mDuDtCoefficient;

    /** Diffusion coefficient. */
    double mDiffusionCoefficient;

    /** Coefficient of the rate of uptake of the dependent variable by non-apoptotic cells. */
    double mSourceCoefficient;

    /** Vector of averaged cell densities on elements of the coarse mesh. */
    std::vector<double> mCellDensityOnCoarseElements;

public:

    /**
     * Constructor.
     *
     * @param rCellPopulation reference to the cell population
     * @param duDtCoefficient rate of reaction (defaults to 1.0)
     * @param diffusionCoefficient rate of diffusion (defaults to 1.0)
     * @param sourceCoefficient the source term coefficient (defaults to 0.0)
     */
    AveragedSourceParabolicPde(AbstractCellPopulation<DIM, DIM>& rCellPopulation,
                               double duDtCoefficient=1.0,
                               double diffusionCoefficient=1.0,
                               double sourceCoefficient=0.0);

    /**
     * @return const reference to the cell population (used in archiving).
     */
    const AbstractCellPopulation<DIM>& rGetCellPopulation() const;

    /**
     * Set up the source terms.
     *
     * \todo this is identical to the one in AveragedSourceEllipticPde so refactor.
     *
     * @param rCoarseMesh reference to the coarse mesh
     * @param pCellPdeElementMap optional pointer to the map from cells to coarse elements
     */
    void virtual SetupSourceTerms(TetrahedralMesh<DIM,DIM>& rCoarseMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap=nullptr);

    /**
     * Overridden ComputeDuDtCoefficientFunction() method.
     *
     * @return the function c(x) in "c(x) du/dt = Grad.(DiffusionTerm(x)*Grad(u))+LinearSourceTerm(x)+NonlinearSourceTerm(x, u)"
     *
     * @param rX the point in space at which the function c is computed
     */
    virtual double ComputeDuDtCoefficientFunction(const ChastePoint<DIM>& rX);

    /**
     * Overridden ComputeSourceTerm() method.
     *
     * @return computed source term.
     *
     * @param rX the point in space at which the nonlinear source term is computed
     * @param u the value of the dependent variable at the point
     * @param pElement the mesh element that x is contained in (optional; defaults to NULL).
     */
    virtual double ComputeSourceTerm(const ChastePoint<DIM>& rX,
                                     double u,
                                     Element<DIM,DIM>* pElement=NULL);

    /**
     * Overridden ComputeSourceTermAtNode() method. That is never called.
     *
     * @return computed source term at a node.
     *
     * @param rNode the node at which the nonlinear source term is computed
     * @param u the value of the dependent variable at the node
     */
    virtual double ComputeSourceTermAtNode(const Node<DIM>& rNode, double u);

    /**
     * Overridden ComputeDiffusionTerm() method.
     *
     * @param rX the point in space at which the diffusion term is computed
     * @param pElement the mesh element that x is contained in (optional; defaults to NULL).
     *
     * @return a matrix.
     */
    virtual c_matrix<double,DIM,DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement=NULL);

    /**
     * @return the uptake rate.
     *
     * @param elementIndex the element we wish to return the uptake rate for
     */
    double GetUptakeRateForElement(unsigned elementIndex);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(AveragedSourceParabolicPde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a AveragedSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const AveragedSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM, DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise a AveragedSourceParabolicPde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, AveragedSourceParabolicPde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM, DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)AveragedSourceParabolicPde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*AVERAGESOURCEPARABOLICPDE_HPP_*/


#include "spectral_clustering.h"

#include <set>
#include <array>
#include <algorithm>

#include <Eigen/Sparse>
#include <igl/eigs.h>

#include <algorithms/shortest_path.h>

using namespace shortest_path;

SpectralClustering::SpectralClustering(std::shared_ptr<Geometry> geometry, unsigned int k, double delta, double eta):
	_delta(delta),
	_eta(eta) {

	if (geometry == nullptr) {
		return;
	}

	// Generate affinity matrix from mesh faces
	Eigen::MatrixXd W = affinity_matrix(geometry);

	// Degree matrix
	Eigen::DiagonalMatrix<double, -1> Ds = W.rowwise().sum().cwiseInverse().cwiseSqrt().asDiagonal();

	// Graph Laplacian
	Eigen::MatrixXd L = Ds * (W * Ds);

	Eigen::MatrixXd M = Eigen::MatrixXd::Identity(L.rows(), L.cols());

	Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(L, M, Eigen::ComputeEigenvectors);
	Eigen::MatrixXd U = es.eigenvectors().real().rightCols(k);

	// kmeans clustering
	Eigen::MatrixXd V = U.colwise().normalized();
	Eigen::MatrixXd Q = V * V.transpose();

	// Group faces by clusters
	Eigen::VectorXi guess = initial_guess(Q, k);

	_segment_by_face = k_means_lloyds(guess, V, k);
}

SpectralClustering::~SpectralClustering() {
}

const Eigen::VectorXi& SpectralClustering::segment_by_face() const {
	return _segment_by_face;
}

Eigen::MatrixXd SpectralClustering::affinity_matrix(std::shared_ptr<Geometry> geometry) {
	Eigen::SparseMatrix<double> G(geometry->faces().rows(), geometry->faces().rows());
	Eigen::SparseMatrix<double> A(geometry->faces().rows(), geometry->faces().rows());

	std::set<Eigen::DenseIndex> eta_list;
	unsigned int num_adj = 0;
	const Eigen::SparseMatrix<int>& adj = geometry->adjacency_matrix();
	const trimesh::trimesh_t& halfedge_mesh = geometry->halfedge();

	unsigned int count = 0;

	std::vector<Eigen::Triplet<double>> g_triplets;
	std::vector<Eigen::Triplet<double>> a_triplets;
	Eigen::DenseIndex v_count = geometry->vertices().rows();
	for (Eigen::DenseIndex i = 0; i < v_count; ++i) {
		for (Eigen::DenseIndex j = i + 1; j < v_count; ++j) {
			trimesh::index_t he_index = halfedge_mesh.directed_edge2he_index(i, j);

			if (he_index < 0) {
				// Invalid vertex pair??
				continue;
			}

			const trimesh::trimesh_t::halfedge_t& he = halfedge_mesh.halfedge(he_index);
			const trimesh::trimesh_t::halfedge_t& op_he = halfedge_mesh.halfedge(he.opposite_he);

			if (he.face < 0 || op_he.face < 0) {
				// This edge is on a boundary!
				continue;
			}

			Eigen::DenseIndex r = he.face;
			Eigen::DenseIndex s = op_he.face;

			Eigen::VectorXi face1 = geometry->faces().row(r);
			Eigen::VectorXi face2 = geometry->faces().row(s);

			double gd = geodesic_distance({ he.to_vertex, op_he.to_vertex }, face1, face2, geometry->vertices());
			double ad = angular_distance(face1, face2, geometry->vertices());

			g_triplets.emplace_back(Eigen::Triplet<double>(r, s, gd));
			g_triplets.emplace_back(Eigen::Triplet<double>(s, r, gd));
			a_triplets.emplace_back(Eigen::Triplet<double>(r, s, ad));
			a_triplets.emplace_back(Eigen::Triplet<double>(s, r, ad));

			++count;
		}
	}

	double geodesic_avg = 0.0;
	double angular_avg = 0.0;

	G.setFromTriplets(g_triplets.begin(), g_triplets.end());
	A.setFromTriplets(a_triplets.begin(), a_triplets.end());

	if (count > 0) {
		geodesic_avg = G.sum() / static_cast<double>(2 * count);
		angular_avg = A.sum() / static_cast<double>(2 * count);
	}

	G = G * (_delta / geodesic_avg);
	A = A * ((1.0 - _delta) / angular_avg);

	Eigen::MatrixXd W = floyd_warshall(G + A);

	std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> inf_entries;
	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < W.cols(); ++j) {
			if (W(i, j) > std::numeric_limits<double>::max()) {
				W(i, j) = 0.0;
				inf_entries.emplace_back(std::pair<Eigen::DenseIndex, Eigen::DenseIndex>(i, j));
			}
		}
	}

	double sigma = W.sum() / (std::pow(geometry->faces().rows(), 2.0));
	double den = 2 * std::pow(sigma, 2.0);
	W = -1.0 * W / den;

	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < W.cols(); ++j) {
			W(i, j) = std::exp(W(i, j));
		}
	}

	for (auto it : inf_entries) {
		W(it.first, it.second) = 0.0;
	}

	for (Eigen::DenseIndex i = 0; i < W.rows(); ++i) {
		W(i, i) = 1.0;
	}

	return W;
}

double SpectralClustering::geodesic_distance(std::array<Eigen::DenseIndex,2> edge_verts, const Eigen::VectorXi& face1, const Eigen::VectorXi& face2, const Eigen::MatrixXd& V) const {
	if (face1.size() <= 0 || face2.size() <= 0) {
		return 0.0;
	}

	Eigen::VectorXd edge_center = (V.row(edge_verts[0]) + V.row(edge_verts[1])).transpose() / 2.0;

	Eigen::VectorXd face1_center = Eigen::VectorXd::Zero(edge_center.rows());
	for (Eigen::DenseIndex i = 0; i < face1.size(); ++i) {
		face1_center += V.row(face1(i)).transpose();
	}
	face1_center /= face1.size();

	Eigen::VectorXd face2_center = Eigen::VectorXd::Zero(edge_center.rows());
	for (Eigen::DenseIndex i = 0; i < face2.size(); ++i) {
		face2_center += V.row(face2(i)).transpose();
	}
	face2_center /= face2.size();

	return (edge_center - face1_center).norm() + (edge_center - face2_center).norm();
}

double SpectralClustering::angular_distance(const Eigen::VectorXi& face1, const Eigen::VectorXi& face2, const Eigen::MatrixXd& V) const {
	if (face1.size() != 3 || face2.size() != 3) {
		return 0.0;
	}

	Eigen::Vector3d e1 = (V.row(face1(0)) - V.row(face1(1))).block<1, 3>(0, 0).normalized().transpose();
	Eigen::Vector3d e2 = (V.row(face1(2)) - V.row(face1(1))).block<1, 3>(0, 0).normalized().transpose();

	Eigen::Vector3d face1_normal = e1.cross(e2).normalized();

	e1 = (V.row(face2(0)) - V.row(face2(1))).block<1, 3>(0, 0).normalized().transpose();
	e2 = (V.row(face2(2)) - V.row(face2(1))).block<1, 3>(0, 0).normalized().transpose();

	Eigen::Vector3d face2_normal = e1.cross(e2).normalized();

	Eigen::VectorXd face1_center = Eigen::VectorXd::Zero(V.row(face1(0)).cols());
	for (Eigen::DenseIndex i = 0; i < face1.size(); ++i) {
		face1_center += V.row(face1(i)).transpose();
	}
	face1_center /= face1.size();

	Eigen::VectorXd face2_center = Eigen::VectorXd::Zero(V.row(face2(0)).cols());
	for (Eigen::DenseIndex i = 0; i < face2.size(); ++i) {
		face2_center += V.row(face2(i)).transpose();
	}
	face2_center /= face2.size();

	bool use_eta = face1_normal.dot((face2_center - face1_center).block<3, 1>(0, 0).normalized()) < 0.0;

	return ((use_eta) ? _eta : 1.0) * (1.0 - face1_normal.dot(face2_normal));
}

Eigen::VectorXi SpectralClustering::k_means_lloyds(const Eigen::VectorXi& guess, const Eigen::MatrixXd& V, unsigned int k) {
	// guess
	Eigen::VectorXi means = guess; //initial_guess(V, k);
	Eigen::VectorXi prev_means = means;

	std::cout << means << std::endl;

	Eigen::VectorXi assignment = Eigen::VectorXi::Constant(V.rows(),-1);
	Eigen::VectorXi next_assignment = assignment;

	double stop_threshold = 0.001 * means.rowwise().norm().minCoeff();

	do {
		// assign
		for (Eigen::DenseIndex i = 0; i < V.rows(); ++i) {
			double min_dist = std::numeric_limits<double>::max();
			Eigen::DenseIndex min_index = -1;

			for (Eigen::DenseIndex j = 0; j < means.rows(); ++j) {
				double dist = (V.row(means(j)) - V.row(i)).norm();

				if (dist < min_dist) {
					min_dist = dist;
					min_index = j;
				}
			}

			if (min_index < 0) {
				// There was no nearest centroid?? Something is wrong..
				throw std::logic_error("Finding nearest centroid failed??");
			}

			next_assignment(i) = min_index;
		}

		if ((assignment - next_assignment).sum() == 0) {
			// converged
			std::cout << "Converged!" << std::endl;
			break;
		}

		// update
		assignment = next_assignment;

		Eigen::MatrixXd numeric_means = Eigen::MatrixXd::Zero(means.rows(), V.cols());
		Eigen::VectorXi c_count = Eigen::VectorXi::Zero(means.rows());
		means = Eigen::VectorXi::Zero(k);

		for (Eigen::DenseIndex i = 0; i < assignment.rows(); ++i) {
			numeric_means.row(assignment(i)) += V.row(i);
			c_count(assignment(i))++;
		}

		for (Eigen::DenseIndex i = 0; i < c_count.rows(); ++i) {
			if (c_count(i) > 0) {
				numeric_means.row(i) /= c_count(i);
			}
		}

		// Find nearest vector in V closest to each numeric mean
		for (Eigen::DenseIndex i = 0; i < numeric_means.rows(); ++i) {
			double min_dist = std::numeric_limits<double>::max();
			Eigen::DenseIndex min_index = -1;

			for (Eigen::DenseIndex j = 0; j < V.rows(); ++j) {
				double dist = (V.row(j) - numeric_means.row(i)).norm();

				if (dist < min_dist) {
					min_dist = dist;
					min_index = j;
				}
			}

			if (min_index < 0) {
				// There was no nearest vector?? Something is wrong..
				throw std::logic_error("Finding nearest point to centroid failed??");
			}

			means(i) = min_index;
		}

		prev_means = means;

	} while (true);

	std::cout << means << std::endl;

	return next_assignment;
}

Eigen::VectorXi SpectralClustering::initial_guess(const Eigen::MatrixXd& Q, unsigned int k) {
	//"""Computes an initial guess for the cluster-centers"""
	int n = Q.rows();
	double min_value = std::numeric_limits<double>::max();
	std::array<int, 2> min_indices = { -1, -1 };

	for (int i = 0; i < Q.rows(); ++i) {
		for (int j = 0; j < Q.cols(); ++j) {
			if (i != j && Q(i, j) < min_value) {
				min_value = Q(i, j);
				min_indices = { i, j };
			}
		}
	}

	std::set<int> chosen = { min_indices[0], min_indices[1] };

	while (chosen.size() < k) {
		double min_max = std::numeric_limits<double>::max();
		double cur_max = 0.0;
		int new_index = -1;

		for (Eigen::DenseIndex i = 0; i < n; ++i) {
			if (chosen.count(i) <= 0) {
				cur_max = std::numeric_limits<double>::min();
				for (auto c : chosen) {
					if (cur_max < Q(c, i)) {
						cur_max = Q(c, i);
					}
				}

				if (min_max - cur_max > 1e-6) {
					min_max = cur_max;
					new_index = i;
				}
			}
		}

		chosen.insert(new_index);
	}

	Eigen::VectorXi guess(chosen.size());
	Eigen::DenseIndex j = 0;
	for (auto c : chosen) {
		guess(j++) = c;
	}

	return guess;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  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_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_INVTRIG_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_2lo.hpp>
#include <boost/simd/constant/pio_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/pio_4lo.hpp>
#include <boost/simd/constant/tan_3pio_8.hpp>
#include <boost/simd/constant/tanpio_8.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/twopio_3.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/function/scalar/oneplus.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

   template < class A0,
             class unit_tag,
             class style,
             class base_A0 = bd::scalar_of_t<A0>
  >
  struct invtrig_base{};

  template < class A0 >
  struct invtrig_base<A0,tag::radian_tag,tag::not_simd_type, float>
  {
    static BOOST_FORCEINLINE A0 asin(A0 a0) BOOST_NOEXCEPT
    {
      A0 sign, x, z;
      x = bs::abs(a0);
      sign = bitofsign(a0);
      if ((x < Constant<A0,0x38d1b717>())) return a0; //1.0e-4
      if ((x >  One<A0>())) return Nan<A0>();
      auto bx_larger_05    = (x > Half<A0>());
      if (bx_larger_05)
      {
        z = Half<A0>()*oneminus(x);
        x =  sqrt(z);
      }
      else
      {
        z = sqr(x);
      }
      A0 z1 = horn<A0,
        0x3e2aaae4,
        0x3d9980f6,
        0x3d3a3ec7,
        0x3cc617e3,
        0x3d2cb352
        > (z);
      z1 = fma(z1, z*x, x);
      if(bx_larger_05)
      {
        z1 = z1+z1;
        z1 = Pio_2<A0>()-z1;
      }
      return bitwise_xor(z1, sign);
    }

    static BOOST_FORCEINLINE A0 acos(const  A0& a0) BOOST_NOEXCEPT
    {
      if (a0 < Mhalf<A0>())
        return Pi<A0>()-asin( sqrt(oneplus(a0)*Half<A0>()))*Two<A0>();
      else if (a0 > Half<A0>())
        return asin( sqrt(oneminus(a0)*Half<A0>()))*Two<A0>();
      return (Pio_2<A0>()-asin(a0));
    }

    static BOOST_FORCEINLINE A0 atan(A0 a0) BOOST_NOEXCEPT
    {
      A0 x  = kernel_atan(a0);
      return bitwise_xor(x, bitofsign(a0));
    }

    static BOOST_FORCEINLINE A0 kernel_atan(A0 a0) BOOST_NOEXCEPT
    {
      if (is_eqz(a0))  return Zero<A0>();
      if (is_inf(a0))  return Pio_2<A0>();
      A0 x = bs::abs(a0);
      A0 y = 0.0;
      A0 more = Zero<A0>();
      if( x > Tan_3pio_8<A0>())
      {
        y = Pio_2<A0>();
        more = Pio_2lo<A0>();
        x = -rec(x);
      }
      else if( x > Tanpio_8<A0>())
      {
        y = Pio_4<A0>();
        more =  Pio_4lo<A0>();
        x = minusone(x)/oneplus(x);
      }
      A0 z = sqr(x);
      A0 z1 = horn<A0
        , 0xbeaaaa2aul  // -3.3333293e-01
        , 0x3e4c925ful  //  1.9991724e-01
        , 0xbe0e1b85ul  // -1.4031009e-01
        , 0x3da4f0d1ul  //  8.5460119e-02
        > (z);
    z1 = fma(x, z1*z, x);

      return y+(z1+more);
    }
  };
}
} }
#endif


/*
 * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: transforms.cpp 23275 2009-08-28 20:27:09Z mariusmuja $
 *
 */

/** \author Radu Bogdan Rusu */

#include <point_cloud_mapping/geometry/angles.h>
#include <point_cloud_mapping/geometry/transforms.h>
#include <point_cloud_mapping/geometry/nearest.h>

#include <Eigen/SVD>

namespace cloud_geometry
{
  namespace transforms
  {

  /**
   * See header file
   */
  bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const sensor_msgs::PointCloud& pc_b_,
		  Eigen::Matrix4d &transformation)
  {

	  assert (pc_a_.get_points_size()==pc_b_.get_points_size());

	  sensor_msgs::PointCloud pc_a = pc_a_;
	  sensor_msgs::PointCloud pc_b = pc_b_;

	  // translate both point cloud so that their centroid will be in origin
	  geometry_msgs::Point32 centroid_a;
	  geometry_msgs::Point32 centroid_b;

	  nearest::computeCentroid(pc_a, centroid_a);
	  nearest::computeCentroid(pc_b, centroid_b);

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  pc_a.points[i].x -= centroid_a.x;
		  pc_a.points[i].y -= centroid_a.y;
		  pc_a.points[i].z -= centroid_a.z;
	  }

	  for (size_t i=0;i<pc_b.get_points_size();++i) {
		  pc_b.points[i].x -= centroid_b.x;
		  pc_b.points[i].y -= centroid_b.y;
		  pc_b.points[i].z -= centroid_b.z;
	  }

	  // solve for rotation
	  Eigen::Matrix3d correlation;
	  correlation.setZero();

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
		  correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
		  correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;

		  correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
		  correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
		  correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;

		  correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
		  correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
		  correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
	  }

	  Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);

	  Eigen::Matrix3d Ut = svd.matrixU().transpose();
	  Eigen::Matrix3d V = svd.matrixV();
	  Eigen::Matrix3d X = V*Ut;

	  double det = X.determinant();
	  if (det<0) {
		  V.col(2) = -V.col(2);
		  X = V*Ut;
	  }

	  transformation.setZero();
	  transformation.topLeftCorner<3,3>() = X;
	  transformation(3,3) = 1;

	  sensor_msgs::PointCloud pc_rotated_a;
	  transformPoints(pc_a_.points, pc_rotated_a.points, transformation);

	  geometry_msgs::Point32 centroid_rotated_a;
	  nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);

	  transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
	  transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
	  transformation(2,3) = centroid_b.z - centroid_rotated_a.z;

	  return true;

  }

  /**
   * See header file
   */
	bool getPointsRigidTransformation(const sensor_msgs::PointCloud& pc_a_, const std::vector<int>& indices_a,
										  const sensor_msgs::PointCloud& pc_b_, const std::vector<int>& indices_b,
										  Eigen::Matrix4d &transformation)
	{

	  assert (indices_a.size()==indices_b.size());

	  sensor_msgs::PointCloud pc_a;
	  sensor_msgs::PointCloud pc_b;

	  getPointCloud(pc_a_, indices_a, pc_a);
	  getPointCloud(pc_b_, indices_b, pc_b);

	  // translate both point cloud so that their centroid will be in origin
	  geometry_msgs::Point32 centroid_a;
	  geometry_msgs::Point32 centroid_b;

	  nearest::computeCentroid(pc_a, centroid_a);
	  nearest::computeCentroid(pc_b, centroid_b);

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  pc_a.points[i].x -= centroid_a.x;
		  pc_a.points[i].y -= centroid_a.y;
		  pc_a.points[i].z -= centroid_a.z;
	  }

	  for (size_t i=0;i<pc_b.get_points_size();++i) {
		  pc_b.points[i].x -= centroid_b.x;
		  pc_b.points[i].y -= centroid_b.y;
		  pc_b.points[i].z -= centroid_b.z;
	  }

	  // solve for rotation
	  Eigen::Matrix3d correlation;
	  correlation.setZero();

	  for (size_t i=0;i<pc_a.get_points_size();++i) {
		  correlation(0,0) += pc_a.points[i].x * pc_b.points[i].x;
		  correlation(0,1) += pc_a.points[i].x * pc_b.points[i].y;
		  correlation(0,2) += pc_a.points[i].x * pc_b.points[i].z;

		  correlation(1,0) += pc_a.points[i].y * pc_b.points[i].x;
		  correlation(1,1) += pc_a.points[i].y * pc_b.points[i].y;
		  correlation(1,2) += pc_a.points[i].y * pc_b.points[i].z;

		  correlation(2,0) += pc_a.points[i].z * pc_b.points[i].x;
		  correlation(2,1) += pc_a.points[i].z * pc_b.points[i].y;
		  correlation(2,2) += pc_a.points[i].z * pc_b.points[i].z;
	  }

	  Eigen::JacobiSVD<Eigen::Matrix3d> svd(correlation);

	  Eigen::Matrix3d Ut = svd.matrixU().transpose();
	  Eigen::Matrix3d V = svd.matrixV();
	  Eigen::Matrix3d X = V*Ut;

	  double det = X.determinant();
	  if (det<0) {
		  V.col(2) = -V.col(2);
		  X = V*Ut;
	  }

	  transformation.setZero();
	  transformation.topLeftCorner<3,3>() = X;
	  transformation(3,3) = 1;

	  sensor_msgs::PointCloud pc_rotated_a;
	  getPointCloud(pc_a_, indices_a, pc_a);
	  transformPoints(pc_a.points, pc_rotated_a.points, transformation);

	  geometry_msgs::Point32 centroid_rotated_a;
	  nearest::computeCentroid(pc_rotated_a, centroid_rotated_a);

	  transformation(0,3) = centroid_b.x - centroid_rotated_a.x;
	  transformation(1,3) = centroid_b.y - centroid_rotated_a.y;
	  transformation(2,3) = centroid_b.z - centroid_rotated_a.z;

//	  transformation.setIdentity();
//	  transformation(0,3) = centroid_b.x - centroid_a.x;
//	  transformation(1,3) = centroid_b.y - centroid_a.y;
//	  transformation(2,3) = centroid_b.z - centroid_a.z;

	  return true;

  }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Obtain a 4x4 rigid transformation matrix (with translation)
      * \param plane_a the normalized coefficients of the first plane
      * \param plane_b the normalized coefficients of the second plane
      * \param tx the desired translation on x-axis
      * \param ty the desired translation on y-axis
      * \param tz the desired translation on z-axis
      * \param transformation the resultant transformation matrix
      */
    void
      getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const std::vector<double> &plane_b,
                                     float tx, float ty, float tz, Eigen::Matrix4d &transformation)
    {
      double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
      // Compute the rotation axis R = Nplane x (0, 0, 1)
      geometry_msgs::Point32 r_axis;
      r_axis.x = plane_a[1]*plane_b[2] - plane_a[2]*plane_b[1];
      r_axis.y = plane_a[2]*plane_b[0] - plane_a[0]*plane_b[2];
      r_axis.z = plane_a[0]*plane_b[1] - plane_a[1]*plane_b[0];

      if (r_axis.z < 0)
        angle = -angle;

      // Build a normalized quaternion
      double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
      double x = r_axis.x * s;
      double y = r_axis.y * s;
      double z = r_axis.z * s;
      double w = cos (0.5 * angle);

      // Convert the quaternion to a 3x3 matrix
      double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
      double wx = w * x; double wy = w * y; double wz = w * z;
      double xy = x * y; double xz = x * z; double yz = y * z;

      transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
      transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
      transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
      transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Obtain a 4x4 rigid transformation matrix (with translation)
      * \param plane_a the normalized coefficients of the first plane
      * \param plane_b the normalized coefficients of the second plane
      * \param tx the desired translation on x-axis
      * \param ty the desired translation on y-axis
      * \param tz the desired translation on z-axis
      * \param transformation the resultant transformation matrix
      */
    void
      getPlaneToPlaneTransformation (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b,
                                     float tx, float ty, float tz, Eigen::Matrix4d &transformation)
    {
      double angle = cloud_geometry::angles::getAngleBetweenPlanes (plane_a, plane_b);
      // Compute the rotation axis R = Nplane x (0, 0, 1)
      geometry_msgs::Point32 r_axis;
      r_axis.x = plane_a[1]*plane_b.z - plane_a[2]*plane_b.y;
      r_axis.y = plane_a[2]*plane_b.x - plane_a[0]*plane_b.z;
      r_axis.z = plane_a[0]*plane_b.y - plane_a[1]*plane_b.x;

      if (r_axis.z < 0)
        angle = -angle;

      // Build a normalized quaternion
      double s = sin (0.5 * angle) / sqrt (r_axis.x * r_axis.x + r_axis.y * r_axis.y + r_axis.z * r_axis.z);
      double x = r_axis.x * s;
      double y = r_axis.y * s;
      double z = r_axis.z * s;
      double w = cos (0.5 * angle);

      // Convert the quaternion to a 3x3 matrix
      double ww = w * w; double xx = x * x; double yy = y * y; double zz = z * z;
      double wx = w * x; double wy = w * y; double wz = w * z;
      double xy = x * y; double xz = x * z; double yz = y * z;

      transformation (0, 0) = xx - yy - zz + ww; transformation (0, 1) = 2*(xy - wz);       transformation (0, 2) = 2*(xz + wy);       transformation (0, 3) = tx;
      transformation (1, 0) = 2*(xy + wz);       transformation (1, 1) = -xx + yy -zz + ww; transformation (1, 2) = 2*(yz - wx);       transformation (1, 3) = ty;
      transformation (2, 0) = 2*(xz - wy);       transformation (2, 1) = 2*(yz + wx);       transformation (2, 2) = -xx -yy + zz + ww; transformation (2, 3) = tz;
      transformation (3, 0) = 0;                 transformation (3, 1) = 0;                 transformation (3, 2) = 0;                 transformation (3, 3) = 1;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Convert an axis-angle representation to a 3x3 rotation matrix
      * \note The formula is given by: A = I * cos (th) + ( 1 - cos (th) ) * axis * axis' - E * sin (th), where
      * E = [0 -axis.z axis.y; axis.z 0 -axis.x; -axis.y axis.x 0]
      * \param axis the axis
      * \param angle the angle
      * \param rotation the resultant rotation
      */
    void
      convertAxisAngleToRotationMatrix (const geometry_msgs::Point32 &axis, double angle, Eigen::Matrix3d &rotation)
    {
      double cos_a = cos (angle);
      double sin_a = sin (angle);
      double cos_a_m = 1.0 - cos_a;

      double a_xy = axis.x * axis.y * cos_a_m;
      double a_xz = axis.x * axis.z * cos_a_m;
      double a_yz = axis.y * axis.z * cos_a_m;

      double s_x = sin_a * axis.x;
      double s_y = sin_a * axis.y;
      double s_z = sin_a * axis.z;

      rotation (0, 0) = cos_a + axis.x * axis.x * cos_a_m;
      rotation (0, 1) = a_xy - s_z;
      rotation (0, 2) = a_xz + s_y;
      rotation (1, 0) = a_xy + s_z;
      rotation (1, 1) = cos_a + axis.y * axis.y * cos_a_m;
      rotation (1, 2) = a_yz - s_x;
      rotation (2, 0) = a_xz - s_y;
      rotation (2, 1) = a_yz + s_x;
      rotation (2, 2) = cos_a + axis.z * axis.z * cos_a_m;
    }

  }
}


#ifdef HOPS_GUROBI_FOUND

#include <Eigen/Core>

#include <hops/LinearProgram/LinearProgramGurobiImpl.hpp>
#include <hops/LinearProgram/GurobiEnvironmentSingleton.hpp>

namespace {
    std::vector<GRBVar> addVariablesToModel(GRBModel *model, size_t numberOfVariables) {
        std::vector<GRBVar> variables;
        for (size_t i = 0; i < numberOfVariables; ++i) {
            variables.emplace_back(model->addVar(
                    -GRB_INFINITY,
                    +GRB_INFINITY,
                    0,
                    GRB_CONTINUOUS,
                    "x_" + std::to_string(i))
            );
        }
        return variables;
    }

    void addLinearConstraints(const Eigen::MatrixXd &inequalityA,
                              const Eigen::VectorXd &inequalityB,
                              GRBModel *model,
                              const std::vector<GRBVar> &variables) {
        for (long i = 0; i < inequalityA.rows(); ++i) {
            GRBLinExpr expression;
            double coefficients[inequalityA.cols()];
            for (long j = 0; j < inequalityA.cols(); ++j) {
                coefficients[j] = inequalityA.coeff(i, j);
            }
            expression.addTerms(coefficients, &variables[0], inequalityA.cols());
            model->addConstr(expression, GRB_LESS_EQUAL, inequalityB(i), "row_" + std::to_string(i));
        }
    }

    void addObjective(const Eigen::VectorXd &objective, GRBModel *model,
                      const std::vector<GRBVar> &variables) {
        GRBLinExpr objectiveExpression = 0.0;
        objectiveExpression.addTerms(objective.data(), &variables[0], objective.rows());
        model->setObjective(objectiveExpression, GRB_MAXIMIZE);
    }

    hops::LinearProgramStatus parseGurobiStatus(int returnCode) {
        switch (returnCode) {
            case 2: {
                return hops::LinearProgramStatus::OPTIMAL;
            }
            case 3: {
                return hops::LinearProgramStatus::INFEASIBLE;
            }
            case 4: {
                return hops::LinearProgramStatus::UNDEFINED;
            }
            case 5: {
                return hops::LinearProgramStatus::UNBOUNDED;
            }
            default: {
                return hops::LinearProgramStatus::ERROR;
            }
        }
    }
}

hops::LinearProgramGurobiImpl::LinearProgramGurobiImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) :
        LinearProgram(A, b),
        model(std::make_unique<GRBModel>(GRBModel(GurobiEnvironmentSingleton::getInstance().getGurobiEnvironment()))) {
    variables = addVariablesToModel(model.get(), A.cols());
    addLinearConstraints(A, b, model.get(), variables);
    model->update();
}

hops::LinearProgramGurobiImpl::LinearProgramGurobiImpl(const hops::LinearProgramGurobiImpl &other) :
        LinearProgram(other.A, other.b),
        variables(other.variables) {
    model = std::make_unique<GRBModel>(*other.model);
}

hops::LinearProgramGurobiImpl &hops::LinearProgramGurobiImpl::operator=(const hops::LinearProgramGurobiImpl &other) {
    this->A = other.A;
    this->b = other.b;
    this->model = std::make_unique<GRBModel>(*other.model);
    this->variables = other.variables;
    return *this;
}

hops::LinearProgramSolution hops::LinearProgramGurobiImpl::solve(const Eigen::VectorXd &objective) const {
    addObjective(objective, model.get(), variables);
    model->update();

    try {
        model->optimize();

        int status = model->get(GRB_IntAttr_Status);

        if (status == GRB_INF_OR_UNBD) {
            model->set(GRB_IntParam_Presolve, 0);
            model->optimize();
            status = model->get(GRB_IntAttr_Status);
        }
        if (status == GRB_OPTIMAL) {
            double objectiveValue = model->get(GRB_DoubleAttr_ObjVal);
            auto numberOfColumns = model->get(GRB_IntAttr_NumVars);
            auto modelVariables = model->getVars();
            Eigen::VectorXd solution(numberOfColumns);
            for (int i = 0; i < numberOfColumns; ++i) {
                solution(i) = modelVariables[i].get(GRB_DoubleAttr_X);
            }
            return LinearProgramSolution(objectiveValue, solution, parseGurobiStatus(status));
        } else if (status == GRB_INFEASIBLE) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::INFEASIBLE);
        } else if (status == GRB_UNBOUNDED) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::UNBOUNDED);
        } else if (status == GRB_UNDEFINED) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::UNDEFINED);
        }
    }
    catch (const GRBException &e) {
        throw std::runtime_error("Gurobi encountered an exception: " + e.getMessage());
    }
    throw std::runtime_error("Exception: Gurobi failed to provide problem status or exception.");
}

std::tuple<Eigen::MatrixXd, Eigen::VectorXd>
hops::LinearProgramGurobiImpl::removeRedundantConstraints(double tolerance) {
    std::vector<long> constraintsToRemove;
    for (int i = 0; i < static_cast<int>(A.rows()); ++i) {
        GRBConstr constraintToTestForRedundancy = model->getConstrByName("row_" + std::to_string(i));
        model->remove(constraintToTestForRedundancy);
        model->update();
        GRBLinExpr constraintLHS;
        double coefficients[A.cols()];
        for (long j = 0; j < A.cols(); ++j) {
            coefficients[j] = A.coeff(i, j);
        }
        constraintLHS.addTerms(coefficients, &variables[0], A.cols());
        try {
            auto temporaryConstraint = model->addConstr(constraintLHS, GRB_LESS_EQUAL, b(i) + 10);
            model->update();
            auto solution = solve(A.row(i));
            model->remove(temporaryConstraint);
            model->update();

            if (solution.status != LinearProgramStatus::OPTIMAL || solution.objectiveValue + tolerance > b(i)) {
                model->addConstr(constraintLHS, GRB_LESS_EQUAL, b(i), "row_" + std::to_string(i));
                model->update();
            } else {
                constraintsToRemove.emplace_back(i);
            }
        }
        catch (GRBException &e) {
            std::cerr << "error code " << e.getErrorCode() << ": " << e.getMessage() << std::endl;
        }
    }

    std::vector<long> constraintsToKeep;
    for (long i = 0; i < A.rows(); ++i) {
        if (std::find(constraintsToRemove.begin(), constraintsToRemove.end(), i) == constraintsToRemove.end()) {
            constraintsToKeep.emplace_back(i);
        }
    }

    Eigen::MatrixXd newA(constraintsToKeep.size(), A.cols());
    Eigen::VectorXd newb(constraintsToKeep.size());
    for (size_t i = 0; i < constraintsToKeep.size(); ++i) {
        newb(i) = b(constraintsToKeep.at(i));
        newA.row(i) = A.row(constraintsToKeep.at(i));
    }

    *this = LinearProgramGurobiImpl(newA, newb);
    return std::make_tuple(A, b);
}

hops::LinearProgramSolution hops::LinearProgramGurobiImpl::computeChebyshevCenter() const {
    //Extend system by dimension for radius
    const long numberOfRows = A.rows();
    const long numberOfColumns = A.cols();
    Eigen::MatrixXd A_ext = A;
    Eigen::MatrixXd l_col(numberOfRows + 1, 1);
    l_col << A.rowwise().norm(), -1;
    A_ext.conservativeResize(numberOfRows + 1, numberOfColumns + 1);
    A_ext.row(numberOfRows) = Eigen::VectorXd::Zero(numberOfColumns + 1);
    A_ext.col(numberOfColumns) = l_col;
    Eigen::VectorXd b_ext = b;
    b_ext.conservativeResize(numberOfRows + 1);
    b_ext(numberOfRows) = 0;

    //make objective
    Eigen::VectorXd obj = Eigen::VectorXd::Zero(numberOfColumns + 1);
    obj(numberOfColumns) = 1;

    LinearProgramSolution chebyshevSolution = LinearProgramGurobiImpl(A_ext, b_ext).solve(obj);
    chebyshevSolution.optimalParameters.conservativeResize(A.cols());
    return chebyshevSolution;
}

std::vector<long> hops::LinearProgramGurobiImpl::computeUnconstrainedDimensions() const {
    std::vector<long> directions;
    for (long i = 0; i < A.cols(); ++i) {
        Eigen::VectorXd objective = Eigen::VectorXd::Zero(A.cols());
        objective(i) = 1.0;
        auto forwardSolution = solve(objective);
        if (forwardSolution.status != hops::LinearProgramStatus::OPTIMAL) {
            directions.push_back(i + 1);
        }

        auto backwardSolution = solve(-objective);
        if (backwardSolution.status != hops::LinearProgramStatus::OPTIMAL) {
            directions.push_back(-i - 1);
        }
    }
    return directions;
}

std::tuple<Eigen::MatrixXd, Eigen::VectorXd>
hops::LinearProgramGurobiImpl::addBoxConstraintsToUnconstrainedDimensions(double lb, double ub) {
    std::vector<long> unconstrainedDimensions = computeUnconstrainedDimensions();

    for (const auto &unconstrainedDimension : unconstrainedDimensions) {
        A.conservativeResize(A.rows() + 1, A.cols());
        A.row(A.rows() - 1) = Eigen::VectorXd::Zero(A.cols());
        b.conservativeResize(b.rows() + 1);
        if (unconstrainedDimension > 0) {
            A(A.rows() - 1, unconstrainedDimension - 1) = 1;
            b(b.rows() - 1) = ub;
        } else {
            A(A.rows() - 1, -unconstrainedDimension - 1) = -1;
            b(b.rows() - 1) = lb;
        }
    }
    *this = LinearProgramGurobiImpl(A, b);
    return std::make_tuple(A, b);
}

#endif //HOPS_GUROBI_FOUND


#ifndef CLOUD_CPP
#define CLOUD_CPP

#include <iostream>
#include "../include/cloud.h"
#include <Eigen/Dense>

using Eigen::MatrixXd;

Cloud::Cloud(float _Ox, float _Oy, float _Oz, size_t _total_points, float _dmin):
    Ox(_Ox), Oy(_Oy), Oz(_Oz), total_points(_total_points), dmin(_dmin)
{
    if( dmin < 0.0f ) dmin = 0.001f;
    if( Ox < 0.0f )   Ox   = 1.0f;
    if( Oy < 0.0f )   Oy   = 1.0f;
    if( Oz < 0.0f )   Oz   = 1.0f;
}

Cloud::~Cloud()
{
    into_points.clear();
}

bool
Cloud::build(QOpenGLShaderProgram* program)
{
    // BUILD CLOUD
    std::random_device rd;
    std::default_random_engine re(rd());

    // Generators
    std::uniform_real_distribution<float> xgen{-Ox, Ox};
    std::uniform_real_distribution<float> ygen{-Oy, Oy};
    std::uniform_real_distribution<float> zgen{-Oz, Oz};

    // Prepare deque to receive information
    into_points.clear();

    for(size_t i=0; i < total_points; ++i){
        float x = xgen(re);
        float y = ygen(re);
        float z = zgen(re);

        if( into(x, y, z) ){
            bool distance_ok = true;
            for(size_t j=0; j < into_points.size(); j+=3){
                float dist = distance(
                    x, y, z,
                    into_points[j+0],
                    into_points[j+1],
                    into_points[j+2]
                );

                if( dist <= dmin ){
                    distance_ok = false;
                    break;
                }
            }

            if( distance_ok ){
                into_points.push_back(x);
                into_points.push_back(y);
                into_points.push_back(z);
            }
        }
    }

    // Once we have all the correct points into our shape.
    // Build datas
    size_t npoints = into_points.size()/3;
    GLfloat* positions = new GLfloat[npoints*3];
    GLuint* indices = new GLuint[npoints];

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        positions[idx+0] = into_points[idx+0];
        positions[idx+1] = into_points[idx+1];
        positions[idx+2] = into_points[idx+2];
        indices[i] = GLuint(i);
    }

    // BUILD BUFFERS (will be sent to GPU with `update_buffers()` method.
    set_vertices_geometry(
        program->attributeLocation("position"), positions, indices);

    set_vertices_colors(
        program->attributeLocation("color"), new GLfloat[npoints*3]);

    return initialize(npoints, npoints, 3);
}

QVector3D
Cloud::compute_gravity_center() const
{
    QVector3D gcenter(0.0f, 0.0f, 0.0f);
    size_t npoints = into_points.size()/3;

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        gcenter += QVector3D(
            into_points[idx+0],
            into_points[idx+1],
            into_points[idx+2]
        );
    }

    return model_matrix() * (gcenter/float(npoints));
}

std::deque<float>
Cloud::compute_deviations() const
{
    std::deque<float> deviations(into_points.size());
    QVector3D gcenter = compute_gravity_center();
    size_t npoints = points_into_cloud();

    std::cerr << gcenter[0] << " " << gcenter[1] << " " << gcenter[2] << std::endl;

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);

        QVector3D p(
            into_points[idx+0],
            into_points[idx+1],
            into_points[idx+2]
        );

        p = model_matrix() * p;

        deviations[idx+0] = p[0] - gcenter.x();
        deviations[idx+1] = p[1] - gcenter.y();
        deviations[idx+2] = p[2] - gcenter.z();
    }

    return deviations;
}

std::deque<float>
Cloud::compute_correlation_matrix() const
{
    std::deque<float> deviations = compute_deviations();
    size_t npoints = points_into_cloud();

    //          | x0, y0, z0 |
    // matrix   | x1, y1, z1 |
    //          | x2, y2, z2 |
    //
    //          | x0, x1, x2 |
    // t_matrix | y0, y1, y2 |
    //          | z0, z1, z2 |
    std::deque<std::deque<float>> matrix(npoints);

    std::deque<std::deque<float>> t_matrix(3);
    t_matrix[0] = std::deque<float>(npoints); // x
    t_matrix[1] = std::deque<float>(npoints); // y
    t_matrix[2] = std::deque<float>(npoints); // z

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        matrix[i] = std::deque<float>(3);
        matrix[i][0] = t_matrix[0][i] = deviations[idx+0];
        matrix[i][1] = t_matrix[1][i] = deviations[idx+1];
        matrix[i][2] = t_matrix[2][i] = deviations[idx+2];
    }

    std::cerr << matrix.size() << " " << t_matrix[0].size() << std::endl;

    // Correlation matrix (out)[3*3] = t_matrix[3*npts] * matrix[npts*3];
    std::deque<float> out(9);
    for(size_t i=0; i < 3; ++i){
        for(size_t j=0; j < 3; ++j){
            out[(i*3)+j] = 0.0f;
            for(size_t h=0; h < npoints; ++h){
                out[(i*3)+j] += (t_matrix[i][h] * matrix[h][j]);
            }
        }
    }

    return out;
}

// Compute determinant of a matrix (3x3)
float
Cloud::compute_determinant(const std::deque<float>& mat)
{
    if(mat.size() != 9)
        return 0;

    return + mat[0]*((mat[4]*mat[8]) - (mat[5]*mat[7]))
           - mat[1]*((mat[3]*mat[8]) - (mat[5]*mat[6]))
           + mat[2]*((mat[3]*mat[7]) - (mat[4]*mat[6]));
}

//https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3%C3%973_matrices
std::deque<float>
Cloud::eigenvalues(const std::deque<float>& matrix)
{
    std::deque<float> eigenvalues(3);
    if( matrix.size() == 9 ){
        float r0 = (matrix[1]*matrix[1]) +
                   (matrix[2]*matrix[2]) +
                   (matrix[5]*matrix[5]);

        if( r0 == 0.0f ){
            eigenvalues[0] = matrix[0];
            eigenvalues[1] = matrix[4];
            eigenvalues[2] = matrix[8];
        }
        else {
            float q = (matrix[0] + matrix[4] + matrix[8])/3;
            float r1 = (
                std::pow(matrix[0] - q, 2.0f) +
                std::pow(matrix[4] - q, 2.0f) +
                std::pow(matrix[8] - q, 2.0f) +
                2 * r0
            );

            float p = std::sqrt(r1/6);
            std::deque<float> A_minus_LambdaI = matrix;
            A_minus_LambdaI[0] -= q;
            A_minus_LambdaI[4] -= q;
            A_minus_LambdaI[8] -= q;

            for(size_t i = 0; i < 9; ++i)
                A_minus_LambdaI[i] *= (1/p);

            float det = Cloud::compute_determinant(A_minus_LambdaI)/2.0f;
            float phi;
            const float PI = std::atan(1.0f) * 4.0f;

            if( det <= -1 ){
                phi = PI / 3.0f;
            }
            else
            if( det >= 1 ){
                phi = 0.0f;
            }
            else {
                phi = std::acos(det) / 3.0f;
            }

            eigenvalues[0] = q + 2 * p * std::cos(phi);
            eigenvalues[2] = q + 2 * p * std::cos(phi + (2*PI/3));
            eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
        }
    }

    return eigenvalues;
}

// Naive test
bool
Cloud::into(float x, float y, float z)
{
    return (  (x >= -Ox && x <= Ox)
           && (y >= -Oy && y <= Oy)
           && (z >= -Oz && z <= Oz) );
}

// Compute euclidean distance between two 3D points.
float
Cloud::distance(float x, float y, float z, float xx, float yy, float zz)
{
    return std::sqrt(
                std::pow(xx-x, 2.0f) +
                std::pow(yy-y, 2.0f) +
                std::pow(zz-z, 2.0f));
}

// ELLIPSOID CLOUD CLASS
EllipsoidCloud::EllipsoidCloud(float _Ox, float _Oy, float _Oz, size_t _total_points, float _dmin):
    Cloud(_Ox, _Oy, _Oz, _total_points, _dmin)
{}

// Maths from https://en.wikipedia.org/wiki/Ellipsoid
bool
EllipsoidCloud::into(float x, float y, float z)
{
    return (((x*x)/(Ox*Ox)) + ((y*y)/(Oy*Oy)) + ((z*z)/(Oz*Oz))) <= 1.0f;
}

#endif // CLOUD_CPP


#include <bsplines/BSplinePose.hpp>
#include <sm/assert_macros.hpp>
// boost::tie
#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>

namespace bsplines {
  using namespace sm::kinematics;

    // given T, T'\in\mathrm{SE}(3), define Exp_{T}(h), h\in\mathbb{R}^6 , Log_{T}(T')
    // [p,\theta]\in\mathbb{R}^6 , \mathrm{SE}(3) \ni T = Exp[p, \theta] = [exp(-\theta) p]
    //                                                                     [      0      1]
    // T = [R t] \in\mathrm{SE}(3), Log T = [t  -log(R)]
    //     [0 1]
    // T \oplus [a, b] = Exp_{T}([a, b]) = Exp[a, b] * T
    // T1 \ominus T2 = Log_{T2}(T1) = Log(T1 * T2^{-1})
    BSplinePose::BSplinePose(int splineOrder, const RotationalKinematics::Ptr & rotationalKinematics)
      : BSpline(splineOrder), rotation_(rotationalKinematics)
    {

    }

    BSplinePose::~BSplinePose()
    {

    }

    Eigen::Matrix4d BSplinePose::transformation(double tk) const
    {
      return curveValueToTransformation(eval(tk));
    }

    // T: transformation  p: axis angle  C: spline control points
    // dT/dC = dT/dp * dp/dC
    Eigen::Matrix4d BSplinePose::transformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JS;  // dp/dC
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;  // dT/dp
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );

      if(J)
      {
        *J = JT * JS;
      }

      return T;
    }

    // Log_{O(\theta)}O(\theta + \delta) = J\delta
    // => J\delta = Log(O(\theta + \delta) * O(\theta)^{-1})
    //            = Log(exp(-(\theta + \delta)) * exp(\theta))
    //            = Log(exp(-Jr(\theta)\delta))
    //            = Jr(\theta)\delta
    Eigen::Matrix3d BSplinePose::orientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
      {
        *J = JO * JS;
      }

      return C;

    }

    // Log_{IO(\theta)}IO(\theta + \delta) = J\delta
    // => J\delta = Log(IO(\theta + \delta) * IO(\theta)^{-1})
    //            = Log(exp(\theta + \delta) * exp(-\theta))
    //            = Log(Jl(\theta)\delta)
    //            = -Jl(\theta)\delta
    //            = -exp(\theta)Jr(\theta)\delta
    Eigen::Matrix3d BSplinePose::inverseOrientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S).transpose();

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
      {
        *J = -C * JO * JS;
      }

      return C;

    }

    Eigen::Matrix4d BSplinePose::inverseTransformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      //std::cout << __FUNCTION__ << "()\n";
      //ASRL_THROW(std::runtime_error,"Not Implemented");
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );
      // Invert the transformation.
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();

      if(J)
      {
        // The "box times" is the linearized transformation way of inverting the jacobian.
        *J = -sm::kinematics::boxTimes(T) * JT * JS;
      }

      if(coefficientIndices)
      {
        *coefficientIndices = localCoefficientVectorIndices(tk);
      }

      return T;
    }

    Eigen::Matrix4d BSplinePose::inverseTransformation(double tk) const
    {
      Eigen::Matrix4d T = curveValueToTransformation(eval(tk));
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();
      return T;
    }

    // d(Tv)/dC = d(Tv)/dT * dT/dp * dp/dC
    // d(Tv)/dT s.t. Exp_{T}(h)v - Tv = d(Tv)/dT * h
    Eigen::Vector4d BSplinePose::transformVectorAndJacobian(double tk, const Eigen::Vector4d & v_tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JT;
      Eigen::Matrix4d T_n_vk = transformationAndJacobian(tk, &JT, coefficientIndices);
      Eigen::Vector4d v_n = T_n_vk * v_tk;

      if(J)
      {
        *J = sm::kinematics::boxMinus(v_n) * JT;
      }

      return v_n;
    }

    Eigen::Vector4d BSplinePose::inverseTransformVectorAndJacobian(double tk, const Eigen::Vector4d & v_tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JT;
      Eigen::Matrix4d T_n_vk = inverseTransformationAndJacobian(tk, &JT, coefficientIndices);
      Eigen::Vector4d v_n = T_n_vk * v_tk;

      if(J)
      {
        *J = sm::kinematics::boxMinus(v_n) * JT;
      }

      return v_n;
    }

    Eigen::Vector3d BSplinePose::position(double tk) const
    {
      return eval(tk).head<3>();
    }

    Eigen::Matrix3d BSplinePose::orientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>());
    }

    Eigen::Matrix3d BSplinePose::inverseOrientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>()).transpose();
    }

    Eigen::Vector3d BSplinePose::linearVelocity(double tk) const
    {
      return evalD(tk,1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearVelocityBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAcceleration(double tk) const
    {
      return evalD(tk,2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d a = evalDAndJacobian(tk,2,J,coefficientIndices).head<3>();
      if(J)
      {
        J->conservativeResize(3,J->cols());
      }
      return a;
    }

    // {}_w\omege_{wb}
    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocity(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);

      omega = -rotation_->parametersToSMatrix(r.tail<3>()) * v.tail<3>();
      return omega;
    }

    // {}_b\omege_{wb}
    // \omega_w_{b,b} (angular velocity of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);
      Eigen::Matrix3d S;
      Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

      omega = -C_w_b.transpose() * S * v.tail<3>();
      return omega;

    }

    // \omega_w_{b,b} (angular velocity of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd Jr;
      Eigen::Matrix3d C_b_w = inverseOrientationAndJacobian(tk,&Jr,NULL);

      Eigen::Vector3d omega = angularVelocityAndJacobian(tk, J, coefficientIndices);
      omega = C_b_w * omega;

      if(J)
      {
        *J = C_b_w * (*J) + sm::kinematics::crossMx(omega) * Jr;
      }

      return omega;
    }

    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocityAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d omega;
      Eigen::Vector3d p;
      Eigen::Vector3d pdot;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jpdot;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      pdot = evalDAndJacobian(tk,1,&Jpdot,coefficientIndices).tail<3>();

      // Rearrange the spline jacobian matrices. Now Jpdot is the
      // jacobian of p wrt the spline coefficients stacked on top
      // of the jacobian of pdot wrt the spline coefficients.
      Jpdot.block(0,0,3,Jpdot.cols()) = Jp.block(3,0,3,Jp.cols());

      //std::cout << "Jpdot\n" << Jpdot << std::endl;
      // \omega = -Jr(\theta) * \dot{\theta}
      // \theta, \dot{\theta} are independent, then
      //    d\omega/dC = d\omega/d\theta * d\theta/dC + d\omega/d\dot{\theta} * d\dot{\theta}/dC
      //  d\omega/d\theta * d\theta/dC + d\omega/d\dot{\theta} * d\dot{\theta}/dC
      // = -R * [d(Jr(\theta)\dot{\theta})/d\theta   Jr(\theta)] * [    d\theta/dC    ]
      //                                                           [ d\dot{\theta}/dC ]
      Eigen::Matrix<double,3,6> Jo;
      omega = -rotation_->angularVelocityAndJacobian(p,pdot,&Jo);

      //std::cout << "Jo:\n" << Jo << std::endl;
      if(J)
      {
        *J = -Jo * Jpdot;
      }

      return omega;
    }

    // {}_b\dot\omega_{wb}
    // \omega_dot_w_{b,b} (angular acceleration of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularAccelerationBodyFrame(double tk) const
    {
    	Eigen::Vector3d p = evalD(tk,0).tail<3>();
      Eigen::Vector3d v = evalD(tk,1).tail<3>();
    	Eigen::Vector3d a = evalD(tk,2).tail<3>();
    	Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(p);

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      return -C_w_b.transpose() * (Jo * va);
    }

    // \omega_dot_w_{b,b} (angular acceleration of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularAccelerationBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd Jr;
    	Eigen::Matrix3d C_b_w = inverseOrientationAndJacobian(tk,&Jr,NULL);

      Eigen::Vector3d acce = angularAccelerationAndJacobian(tk, J, coefficientIndices);
      acce = C_b_w * acce;

      if(J)
    	{
    		*J = C_b_w * (*J) + sm::kinematics::crossMx(acce) * Jr;
    	}

      return acce;
    }

    // {}_w\dot\omega_{wb}
    // \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularAcceleration(double tk) const
    {
    	Eigen::Vector3d p = evalD(tk,0).tail<3>();
      Eigen::Vector3d v = evalD(tk,1).tail<3>();
    	Eigen::Vector3d a = evalD(tk,2).tail<3>();

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      return -(Jo * va);
    }

    // \todo Only support RotationVector by now
    // \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Vector3d p;
      Eigen::Vector3d v;
    	Eigen::Vector3d a;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jv;
      Eigen::MatrixXd Ja;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      v = evalDAndJacobian(tk,1,&Jv,NULL).tail<3>();
      a = evalDAndJacobian(tk,2,&Ja,coefficientIndices).tail<3>();

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      Eigen::Vector3d acce = -(Jo * va);

      // Rearrange the spline jacobian matrices. Now Ja is the
    	// jacobian of p wrt the spline coefficients stacked on top
    	// of the jacobian of a wrt the spline coefficients.
    	Ja.block(0,0,3,Ja.cols()) = Jp.block(3,0,3,Jp.cols());

      // Rearrange the spline jacobian matrices. Now Jv is the
    	// jacobian of p wrt the spline coefficients stacked on top
    	// of the jacobian of v wrt the spline coefficients.
    	Jv.block(0,0,3,Jv.cols()) = Jp.block(3,0,3,Jp.cols());

      rotation_->angularVelocityAndJacobian(p,a,&Jo);

      // f(\theta, \dot\theta) = \frac{\partial Jr(\theta)\dot\theta}{\partial\theta}\dot\theta
      // [ \frac{\partial f}{\partial\theta}  \frac{\partial f}{\partial\dot\theta} ]
      Eigen::Matrix<double,3,6> Jpp;

      double factor[5];
      const double pv = p.dot(v);
      const double v2 = v.squaredNorm();
      const double p2 = p.squaredNorm();
      if(p2 < 1e-14)
      {
        const double p4 = p2 * p2;
        // Series[(x Sin[x] + 2 Cos[x] - 2) / x^4, {x, 0, 4}]
        factor[0] = -1.0 / 12 + p2 / 180 - p4 / 6720;
        // Series[(x - Sin[x]) / x^3, {x, 0, 4}]
        factor[1] = 1.0 / 6 - p2 / 120 + p4 / 5040;
        // Series[(3 Sin[x] - x Cos[x] - 2 x) / (x^5), {x, 0, 4}]
        factor[2] = -1.0 / 60 + p2 / 1260 - p4 / 60480;
        // Series[((x^2 - 8) Cos[x] - 5 x Sin[x] + 8) / x^6, {x, 0, 4}]
        factor[3] = 1.0 / 90 - p2 / 1680 + p4 / 75600;
        // Series[((x^2 - 15) Sin[x] + 7 x Cos[x] + 8 x) / x^7, {x, 0, 4}]
        factor[4] = 1.0 / 630 - p2 / 15120 + p4 / 831600;
      }
      else
      {
        const double p1 = std::sqrt(p2);
        const double p3 = p1 * p2;
        const double p4 = p1 * p3;
        const double p5 = p1 * p4;
        const double p6 = p1 * p5;
        const double p7 = p1 * p6;
        const double cosp = std::cos(p1);
        const double sinp = std::sin(p1);

        factor[0] = (p1 * sinp + 2 * cosp - 2) / p4;
        factor[1] = (p1 - sinp) / p3;
        factor[2] = (3 * sinp - p1 * cosp - 2 * p1) / p5;
        factor[3] = ((p2 - 8) * cosp - 5 * p1 * sinp + 8) / p6;
        factor[4] = ((p2 - 15) * sinp + 7 * p1 * cosp + 8 * p1) / p7;
      }

      Jpp.leftCols<3>() = factor[0] * sm::kinematics::crossMx(v) * (pv * Eigen::Matrix3d::Identity() + p * v.transpose()) +
                          factor[1] * (v2 * Eigen::Matrix3d::Identity() - v * v.transpose()) +
                          factor[2] * (pv * pv * Eigen::Matrix3d::Identity() + 2 * pv * p * v.transpose() - 2 * pv * v * p.transpose() - p2 * v * v.transpose()) +
                          factor[3] * pv * sm::kinematics::crossMx(v) * p * p.transpose() +
                          factor[2] * (v2 * p * p.transpose() - pv * v * p.transpose()) +
                          factor[4] * (pv * pv * p * p.transpose() - pv * p2 * v * p.transpose());
      Jpp.rightCols<3>() = factor[0] * (sm::kinematics::crossMx(v) * p * p.transpose() - pv * sm::kinematics::crossMx(p)) +
                           factor[1] * (2 * p * v.transpose() - v * p.transpose() - pv * Eigen::Matrix3d::Identity()) +
                           factor[2] * (2 * pv * p * p.transpose() - pv * p2 * Eigen::Matrix3d::Identity() - p2 * v * p.transpose());

      if(J)
    	{
    		*J = -Jpp * Jv - Jo * Ja;
    	}

      return acce;
    }

    void BSplinePose::initPoseSpline(double t0, double t1, const Eigen::Matrix4d & T_n_t0, const Eigen::Matrix4d & T_n_t1)
    {
      Eigen::VectorXd v0 = transformationToCurveValue(T_n_t0);
      Eigen::VectorXd v1 = transformationToCurveValue(T_n_t1);

      initSpline(t0,t1,v0,v1);
    }

    void BSplinePose::addPoseSegment(double tk, const Eigen::Matrix4d & T_n_tk)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment(tk, vk);
    }

    void BSplinePose::addPoseSegment2(double tk, const Eigen::Matrix4d & T_n_tk, double lambda)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment2(tk, vk, lambda);
    }

    Eigen::Matrix4d BSplinePose::curveValueToTransformation( const Eigen::VectorXd & c ) const
    {
      SM_ASSERT_EQ_DBG(Exception, c.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(c.tail<3>());
      T.topRightCorner<3,1>() = c.head<3>();

      return T;
    }

    // dT/dp s.t. T(p + \delta) = Exp(dT/dp \delta)T(p)
    Eigen::Matrix4d BSplinePose::curveValueToTransformationAndJacobian( const Eigen::VectorXd & p, Eigen::MatrixXd * J ) const
    {
      SM_ASSERT_EQ_DBG(Exception, p.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      Eigen::Matrix3d S;
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);
      T.topRightCorner<3,1>() = p.head<3>();

      if(J)
      {
        *J = Eigen::MatrixXd::Identity(6,6);
        J->topRightCorner<3,3>() = -crossMx(p.head<3>()) * S;
        J->bottomRightCorner<3,3>() = S;
      }

      return T;

    }

    Eigen::VectorXd BSplinePose::transformationToCurveValue( const Eigen::Matrix4d & T ) const
    {
      Eigen::VectorXd c(6);
      c.head<3>() = T.topRightCorner<3,1>();
      c.tail<3>() = rotation_->rotationMatrixToParameters(T.topLeftCorner<3,3>());

      return c;
    }

    void BSplinePose::initPoseSpline2(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline2(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSpline3(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline3(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparse(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSplineSparse(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparseKnots(const Eigen::VectorXd &times, const Eigen::MatrixXd &interpolationPoints, const Eigen::VectorXd knots, double lambda)
    {
    	initSplineSparseKnots(times, interpolationPoints, knots, lambda);
    }

    RotationalKinematics::Ptr BSplinePose::rotation() const
    {
      return rotation_;
    }
  } // namespace bsplines


/**
 * laminate_main reads the input file `laminate_input` and `material_data`,
 * construct a laminate model containing material properties and response to
 * the load, and save the stress and strain profile into
 * `laminate_profile_data.txt`, and save the A,B, and D submatrices into
 * `stifness_submatrices ABD.txt`.
 *
 * `laminate_profile_data.txt` contains the following columns (in order):
 * Coordinates, stress_x, stress_y, stress_xy, strain_x, strain_y, strain_xy.
 *
 * `stiffness_submatrices_ABD.txt` contains the A, B, and D submatrices of the
 * composite laminate, in the given order separate by new lines.
 *
 */

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include "../include/input_parser.h"
#include "../include/ply.h"
#include "../include/laminate.h"

void save_laminate_profile(laminate& lam);

int main() {
    std::vector<std::string> input_strings =
        read_composite_input("input_files/laminate_input.lmc");
    std::vector<ply> ply_vector =
        get_ply_vector(input_strings, "input_files/material_data.lmc");
    Eigen::Matrix<double, 6, 1> load_vector = get_load_vector(input_strings[3]);
    double min_thickness = get_minimum_ply_thickness(input_strings[2]);
    double pt_spacing = min_thickness/20.;
    laminate lam(ply_vector, load_vector, pt_spacing);
    save_laminate_profile(lam);
    std::cout << "Laminate_main -- Data saved." << std::endl;
    return 0;
}

void save_laminate_profile(laminate& lam) {
    std::ofstream profile_file;
    profile_file.open("output_files/laminate_profile_data.txt");
    auto i_sigma = lam.stresses_.begin();
    auto i_eps = lam.strains_.begin();
    auto i_pt = lam.profile_pt_.begin();
    for (; i_sigma != lam.stresses_.end(); i_sigma++, i_eps++, i_pt++) {
        Eigen::Matrix<double, 7, 1> combined_data;
        combined_data << *i_pt, *i_sigma, *i_eps;

        Eigen::Map<Eigen::Matrix<double, 1, 7>>
            row(combined_data.data(), combined_data.size());
        profile_file << row << std::endl;
    }
    std::ofstream stiffness_file;
    stiffness_file.open("output_files/stiffness_submatrices ABD.txt");
    stiffness_file << lam.A_;
    stiffness_file << std::endl<< std::endl;
    stiffness_file << lam.B_;
    stiffness_file << std::endl << std::endl;
    stiffness_file << lam.D_;
    stiffness_file << std::endl << std::endl;
}


#ifdef COMPILATION// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4-*-
$CXX -DNDEBUG $0 -o $0x -lboost_timer&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2019-2020

#include "../array.hpp"

#include<iostream>
#include<vector>
#include<numeric> // iota
#include<algorithm>

namespace multi = boost::multi;
using std::cout;

template<class Matrix, class Vector, class idx = typename std::decay_t<Vector>::difference_type>
auto gj_solve(Matrix&& A, Vector&& y)->decltype(y[0]/=A[0][0], y){
	idx Asize = size(A);
	for(idx r = 0; r != Asize; ++r){
		auto&& Ar = A[r]; auto const& Arr = Ar[r];
		for(idx c = r + 1; c != Asize; ++c) Ar[c] /= Arr;
		auto const& yr = (y[r] /= Arr);
		for(idx r2 = r + 1; r2 != Asize; ++r2){
			auto&& Ar2 = A[r2]; auto const& Ar2r = A[r2][r];
			for(idx c = r + 1; c != Asize; ++c) Ar2[c] -= Ar2r*Ar[c];
			y[r2] -= Ar2r*yr;
		}
	}
	for(idx r = Asize - 1; r > 0; --r){
		auto const& yr = y[r];
		for(idx r2 = r-1; r2 >=0; --r2) y[r2] -= A[r2][r]*yr;
	}
	return y;
}

template<class Matrix, class Vector, class idx = typename std::decay_t<Vector>::difference_type>
auto gj_solve2(Matrix&& A, Vector&& y)->decltype(y[0]/=A[0][0], y){
	idx Asize = size(A);
	for(idx r = 0; r != Asize; ++r){
		auto&& Ar = A[r]; auto const& Arr = Ar[r];
	//	std::transform(Ar.begin() + r + 1, Ar.end(), Ar.begin() + r + 1, [&](auto const& a){return a/Arr;});
		for(idx c = r + 1; c != Asize; ++c) Ar[c] /= Arr;
		auto const& yr = (y[r] /= Arr);
		for(idx r2 = r + 1; r2 != Asize; ++r2){
			auto&& Ar2 = A[r2]; auto const& Ar2r = A[r2][r];
			std::transform(std::move(Ar2).begin() + r + 1, std::move(Ar2).end(), std::move(Ar).begin() + r + 1, std::move(Ar2).begin() + r + 1, [&](auto&& a, auto&& b){return a - Ar2r*b;});
			y[r2] -= Ar2r*yr;
		}
	}
	for(idx r = Asize - 1; r > 0; --r){
		auto const& yr = y[r];
		for(idx r2 = r-1; r2 >=0; --r2) y[r2] -= A[r2][r]*yr;
	}
	return y;
}

#include <boost/timer/timer.hpp>

int main(){
	{
		multi::array<double, 2> A = {{-3., 2., -4.},{0., 1., 2.},{2., 4., 5.}};
		multi::array<double, 1> y = {12.,5.,2.}; //(M); assert(y.size() == M); iota(y.begin(), y.end(), 3.1);
		gj_solve(A, y);
		cout << y[0] <<" "<< y[1] <<" "<< y[2] << std::endl;
	}
	{
		multi::array<double, 2> A({6000, 7000}); std::iota(A.data(), A.data() + A.num_elements(), 0.1);
		std::transform(A.data(), A.data() + A.num_elements(), A.data(), [](auto x){return x/=2.e6;});
		std::vector<double> y(3000); std::iota(y.begin(), y.end(), 0.2);
		{
			boost::timer::auto_cpu_timer t;
			gj_solve(A({1000, 4000}, {0, 3000}), y);
		}
		cout << y[45] << std::endl;
	}
	{
		multi::array<double, 2> A({6000, 7000}); std::iota(A.data(), A.data() + A.num_elements(), 0.1);
		std::transform(A.data(), A.data() + A.num_elements(), A.data(), [](auto x){return x/=2.e6;});
		std::vector<double> y(3000); std::iota(y.begin(), y.end(), 0.2);
		{
			boost::timer::auto_cpu_timer t;
			gj_solve2(A({1000, 4000}, {0, 3000}), y);
		}
		cout << y[45] << std::endl;
	}
}