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

#ifndef SMOOTH__LIE_GROUP_HPP_
#define SMOOTH__LIE_GROUP_HPP_

#include <Eigen/Core>

#include <concepts>

#include "manifold.hpp"

/**
 * @file lie_group.hpp Internal and external LieGroup interfaces and free LieGroup functions.
 */

namespace smooth {

namespace traits {

/**
 * @brief Trait class for making a class a LieGroup instance via specialization.
 */
template<typename T>
struct lie;

}  // namespace traits

// clang-format off

/**
 * @brief Class-external Lie group interface defined through the traits::lie trait class.
 */
template<typename G>
concept LieGroup =
requires (Eigen::Index dof) {
  // Underlying scalar type
  typename traits::lie<G>::Scalar;
  // Default representation
  typename traits::lie<G>::PlainObject;
  // Compile-time degrees of freedom (tangent space dimension). Can be dynamic (equal to -1)
  {traits::lie<G>::Dof}->std::convertible_to<Eigen::Index>;
  // Commutativity
  {traits::lie<G>::IsCommutative}->std::convertible_to<bool>;
  // Return the identity element (dof = Dof for static size)
  {traits::lie<G>::Identity(dof)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Return a random element (dof = Dof for static size)
  {traits::lie<G>::Random(dof)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
} &&
// GROUP INTERFACE
requires(const G & g1, const G & g2, typename traits::lie<G>::Scalar eps) {
  // Group adjoint
  {traits::lie<G>::Ad(g1)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Group composition
  {traits::lie<G>::composition(g1, g2)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Run-time degrees of freedom (tangent space dimension).
  {traits::lie<G>::dof(g1)}->std::convertible_to<Eigen::Index>;
  // Group inverse
  {traits::lie<G>::inverse(g1)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Check if two elements are (approximately) equal
  {traits::lie<G>::isApprox(g1, g2, eps)}->std::convertible_to<bool>;
  // Group logarithm (maps from group to algebra)
  {traits::lie<G>::log(g1)}->std::convertible_to<Eigen::Vector<typename traits::lie<G>::Scalar, traits::lie<G>::Dof>>;
} &&
// TANGENT INTERFACE
requires(const Eigen::Vector<typename traits::lie<G>::Scalar, traits::lie<G>::Dof> & a) {
  // Algebra adjoint
  {traits::lie<G>::ad(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Algebra exponential (maps from algebra to group)
  {traits::lie<G>::exp(a)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Right derivative of the exponential map
  {traits::lie<G>::dr_exp(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Right derivative of the exponential map inverse
  {traits::lie<G>::dr_expinv(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Second right derivative of the exponential map
  {traits::lie<G>::d2r_exp(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, (traits::lie<G>::Dof > 0 ? traits::lie<G>::Dof * traits::lie<G>::Dof : -1)>>;
  // Second right derivative of the exponential map inverse
  {traits::lie<G>::d2r_expinv(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, (traits::lie<G>::Dof > 0 ? traits::lie<G>::Dof * traits::lie<G>::Dof : -1)>>;
} && (
  // Cast to different scalar type
  !std::is_convertible_v<typename traits::lie<G>::Scalar, double> ||
  requires (const G & g) {
    {traits::lie<G>::template cast<double>(g)};
  }
) && (
  !std::is_convertible_v<typename traits::lie<G>::Scalar, float> ||
  requires (const G & g) {
    {traits::lie<G>::template cast<double>(g)};
  }
) &&
// PlainObject must be default-constructible
std::is_default_constructible_v<typename traits::lie<G>::PlainObject> &&
std::is_copy_constructible_v<typename traits::lie<G>::PlainObject> &&
// PlainObject must be assignable from G
std::is_assignable_v<typename traits::lie<G>::PlainObject &, G>;

////////////////////////////////////////////////
//// Lie group interface for NativeLieGroup ////
////////////////////////////////////////////////

/**
 * @brief Concept defining class with an internal Lie group interface.
 *
 * Concept satisfied if G has members that correspond to the LieGroup concept.
 */
template<typename G>
concept NativeLieGroup = requires
{
  typename G::Scalar;
  typename G::Tangent;
  typename G::PlainObject;
  {G::Dof}->std::convertible_to<Eigen::Index>;
  {G::IsCommutative}->std::convertible_to<bool>;
} &&
(!(G::Dof > 0) || requires {
  {G::Identity()}->std::convertible_to<typename G::PlainObject>;
  {G::Random()}->std::convertible_to<typename G::PlainObject>;
}) &&
(!(G::Dof == -1) || requires (Eigen::Index dof) {
  {G::Identity(dof)}->std::convertible_to<typename G::PlainObject>;
  {G::Random(dof)}->std::convertible_to<typename G::PlainObject>;
}) &&
(G::Tangent::SizeAtCompileTime == G::Dof) &&
requires(const G & g1, const G & g2, typename G::Scalar eps) {
  {g1.dof()}->std::convertible_to<Eigen::Index>;  // degrees of freedom at runtime
  {g1.Ad()}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {g1 * g2}->std::convertible_to<typename G::PlainObject>;
  {g1.inverse()}->std::convertible_to<typename G::PlainObject>;
  {g1.isApprox(g2, eps)}->std::convertible_to<bool>;
  {g1.log()}->std::convertible_to<Eigen::Vector<typename G::Scalar, G::Dof>>;
} &&
requires(const Eigen::Vector<typename G::Scalar, G::Dof> & a) {
  {G::ad(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::exp(a)}->std::convertible_to<typename G::PlainObject>;
  {G::dr_exp(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::dr_expinv(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::d2r_exp(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof * G::Dof>>;
  {G::d2r_expinv(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof * G::Dof>>;
};

// clang-format on

namespace traits
{

/**
 * @brief LieGroup interface for NativeLieGroup
 */
template<NativeLieGroup G>
struct lie<G>
{
  // \cond
  using Scalar = typename G::Scalar;
  template<typename NewScalar>
  using CastT       = typename G::template CastT<NewScalar>;
  using PlainObject = typename G::PlainObject;

  static constexpr Eigen::Index Dof   = G::Dof;
  static constexpr bool IsCommutative = G::IsCommutative;

  // group interface

  static inline PlainObject Identity([[maybe_unused]] Eigen::Index dof)
  {
    if constexpr (G::Dof == -1) {
      return G::Identity(dof);
    } else {
      return G::Identity();
    }
  }
  static inline PlainObject Random([[maybe_unused]] Eigen::Index dof)
  {
    if constexpr (G::Dof == -1) {
      return G::Random(dof);
    } else {
      return G::Random();
    }
  }
  static inline typename G::TangentMap Ad(const G & g) { return g.Ad(); }
  template<NativeLieGroup Go>
  static inline PlainObject composition(const G & g1, const Go & g2)
  {
    return g1.operator*(g2);
  }
  static inline Eigen::Index dof(const G &) { return G::Dof; }
  static inline PlainObject inverse(const G & g) { return g.inverse(); }
  template<NativeLieGroup Go>
  static inline bool isApprox(const G & g, const Go & go, Scalar eps)
  {
    return g.isApprox(go, eps);
  }
  static inline typename G::Tangent log(const G & g) { return g.log(); }
  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const G & g)
  {
    return g.template cast<NewScalar>();
  }

  // tangent interface

  template<typename Derived>
  static inline typename G::TangentMap ad(const Eigen::MatrixBase<Derived> & a)
  {
    return G::ad(a);
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::exp(a);
  }
  template<typename Derived>
  static inline typename G::TangentMap dr_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::dr_exp(a);
  }
  template<typename Derived>
  static inline typename G::TangentMap dr_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return G::dr_expinv(a);
  }
  template<typename Derived>
  static inline typename G::Hessian d2r_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::d2r_exp(a);
  }
  template<typename Derived>
  static inline typename G::Hessian d2r_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return G::d2r_expinv(a);
  }
  // \endcond
};

///////////////////////////////////////////////
//// Lie group interface for Eigen vectors ////
///////////////////////////////////////////////

/**
 * @brief LieGroup interface for RnType
 */
template<RnType G>
struct lie<G>
{
  // \cond
  static constexpr int Dof            = G::SizeAtCompileTime;
  static constexpr bool IsCommutative = true;

  using Scalar      = typename G::Scalar;
  using PlainObject = Eigen::Vector<Scalar, Dof>;
  template<typename NewScalar>
  using CastT = Eigen::Vector<NewScalar, Dof>;

  // group interface

  static inline PlainObject Identity(Eigen::Index dof) { return G::Zero(dof); }
  static inline PlainObject Random(Eigen::Index dof) { return G::Random(dof); }
  static inline Eigen::Matrix<Scalar, Dof, Dof> Ad(const G & g)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(g.size(), g.size());
  }
  template<typename Derived>
  static inline PlainObject composition(const G & g1, const Eigen::MatrixBase<Derived> & g2)
  {
    return g1 + g2;
  }
  static inline Eigen::Index dof(const G & g) { return g.size(); }
  static inline PlainObject inverse(const G & g) { return -g; }
  template<typename Derived>
  static inline bool isApprox(const G & g, const Eigen::MatrixBase<Derived> & g2, Scalar eps)
  {
    return g.isApprox(g2, eps);
  }
  static inline Eigen::Vector<Scalar, Dof> log(const G & g) { return g; }
  template<typename NewScalar>
  static inline Eigen::Vector<NewScalar, Dof> cast(const G & g)
  {
    return g.template cast<NewScalar>();
  }

  // tangent interface

  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> ad(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Zero(a.size(), a.size());
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return a;
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> dr_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(a.size(), a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> dr_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(a.size(), a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>
  d2r_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>::Zero(
      a.size(), a.size() * a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>
  d2r_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>::Zero(
      a.size(), a.size() * a.size());
  }
  // \endcond
};

///////////////////////////////////////////////////////////////
//// Lie group interface for built-in floating point types ////
///////////////////////////////////////////////////////////////

/**
 * @brief LieGroup interface for ScalarType
 */
template<ScalarType G>
struct lie<G>
{
  // \cond
  using Scalar      = G;
  using PlainObject = G;
  template<typename NewScalar>
  using CastT = NewScalar;

  static constexpr int Dof            = 1;
  static constexpr bool IsCommutative = true;

  // group interface

  static inline PlainObject Identity(Eigen::Index) { return G(0); }
  static inline PlainObject Random(Eigen::Index)
  {
    return G(Scalar(-1) + static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX / 2));
  }
  static inline Eigen::Matrix<Scalar, 1, 1> Ad(G) { return Eigen::Matrix<Scalar, 1, 1>{1}; }
  static inline PlainObject composition(G g1, G g2) { return g1 + g2; }
  static inline Eigen::Index dof(G) { return 1; }
  static inline PlainObject inverse(G g) { return -g; }
  static inline bool isApprox(G g1, G g2, Scalar eps)
  {
    using std::abs;
    return abs<G>(g1 - g2) <= eps * abs<G>(g1);
  }
  static inline Eigen::Matrix<Scalar, 1, 1> log(G g) { return Eigen::Matrix<Scalar, 1, 1>{g}; }
  template<typename NewScalar>
  static inline NewScalar cast(G g)
  {
    return static_cast<NewScalar>(g);
  }

  // tangent interface

  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> ad(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return a(0);
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> dr_exp(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Identity();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> dr_expinv(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Identity();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> d2r_exp(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> d2r_expinv(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  // \endcond
};

/**
 * @brief Manifold interface for LieGroup that are not already Manifold.
 */
template<LieGroup G>
  requires(!RnType<G> && !ScalarType<G>)
struct man<G>
{
  // \cond
  using Scalar      = typename traits::lie<G>::Scalar;
  using PlainObject = typename traits::lie<G>::PlainObject;
  template<typename NewScalar>
  using CastT = typename traits::lie<G>::template CastT<NewScalar>;

  static constexpr Eigen::Index Dof = traits::lie<G>::Dof;

  static inline PlainObject Default(Eigen::Index dof) { return traits::lie<G>::Identity(dof); }

  static inline Eigen::Index dof(const G & g) { return traits::lie<G>::dof(g); }

  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const G & g)
  {
    return traits::lie<G>::template cast<NewScalar>(g);
  }

  template<typename Derived>
  static inline PlainObject rplus(const G & g, const Eigen::MatrixBase<Derived> & a)
  {
    return traits::lie<G>::composition(g, traits::lie<G>::exp(a));
  }

  template<LieGroup Go = G>
  static inline Eigen::Matrix<Scalar, Dof, 1> rminus(const G & g1, const Go & g2)
  {
    return traits::lie<G>::log(traits::lie<Go>::composition(traits::lie<Go>::inverse(g2), g1));
  }
  // \endcond
};

}  // namespace traits

////////////////////////////////////////////////////////
//// Free functions that dispatch to traits::lie<G> ////
////////////////////////////////////////////////////////

// Group interface

template<LieGroup G>
static constexpr bool IsCommutative = traits::lie<G>::IsCommutative;

/**
 * @brief Identity in Lie group
 *
 * @param dof degrees of freedom
 */
template<LieGroup G>
inline PlainObject<G> Identity(Eigen::Index dof)
{
  return traits::lie<G>::Identity(dof);
}

/**
 * @brief Identity in Lie group with static Dof
 */
template<LieGroup G>
  requires(Dof<G> > 0)
inline PlainObject<G> Identity() { return traits::lie<G>::Identity(Dof<G>); }

/**
 * @brief Random element in Lie group
 *
 * @param dof degrees of freedom
 */
template<LieGroup G>
inline PlainObject<G> Random(Eigen::Index dof)
{
  return traits::lie<G>::Random(dof);
}

/**
 * @brief Random element in Lie group with static Dof
 */
template<LieGroup G>
  requires(Dof<G> > 0)
inline PlainObject<G> Random() { return traits::lie<G>::Random(Dof<G>); }

/**
 * @brief Group adjoint \f$ Ad_g a \coloneq (G * \hat(a) * G^{-1})^{\wedge} \f$
 */
template<LieGroup G>
inline TangentMap<G> Ad(const G & g)
{
  return traits::lie<G>::Ad(g);
}

/**
 * @brief Group binary composition
 */
template<LieGroup G, typename Arg>
inline PlainObject<G> composition(const G & g, Arg && a)
{
  return traits::lie<G>::composition(g, std::forward<Arg>(a));
}

/**
 * @brief Group multinary composition
 */
template<LieGroup G, typename Arg, typename... Args>
inline PlainObject<G> composition(const G & g, Arg && a, Args &&... as)
{
  return composition(composition(g, std::forward<Arg>(a)), std::forward<Args>(as)...);
}

/**
 * @brief Group inverse
 */
template<LieGroup G>
inline PlainObject<G> inverse(const G & g)
{
  return traits::lie<G>::inverse(g);
}

/**
 * @brief Check if two group elements are approximately equal
 */
template<LieGroup G, typename Arg>
inline bool isApprox(
  const G & g,
  Arg && a,
  typename traits::lie<G>::Scalar eps =
    Eigen::NumTraits<typename traits::lie<G>::Scalar>::dummy_precision())
{
  return traits::lie<G>::isApprox(g, std::forward<Arg>(a), eps);
}

/**
 * @brief Group logarithm
 *
 * @see exp()
 */
template<LieGroup G>
inline Tangent<G> log(const G & g)
{
  return traits::lie<G>::log(g);
}

// Tangent interface

/**
 * @brief Lie algebra adjoint \f$ ad_a b = [a, b] \f$
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> ad(Arg && a)
{
  return traits::lie<G>::ad(std::forward<Arg>(a));
}

/**
 * @brief Lie algebra exponential
 *
 * @see log()
 */
template<LieGroup G, typename Arg>
inline PlainObject<G> exp(Arg && a)
{
  return traits::lie<G>::exp(std::forward<Arg>(a));
}

/**
 * @brief Right Jacobian of exponential map
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> dr_exp(Arg && a)
{
  return traits::lie<G>::dr_exp(std::forward<Arg>(a));
}

/**
 * @brief Right Jacobian of exponential map inverse
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> dr_expinv(Arg && a)
{
  return traits::lie<G>::dr_expinv(std::forward<Arg>(a));
}

/**
 * @brief Right Hessian of exponential map
 */
template<LieGroup G, typename Arg>
inline Hessian<G> d2r_exp(Arg && a)
{
  return traits::lie<G>::d2r_exp(std::forward<Arg>(a));
}

/**
 * @brief Right Hessian of exponential map inverse
 */
template<LieGroup G, typename Arg>
inline Hessian<G> d2r_expinv(Arg && a)
{
  return traits::lie<G>::d2r_expinv(std::forward<Arg>(a));
}

// Convenience methods

/**
 * @brief Left-plus
 */
template<LieGroup G, typename Derived>
inline PlainObject<G> lplus(const G & g, const Eigen::MatrixBase<Derived> & a)
{
  return composition(::smooth::exp<G>(a), g);
}

/**
 * @brief Left-minus
 */
template<LieGroup G, LieGroup Go>
inline Tangent<G> lminus(const G & g1, const Go & g2)
{
  return log(composition(g1, inverse(g2)));
}

/**
 * @brief Left Jacobian of exponential map
 */
template<LieGroup G, typename Derived>
inline TangentMap<G> dl_exp(const Eigen::MatrixBase<Derived> & a)
{
  return dr_exp<G>(-a);
}

/**
 * @brief Left Jacobian of exponential map inverse
 */
template<LieGroup G, typename Derived>
inline TangentMap<G> dl_expinv(const Eigen::MatrixBase<Derived> & a)
{
  return dr_expinv<G>(-a);
}

/**
 * @brief Left Hessian of exponential map
 */
template<LieGroup G, typename Derived>
inline Hessian<G> d2l_exp(const Eigen::MatrixBase<Derived> & a)
{
  return -d2r_exp<G>(-a);
}

/**
 * @brief Left Hessian of exponential map inverse
 */
template<LieGroup G, typename Derived>
inline Hessian<G> d2l_expinv(const Eigen::MatrixBase<Derived> & a)
{
  return -d2r_expinv<G>(-a);
}

}  // namespace smooth

#endif  // SMOOTH__LIE_GROUP_HPP_


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

#define BOOST_TEST_MODULE table_test

// Standard includes
#include <exception>
#include <iostream>

// Third party includes
#include <boost/lexical_cast.hpp>
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/table.h"

using namespace std;
using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(table_test)

BOOST_AUTO_TEST_CASE(create_test) {
  Table tb;
  Table tb2(tb);
}

BOOST_AUTO_TEST_CASE(size_test) {
  Table tb;
  BOOST_CHECK_EQUAL(tb.size(), 0);
}

BOOST_AUTO_TEST_CASE(pushback_test) {
  Table tb;
  for (double x = 0; x < 10; ++x) {
    double y = 2 * x;
    tb.push_back(x, y);
  }
  BOOST_CHECK_EQUAL(tb.size(), 10);
}

BOOST_AUTO_TEST_CASE(resize_test) {
  Table tb;
  tb.resize(0);
  tb.resize(10);

  bool error_thrown = false;
  try {
    tb.resize(-5);
  } catch (...) {
    error_thrown = true;
  }
  BOOST_CHECK(error_thrown);
}

BOOST_AUTO_TEST_CASE(xy_test) {
  Table tb;
  for (double x = 0; x < 10; ++x) {
    double y = 2 * x;
    tb.push_back(x, y);
  }

  auto x_v = tb.x();
  auto y_v = tb.y();
  for (votca::Index i = 0; i < 10; ++i) {
    votca::Index x = i;
    votca::Index y = 2 * x;
    BOOST_CHECK_EQUAL(static_cast<votca::Index>(x_v(i)),
                      static_cast<votca::Index>(tb.x(i)));
    BOOST_CHECK_EQUAL(static_cast<votca::Index>(y_v(i)),
                      static_cast<votca::Index>(tb.y(i)));
    BOOST_CHECK_EQUAL(x, static_cast<votca::Index>(tb.x(i)));
    BOOST_CHECK_EQUAL(y, static_cast<votca::Index>(tb.y(i)));
    BOOST_CHECK_EQUAL(x, static_cast<votca::Index>(x_v(i)));
    BOOST_CHECK_EQUAL(y, static_cast<votca::Index>(y_v(i)));
  }
}

BOOST_AUTO_TEST_CASE(getMinMax_test) {
  Table tb;
  for (double x = 0; x < 10; ++x) {
    double y = 2 * x;
    tb.push_back(x, y);
  }

  BOOST_CHECK_EQUAL(static_cast<votca::Index>(tb.getMinX()), 0);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(tb.getMaxX()), 9);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(tb.getMinY()), 0);
  BOOST_CHECK_EQUAL(static_cast<votca::Index>(tb.getMaxY()), 18);
}

BOOST_AUTO_TEST_CASE(generate_grid_spacing_test) {
  Table tb;
  double min_v = 1.2;
  double max_v = 2.0;

  tb.GenerateGridSpacing(min_v, max_v, 0.2);

  BOOST_CHECK_EQUAL(tb.size(), 5);
  BOOST_CHECK_CLOSE(tb.getMinX(), 1.2, 1e-5);
  BOOST_CHECK_CLOSE(tb.getMaxX(), 2.0, 1e-5);
}

BOOST_AUTO_TEST_CASE(generate_grid_spacing_test_low) {
  Table tb;
  double min_v = 1.2;
  double max_v = 2.0;

  tb.GenerateGridSpacing(min_v, max_v, 0.3);

  BOOST_CHECK_EQUAL(tb.size(), 3);
  BOOST_CHECK_CLOSE(tb.getMinX(), 1.2, 1e-5);
  BOOST_CHECK_CLOSE(tb.getMaxX(), 2.0, 1e-5);
}

BOOST_AUTO_TEST_CASE(generate_grid_spacing_test_high) {
  Table tb;
  double min_v = 1.2;
  double max_v = 2.0;

  tb.GenerateGridSpacing(min_v, max_v, 0.15);

  BOOST_CHECK_EQUAL(tb.size(), 6);
  BOOST_CHECK_CLOSE(tb.getMinX(), 1.2, 1e-5);
  BOOST_CHECK_CLOSE(tb.getMaxX(), 2.0, 1e-5);
}

BOOST_AUTO_TEST_CASE(smoothing_test) {
  Table tb;
  double min_v = 1.2;
  double max_v = 2.0;

  tb.GenerateGridSpacing(min_v, max_v, 0.1);

  BOOST_CHECK_EQUAL(tb.size(), 9);
  tb.y() = tb.x().array().sinh();
  tb.Smooth(2);
  Eigen::VectorXd refy = Eigen::VectorXd::Zero(9);
  refy << 1.50946, 1.70595, 1.91384, 2.13995, 2.38747, 2.65889, 2.95692,
      3.28227, 3.62686;

  bool equal = tb.y().isApprox(refy, 1e-5);

  if (!equal) {
    std::cout << "result value" << std::endl;
    std::cout << tb.y().transpose() << std::endl;
    std::cout << "ref value" << std::endl;
    std::cout << refy.transpose() << std::endl;
  }
  BOOST_CHECK_EQUAL(equal, true);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <hpx/hpx_init.hpp>
#include <hpx/include/runtime.hpp>
#include <hpx/lcos/when_all.hpp>
#include <hpx/include/iostreams.hpp>
//
#include <random>
#include <utility>
#include <vector>

#include  <boost/nondet_random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

//
// This is a simple example which generates random numbers and returns
// pass or fail from a routine.
// When called by many threads returning a vector of futures - if the user wants to
// reduce the vector of pass/fails into a single pass fail based on a simple
// any fail = !pass rule, then this example shows how to do it.
// The user can experiment with the failure rate to see if the statistics match
// their expectations.
// Also. Routine can use either a lambda, or a function under control of USE_LAMBDA

#define TEST_SUCCESS 1
#define TEST_FAIL    0
//
#define FAILURE_RATE_PERCENT 5
#define SAMPLES_PER_LOOP     10
#define TEST_LOOPS           1000
//
boost::random::random_device rseed;
boost::random::mt19937 gen(rseed());
boost::random::uniform_int_distribution<int> dist(0,99); // interval [0,100)

#define USE_LAMBDA

//----------------------------------------------------------------------------
int reduce(hpx::future<std::vector<hpx::future<int> > > &&futvec)
{
  int res = TEST_SUCCESS;
  std::vector<hpx::future<int> > vfs = futvec.get();
  for (hpx::future<int>& f: vfs) {
    if (f.get() == TEST_FAIL) return TEST_FAIL;
  }
  return res;
}

//----------------------------------------------------------------------------
int generate_one()
{
  // generate roughly x% fails
  int result = TEST_SUCCESS;
  if (dist(gen)>=(100-FAILURE_RATE_PERCENT)) {
    result = TEST_FAIL;
  }
  return result;
}

//----------------------------------------------------------------------------
hpx::future<int> test_reduce()
{
  std::vector<hpx::future<int> > req_futures;
  //
  for (int i=0; i<SAMPLES_PER_LOOP; i++) {
    // generate random sequence of pass/fails using % fail rate per incident
    hpx::future<int> result = hpx::async(generate_one);
    req_futures.push_back(std::move(result));
  }

  hpx::future<std::vector<hpx::future<int> > > all_ready = hpx::when_all(req_futures);

#ifdef USE_LAMBDA
  hpx::future<int> result = all_ready.then(
    [](hpx::future<std::vector<hpx::future<int> > > &&futvec) -> int {
      // futvec is ready or the lambda would not be called
      std::vector<hpx::future<int> > vfs = futvec.get();
      // all futures in v are ready as fut is ready
      int res = TEST_SUCCESS;
      for (hpx::future<int>& f: vfs) {
        if (f.get() == TEST_FAIL) return TEST_FAIL;
      }
      return res;
  });
#else
  hpx::future<int> result = all_ready.then(reduce);
#endif
  //
  return result;
}

//----------------------------------------------------------------------------
int hpx_main()
{
  hpx::util::high_resolution_timer htimer;
  // run N times and see if we get approximately the right amount of fails
  int count = 0;
  for (int i=0; i<TEST_LOOPS; i++) {
    int result = test_reduce().get();
    count += result;
  }
  double pr_pass  = std::pow(1.0 - FAILURE_RATE_PERCENT/100.0, SAMPLES_PER_LOOP);
  double exp_pass = TEST_LOOPS*pr_pass;
  hpx::cout << "From " << TEST_LOOPS << " tests, we got "
    << "\n " << count << " passes"
    << "\n " << exp_pass << " expected \n"
    << "\n " << htimer.elapsed() << " seconds \n" << hpx::flush;
  // Initiate shutdown of the runtime system.
  return hpx::finalize();
}

//----------------------------------------------------------------------------
int main(int argc, char* argv[])
{
  // Initialize and run HPX.
  return hpx::init(argc, argv);
}


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

// Librerie standard
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include <vector>
#include <thread>

// Libreria Eigen Dense
#include <Eigen/Dense>

// Librerie Libfranka
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

// Contiene definizioni di funzioni utili
#include "../examples/examples_common.h"

// Header file con le dichiarazioni delle funzioni custom
#include "CustomLibrary/StateSaver.h"

const int t_fin = 20; // 20 s
const int fs = 1000; // 1 KHz
const int ncampioni = t_fin*fs;

/**
 * Esempio programma C++ per il controllo in coppia del robot Panda.
 * Tale esempio è una modifica del file examples/cartesian_impedance_control.cpp.
 * E' stato realizzato un controllo di tipo PD con compensazione di gravità in cui la posa
 * desiderata è pari alla posa iniziale del robot.
 *
 * Nota: l'orientamento dell'EndEffector rispetto alla terna base è espresso attraverso quaternione unitario.
 * Nota: il Panda Robot compensa da se le coppie gravitazionali e gli attriti quindi non è
 * necessario compensare le coppie gravitazionali. Inoltre, l'esempio prevede la compensazione
 * delle coppie di Coriolis e Centrifughe.
 *
 *
 *
 * Struttura del codice:
 * 1. Setup del robot
 * 2. Definizione dei parametri.
 * 3. Definizione del loop di controllo.
 * 4. Esecuzione del loop di controllo.
 *
 * Osservazione: il loop di controllo DEVE essere eseguito ad una frequenza di 1 Khz.
 * Per tale motivo la scrittura su file viene rimandata alla fine del loop di controllo.
 *
 */

// Funzione per compattare posizione e quaternione sotto un unico vettore posa 7x1
Eigen::Matrix<double,7,1> posquat2mat(const Eigen::Vector3d& position, const Eigen::Quaterniond quaternion){

    Eigen::Matrix<double,7,1> pose;
    pose(0,0) = position(0);
    pose(1,0) = position(1);
    pose(2,0) = position(2);
    pose(3,0) = quaternion.w();
    pose(4,0) = quaternion.x();
    pose(5,0) = quaternion.y();
    pose(6,0) = quaternion.z();

    return pose;
}

// Per utilizzare le variabili static della classe StateSaver serve istanziarle fuori dal main
  double** StateSaver::buffer_6 = nullptr;
  double** StateSaver::buffer_7 = nullptr;

  Eigen::Matrix<double, 6, 1> StateSaver::error;
  Eigen::Matrix<double,7,1> StateSaver::tau_measured;
  Eigen::Matrix<double,7,1> StateSaver::q;
  Eigen::Matrix<double, 7, 1> StateSaver::qdot;
  Eigen::Matrix<double, 7, 1> StateSaver::pose;

int main(int argc, char** argv) {

    if (argc != 2) {
        std::cerr << "Specificare l'indirizzo IP del robot." << std::endl;
        return -1;
    }

  // Controllo di cedevolezza attiva (PD + compensazione di gravità)
    const double rigidezza_translazionale{150.0};
    const double rigidezza_torsionale{10.0};
    Eigen::MatrixXd rigidezza(6, 6), smorzamento(6, 6);

  // Costruzione di Kp = [K_t O ; O K_o] = rigidezza
    rigidezza.setZero();
    rigidezza.topLeftCorner(3, 3) << rigidezza_translazionale * Eigen::MatrixXd::Identity(3, 3);
    rigidezza.bottomRightCorner(3, 3) << rigidezza_torsionale * Eigen::MatrixXd::Identity(3, 3);

  // Costruzione di Kd = [K_d_t O ; O K_d_o] = smorzamento
    smorzamento.setZero();
    smorzamento.topLeftCorner(3, 3) << 2.0 * sqrt(rigidezza_translazionale) *
                                        Eigen::MatrixXd::Identity(3, 3);
    smorzamento.bottomRightCorner(3, 3) << 2.0 * sqrt(rigidezza_torsionale) *
                                            Eigen::MatrixXd::Identity(3, 3);

    try {

      // Connessione al robot (inizializzazione variabile robot)
        franka::Robot robot(argv[1]);

      // Inizializzazione oggetto StateSaver per salvare su file
        StateSaver::buffer_7 = new double*[ncampioni* 7];
        StateSaver::buffer_6 = new double*[ncampioni * 6];

        for(int i = 0; i< ncampioni * 7; i++)
            StateSaver::buffer_7[i] = new double[lenght_buffer7]; // tau_m, q, dq, pose, pose_dot

        for(int i = 0; i< ncampioni * 6; i++)
            StateSaver::buffer_6[i] = new double[lenght_buffer6]; // error

        StateSaver sv;

      // Set collision behavior (https://frankaemika.github.io/libfranka/classfranka_1_1Robot.html#a168e1214ac36d74ac64f894332b84534)
        robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
        robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
        robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});

      // Modello cinematico e dinamico del robot
        franka::Model model = robot.loadModel();

      // Lettura dello stato attuale del robot
        franka::RobotState initial_state = robot.readOnce();

        Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));

      // Estrazione della posizione dalla matrice di trasformazione
        Eigen::Vector3d position_d(initial_transform.translation());

      // Estrazione dell'orientamento in forma di quaternione a partire dalla matrice di rotazione
        Eigen::Quaterniond orientation_d(initial_transform.linear());

      // Variabile per tenere traccia del tempo trascorso

        double time = 0.0;

      // Definizione della callback per il loop di controllo in coppia:

        std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                          franka::Duration duration) -> franka::Torques {

      // Update time.
        time += duration.toSec();

      // Calcolo delle coppie di Coriolis a partire dal modello dinamico e dallo stato del robot
        std::array<double, 7> coriolis_array = model.coriolis(robot_state);

      // Jacobiano geometrico del manipolatore
        std::array<double, 42> jacobian_array =
        model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      // Conversione a Eigen Map
        Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
        Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
        Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
        Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());

      // Estrazione di posizione e orientamento attuali
        Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
        Eigen::Vector3d position(transform.translation());
        Eigen::Quaterniond orientation(transform.linear());

      // Calcolo errore in posizione:
        Eigen::Matrix<double, 6, 1> error;
        error.head(3) << position - position_d;

      // Calcolo errore in orientamento:
        if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
            orientation.coeffs() << -orientation.coeffs();
        }

        Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
        error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();

        error.tail(3) << -transform.linear() * error.tail(3);

      // Costruzione delle coppie di controllo
        Eigen::VectorXd tau_task(7), tau_d(7);

      // L'errore è stato definito con il segno opposto quindi serve invertire i segni.
        tau_task << jacobian.transpose() * (-rigidezza * error - smorzamento * (jacobian * dq));

      // Salvataggio dello stato del robot attuale

        sv.tau_measured = (Eigen::Matrix<double, 7, 1>) robot_state.tau_J.data();
        sv.q = q;
        sv.qdot = dq;
        sv.pose = posquat2mat(position,orientation);
        sv.error = error;

      // Si lanciano due thread che bufferizzano i dati raccolti
        sv.fill_buffer();

      // Compensazione delle coppie di Coriolis
        tau_d << tau_task + coriolis;

        std::array<double, 7> tau_d_array{};
        Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;

      // Si attende che i thread terminino la scrittura nei buffer
        sv.wait_filler();

        if (time >= t_fin) {
          franka::Torques tau_m = tau_d_array;
          std::cout << std::endl << "Fine controllo di impedenza" << std::endl;
          return franka::MotionFinished(tau_m);
        }
        return tau_d_array;

        };

      // Start del loop di controllo real-time
        robot.control(impedance_control_callback);

      // Scrittura su file
         sv.scrivi_su_file(ncampioni);

      // Free memory
        for(int i = 0; i< ncampioni*7; i++)
            delete[] StateSaver::buffer_7[i];
        delete[] StateSaver::buffer_7;

        for(int i = 0; i< ncampioni*6; i++)
            delete[] StateSaver::buffer_6[i];
        delete[] StateSaver::buffer_6;

      } catch (const franka::Exception& ex) {
          // print exception
          std::cout << ex.what() << std::endl;
      }

      return 0;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [significants]
#include <boost/simd/exponential.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs = boost::simd;
using iT = std::int32_t;
using pack_it = bs::pack<iT, 4>;
using pack_ft = bs::pack<float, 4>;

int main() {
  pack_ft pf(1234.567f);
  pack_it qi = {1, 2, 3, 4};

  std::cout << "---- simd" << '\n'
            << "<- pf =                       " << pf << '\n'
            << "<- qi =                       " << qi << '\n'
            << "-> bs::significants(pf, qi) = " << bs::significants(pf, qi)
            << '\n';

  float xf = 2.345678f;
  iT yi = 3;

  std::cout << "---- scalar" << '\n'
            << "<- xf =                       " << xf << '\n'
            << "<- yi =                       " << yi << '\n'
            << "-> bs::significants(xf, yi) = " << bs::significants(xf, yi)
            << '\n';
  return 0;
}
//! [significants]


#include "fileread.hpp"
#include "pairwiseICP.hpp"
#include "pointCloudDeal.hpp"
#include "viz.hpp"
#include "loopdetect.hpp"
//#include "types.hpp"
#include <boost/graph/graph_concepts.hpp>
#include<Eigen/Dense>

using namespace std;

ostream& operator<<(ostream& out, const Eigen::Quaternion<float>& s)
{
  out<<s.x()<<" "<<s.y()<<" "<<s.z()<<" "<<s.w();
  return out;
}

ostream& operator<<(ostream& out, const Eigen::Vector3f & s)
{
  for(int i=0;i<s.rows();i++)
  {
    if(i<s.rows()-1)
    {
    out<<s(i)<<" ";
    }
    else{
     out<<s(i);
    }
  }
  return out;
}

int main(int argc, char**argv)
{
  string dir="/home/shaoan/projects/SLAM6D/dat_et4/";
  const int maxnumFile=15;
  vector<Eigen::Vector3f> historyTrans;
  vector<Eigen::Matrix3f> historyRotations;

  Eigen::Matrix3f R_current;
  Eigen::Vector3f t_current;
  vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> historyPointCloudPtr;
  vector<map3d::vertix<map3d::pose> > initialVertix;
  vector<map3d::edge<map3d::pose> >  edges;
  for(int nfile=0;nfile<maxnumFile;nfile++)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr currentPointCloud(new pcl::PointCloud<pcl::PointXYZ>);
    map3d::readfile(dir.c_str(),nfile,currentPointCloud,R_current,t_current);

    //store the VERTEX_SE3
    map3d::pose tempose;
    tempose.q=R_current;
    tempose.t=t_current;
    map3d::vertix<map3d::pose>  temVertix;
    temVertix.Header="VERTEX_SE3:QUAT";
    temVertix.pose_ID=nfile;
    temVertix.initialPose=tempose;
    initialVertix.push_back(temVertix);
    //store the indexes of PointCloud
    historyPointCloudPtr.push_back(currentPointCloud);

    //if number of file bigger than 1, examine whether a edge can be detected from history trace.
    vector<int> indexes;
     map3d::backend::Loop loop(300);
    if(nfile>0)
    {
      if(loop.flagLoop(historyTrans,t_current))
      {
	cout<<"label 1";
	loop.getLoopIndex(indexes);
      for(int i=0;i<indexes.size();i++)
      {
	map3d::edge<map3d::pose> temedge;
	Eigen::Matrix3f R_delta=R_current.transpose()*historyRotations[indexes[i]];
	Eigen::Vector3f t_delta=R_current.transpose()*(historyTrans[indexes[i]]-t_current);
	//cout<<" label 2";
	map3d::ICP pairwiseicp(500,1010,10,25);
	pairwiseicp.setInputCloud(historyPointCloudPtr[indexes[i]],currentPointCloud);
        pairwiseicp.setParamsConvergence();
	pairwiseicp.solve(R_delta,t_delta);
	//cout<<" label 3";
      //If a edge is added, we need to compute related InformationMatrix and store it.
        if(pairwiseicp.flag==true)
	{
	  map3d::pose tempose;
	  tempose.q=R_delta;
	  tempose.t=t_delta;
	  Eigen::Matrix<float,6,6> InformationMatrix;
	  //cout<<"label 5";
	  pairwiseicp.getInformationMatrix(InformationMatrix);
	 // cout<<" label 4";
	  temedge.a_ID=nfile;
	  temedge.b_ID=indexes[i];
	  temedge.Header="EDGE_SE3:QUAT";
	  temedge.pose_ab=tempose;
	  temedge.informationMatrix_ab=InformationMatrix;
	  edges.push_back(temedge);
	}
       }
      }
    }
     historyTrans.push_back(t_current);
     historyRotations.push_back(R_current);
  }

     ofstream file;
     //ios_base::out is same as ios_base::trunc.Open the file for output. if the file existed, it will be discarded.
     file.open("indoor.g2o",ios_base::out);
     if(file.good())
     {
       for(int i=0;i<initialVertix.size();i++)
       {
	 file<<initialVertix[i].Header<<" "<<initialVertix[i].pose_ID<<" "<< initialVertix[i].initialPose.t<<" "<<initialVertix[i].initialPose.q<<endl;
	}
	for(int i=0;i<edges.size();i++)
	{
	  file<<edges[i].Header<<" "<<edges[i].a_ID<<" "<<edges[i].b_ID;
	  file<<" "<<edges[i].pose_ab.t<<" "<<edges[i].pose_ab.q<<" ";
	  for(int k=0;k<6;k++)
	  {
	    for(int l=k;l<6;l++)
	    {
	      file<<edges[i].informationMatrix_ab(k,l)<<" ";
	    }
	  }
	  file<<endl;
	}
    }

}


#include <cmath>
#include <functional>
#include <iostream>

#include <Eigen/Dense>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include <plug_in_controller.h>

namespace plug_in_controller {

void run(const std::string& robot_ip,
         const Parameters plug_in_params,
         const double duration,  // in seconds
         const std::array<double, 3> target_position_array,
         const std::array<double, 3> target_tolerance_array) {
  // Parameters for stopping criterion
  Eigen::Vector3d target_position(Eigen::Vector3d::Map(target_position_array.data()));
  Eigen::Vector3d target_tolerance(Eigen::Vector3d::Map(target_tolerance_array.data()));

  // Compliance parameters
  Eigen::MatrixXd cartesian_stiffness(6, 6), cartesian_damping(6, 6);
  cartesian_stiffness.setZero();
  cartesian_stiffness.topLeftCorner(3, 3)
      << plug_in_params.translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  cartesian_stiffness.bottomRightCorner(3, 3)
      << plug_in_params.rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  cartesian_damping.setZero();
  cartesian_damping.topLeftCorner(3, 3)
      << 2.0 * sqrt(plug_in_params.translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
  cartesian_damping.bottomRightCorner(3, 3)
      << 2.0 * sqrt(plug_in_params.rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3);

  // Force parameters
  Eigen::VectorXd force_error_integral(6);
  // force control P, I gain
  double k_p{0.0};
  double k_i{1.0};

  franka::Robot robot(robot_ip);
  // load the kinematics and dynamics model
  franka::Model model = robot.loadModel();

  // initial robot state
  franka::RobotState initial_state = robot.readOnce();
  Eigen::Map<Eigen::Matrix<double, 6, 1>> force_initial(initial_state.O_F_ext_hat_K.data());
  force_error_integral.setZero();

  // equilibrium point is the initial position
  Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
  Eigen::Vector3d position_d(initial_transform.translation());
  Eigen::Quaterniond orientation_initial(initial_transform.linear());

  double time = 0.0;

  // define callback for the torque control loop
  std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
      plug_in_control_callback =
          [&](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques {
    time += period.toSec();

    // get state variables
    std::array<double, 7> coriolis_array = model.coriolis(robot_state);
    std::array<double, 42> jacobian_array =
        model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

    // convert to Eigen
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
    Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
    Eigen::Map<const Eigen::Matrix<double, 6, 1>> force(robot_state.O_F_ext_hat_K.data());
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
    Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
    Eigen::Vector3d position(transform.translation());
    Eigen::Quaterniond orientation(transform.linear());

    // Compute desired orientation of equilibrium pose and desired forces
    // wiggle motion
    Eigen::AngleAxisd angle_axis_wiggle_x;
    angle_axis_wiggle_x.axis() << 1, 0, 0;
    angle_axis_wiggle_x.angle() = sin(2.0 * M_PI * time * plug_in_params.wiggle_frequency_x) *
                                  plug_in_params.wiggle_amplitude_x;
    Eigen::AngleAxisd angle_axis_wiggle_y;
    angle_axis_wiggle_y.axis() << 0, 1, 0;
    angle_axis_wiggle_y.angle() = sin(2.0 * M_PI * time * plug_in_params.wiggle_frequency_y) *
                                  plug_in_params.wiggle_amplitude_y;
    Eigen::Quaterniond wiggle_x(angle_axis_wiggle_x);
    Eigen::Quaterniond wiggle_y(angle_axis_wiggle_y);
    Eigen::Quaterniond orientation_d(wiggle_y * (wiggle_x * orientation_initial));

    // desired forces
    Eigen::VectorXd desired_force(6);
    desired_force.setZero();
    desired_force(2) = -plug_in_params.desired_force;

    // Compute error to desired equilibrium pose
    // position error
    Eigen::Matrix<double, 6, 1> error;
    error.head(3) << position - position_d;

    // orientation error
    if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
      orientation.coeffs() << -orientation.coeffs();
    }
    // "difference" quaternion
    Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
    error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
    // Transform to base frame
    error.tail(3) << -transform.linear() * error.tail(3);

    // Compute error to desired force removing initial bias
    Eigen::VectorXd force_error;
    force_error = desired_force - force + force_initial;
    force_error_integral = force_error_integral + period.toSec() * force_error;

    // Compute control
    Eigen::VectorXd force_control(6), tau_force(7), tau_cart(7), tau_cmd(7);

    // Force control term
    force_control = desired_force + k_p * force_error + k_i * force_error_integral;
    force_control << 0, 0, force_control(2), 0, 0, 0;
    tau_force = jacobian.transpose() * force_control;

    // Cartesian control term
    tau_cart << jacobian.transpose() *
                    (-cartesian_stiffness * error - cartesian_damping * (jacobian * dq));

    // Commanded torque
    tau_cmd << tau_cart + tau_force + coriolis;

    std::array<double, 7> tau_d_array{};
    Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;

    // check if motion is completed
    Eigen::Matrix<double, 3, 1> target_error((target_position - position).cwiseAbs());
    bool is_inserted =
        (target_error[0] <= target_tolerance[0] && target_error[1] <= target_tolerance[1] &&
         target_error[2] <= target_tolerance[2]);
    if (time <= duration && is_inserted) {
      return franka::MotionFinished(franka::Torques(tau_d_array));
    }
    if (time > duration) {
      throw franka::Exception("Timeout: Plug in controller failed!");
    }
    return tau_d_array;
  };

  robot.control(plug_in_control_callback);
}

}  // namespace plug_in_controller


#include <stan/math/rev.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/rev/functor/util_rk45.hpp>
#include <test/unit/math/prim/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/functor/forced_harmonic_oscillator.hpp>
#include <test/unit/math/prim/functor/lorenz.hpp>
#include <iostream>
#include <sstream>
#include <vector>

template <typename F, typename T_y0, typename T_theta>
void sho_value_test(F harm_osc, std::vector<double>& y0, double t0,
                    std::vector<double>& ts, std::vector<double>& theta,
                    std::vector<double>& x, std::vector<int>& x_int) {
  using stan::math::promote_scalar;
  using stan::math::var;

  std::vector<std::vector<var>> ode_res_vd = stan::math::integrate_ode_rk45(
      harm_osc, promote_scalar<T_y0>(y0), t0, ts,
      promote_scalar<T_theta>(theta), x, x_int, 0);
  EXPECT_NEAR(0.995029, ode_res_vd[0][0].val(), 1e-5);
  EXPECT_NEAR(-0.0990884, ode_res_vd[0][1].val(), 1e-5);

  EXPECT_NEAR(-0.421907, ode_res_vd[99][0].val(), 1e-5);
  EXPECT_NEAR(0.246407, ode_res_vd[99][1].val(), 1e-5);
}

void sho_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;

  test_ode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_fun, double, var>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, double>(harm_osc, y0, t0, ts, theta, x,
                                                x_int);
  sho_value_test<harm_osc_ode_fun, var, var>(harm_osc, y0, t0, ts, theta, x,
                                             x_int);
}

void sho_data_finite_diff_test(double t0) {
  using stan::math::var;
  harm_osc_ode_data_fun harm_osc;

  std::vector<double> theta;
  theta.push_back(0.15);

  std::vector<double> y0;
  y0.push_back(1.0);
  y0.push_back(0.0);

  std::vector<double> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x(3, 1);
  std::vector<int> x_int(2, 0);

  test_ode(harm_osc, t0, ts, y0, theta, x, x_int, 1e-8, 1e-4);

  sho_value_test<harm_osc_ode_data_fun, double, var>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, double>(harm_osc, y0, t0, ts,
                                                     theta, x, x_int);
  sho_value_test<harm_osc_ode_data_fun, var, var>(harm_osc, y0, t0, ts, theta,
                                                  x, x_int);
}

TEST(StanAgradRevOde_integrate_ode_rk45, harmonic_oscillator_finite_diff) {
  sho_finite_diff_test(0);
  sho_finite_diff_test(1.0);
  sho_finite_diff_test(-1.0);

  sho_data_finite_diff_test(0);
  sho_data_finite_diff_test(1.0);
  sho_data_finite_diff_test(-1.0);
}

TEST(StanAgradRevOde_integrate_ode_rk45, lorenz_finite_diff) {
  lorenz_ode_fun lorenz;

  std::vector<double> y0;
  std::vector<double> theta;
  double t0;
  std::vector<double> ts;

  t0 = 0;

  theta.push_back(10.0);
  theta.push_back(28.0);
  theta.push_back(8.0 / 3.0);
  y0.push_back(10.0);
  y0.push_back(1.0);
  y0.push_back(1.0);

  std::vector<double> x;
  std::vector<int> x_int;

  for (int i = 0; i < 100; i++)
    ts.push_back(0.1 * (i + 1));

  test_ode(lorenz, t0, ts, y0, theta, x, x_int, 1e-8, 1e-1);
}

TEST(StanAgradRevOde_integrate_ode_rk45, time_steps_as_param) {
  using stan::math::integrate_ode_rk45;
  using stan::math::to_var;
  using stan::math::value_of;

  const double t0 = 0.0;
  forced_harm_osc_ode_fun ode;
  std::vector<double> theta{0.15, 0.25};
  std::vector<double> y0{1.0, 0.0};
  std::vector<stan::math::var> ts;
  for (int i = 0; i < 100; i++)
    ts.push_back(t0 + 0.1 * (i + 1));
  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);
  stan::math::var t0v = 0.0;

  std::vector<std::vector<stan::math::var>> res;

  std::vector<std::vector<double>> res_d
      = integrate_ode_rk45(ode, y0, t0, value_of(ts), theta, x, x_int);

  // here we only test first & last steps, and rely on the
  // fact that results in-between affect the initial
  // condition of the last step to check their validity.
  auto test_val = [&res_d, &res]() {
    EXPECT_NEAR(res_d[0][0], res[0][0].val(), 1e-5);
    EXPECT_NEAR(res_d[0][1], res[0][1].val(), 1e-5);
    EXPECT_NEAR(res_d[99][0], res[99][0].val(), 1e-5);
    EXPECT_NEAR(res_d[99][1], res[99][1].val(), 1e-5);
  };
  res = integrate_ode_rk45(ode, y0, t0, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0v, t0, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0, t0, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0v, t0, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0, t0v, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0v, t0v, ts, theta, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0, t0v, ts, thetav, x, x_int);
  test_val();
  res = integrate_ode_rk45(ode, y0v, t0v, ts, thetav, x, x_int);
  test_val();
}

TEST(StanAgradRevOde_integrate_ode_rk45, time_steps_as_param_AD) {
  using stan::math::integrate_ode_rk45;
  using stan::math::to_var;
  using stan::math::value_of;
  using stan::math::var;
  const double t0 = 0.0;
  const int nt = 100;  // nb. of time steps
  const int ns = 2;    // nb. of states
  std::ostream* msgs = NULL;

  forced_harm_osc_ode_fun ode;

  std::vector<double> theta{0.15, 0.25};
  std::vector<double> y0{1.0, 0.0};
  std::vector<stan::math::var> ts;
  for (int i = 0; i < nt; i++)
    ts.push_back(t0 + 0.1 * (i + 1));

  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);
  stan::math::var t0v = 0.0;

  std::vector<std::vector<stan::math::var>> res;
  std::vector<double> g;
  auto test_ad = [&res, &g, &ts, &ode, &nt, &ns, &theta, &x, &x_int, &msgs]() {
    for (auto i = 0; i < nt; ++i) {
      std::vector<double> res_d = value_of(res[i]);
      for (auto j = 0; j < ns; ++j) {
        g.clear();
        res[i][j].grad(ts, g);
        for (auto k = 0; k < nt; ++k) {
          if (k != i) {
            EXPECT_FLOAT_EQ(g[k], 0.0);
          } else {
            std::vector<double> y0(res_d.begin(), res_d.begin() + ns);
            EXPECT_FLOAT_EQ(g[k],
                            ode(ts[i].val(), y0, theta, x, x_int, msgs)[j]);
          }
        }
        stan::math::set_zero_all_adjoints();
      }
    }
  };
  res = integrate_ode_rk45(ode, y0, t0, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_rk45(ode, y0v, t0, ts, theta, x, x_int);
  test_ad();
  res = integrate_ode_rk45(ode, y0, t0, ts, thetav, x, x_int);
  test_ad();
  res = integrate_ode_rk45(ode, y0v, t0, ts, thetav, x, x_int);
  test_ad();
}

TEST(StanAgradRevOde_integrate_ode_rk45, t0_as_param_AD) {
  using stan::math::integrate_ode_rk45;
  using stan::math::to_var;
  using stan::math::value_of;
  using stan::math::var;
  const double t0 = 0.0;
  std::ostream* msgs = NULL;

  harm_osc_ode_fun ode;

  std::vector<double> theta{0.15};
  std::vector<double> y0{1.0, 0.0};
  std::vector<double> ts = {5.0, 10.0};

  std::vector<double> x;
  std::vector<int> x_int;
  std::vector<stan::math::var> y0v = to_var(y0);
  std::vector<stan::math::var> thetav = to_var(theta);
  stan::math::var t0v = to_var(t0);

  std::vector<std::vector<stan::math::var>> res;
  auto test_ad = [&res, &t0v, &ode, &theta, &x, &x_int, &msgs]() {
    res[0][0].grad();
    EXPECT_FLOAT_EQ(t0v.adj(), -0.66360742442816977871);
    stan::math::set_zero_all_adjoints();
    res[0][1].grad();
    EXPECT_FLOAT_EQ(t0v.adj(), 0.23542843380353062344);
    stan::math::set_zero_all_adjoints();
    res[1][0].grad();
    EXPECT_FLOAT_EQ(t0v.adj(), -0.2464078910913158893);
    stan::math::set_zero_all_adjoints();
    res[1][1].grad();
    EXPECT_FLOAT_EQ(t0v.adj(), -0.38494826636037426937);
    stan::math::set_zero_all_adjoints();
  };
  res = integrate_ode_rk45(ode, y0, t0v, ts, theta, x, x_int, nullptr, 1e-10,
                           1e-10, 1e6);
  test_ad();
  res = integrate_ode_rk45(ode, y0v, t0v, ts, theta, x, x_int, nullptr, 1e-10,
                           1e-10, 1e6);
  test_ad();
  res = integrate_ode_rk45(ode, y0, t0v, ts, thetav, x, x_int, nullptr, 1e-10,
                           1e-10, 1e6);
  test_ad();
  res = integrate_ode_rk45(ode, y0v, t0v, ts, thetav, x, x_int, nullptr, 1e-10,
                           1e-10, 1e6);
  test_ad();
}


#ifndef PRECONDITIONED_CONJUGATE_GRADIENT_H
#define PRECONDITIONED_CONJUGATE_GRADIENT_H
#include <Eigen/Dense>
#include <mtao/logging/timer.hpp>
#include <iostream>

template <typename Matrix>
struct DenseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    template <typename A, typename B, typename C>
    inline Scalar tripleVectorProduct(const A & a, const B & b, const C & c) const {
        return a.cwiseProduct(b).dot(c);
    }
    inline Scalar tripleProduct(const Matrix & a, uint i, uint j) const
    {
        return tripleVectorProduct(a.row(i).head(j),a.row(j).head(j),a.diagonal().head(j));
    }
    DenseLDLT(const Matrix & A)
    {
        LD=A.template triangularView<Eigen::Lower>();
        int i,j;
        for(i=0; i<A.rows(); ++i)
        {
            for(j=0; j<i; ++j)
                if(std::abs(LD(j,j))>0.0001)
                {
                    LD(i,j)-=tripleProduct(LD,i,j);
                    LD(i,j)/=LD(j,j);
                }
                else
                {
                    LD(i,j)=0;
                }
            LD(i,i) -= tripleProduct(LD,i,i);
        }

    }
    template <typename Vector>
    void solve(const Vector & b, Vector & x) const
    {
        x = LD.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseQuotient(LD.diagonal());//safe beacuse it's a dot
        LD.template triangularView<Eigen::UnitLower>().transpose().solveInPlace(x);
    }
    Matrix getA() const
    {
        Matrix A = LD.template triangularView<Eigen::UnitLower>().transpose();
        A = LD.diagonal().asDiagonal() * A;
        A = LD.template triangularView<Eigen::UnitLower>() * A;

        return A;
    }
private:
    Matrix LD;
};

template <typename Matrix, typename Vector>
struct SparseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    SparseLDLT() {}
    SparseLDLT(const Matrix & A)
    {
        // L=tril(A);
        L=A.template triangularView<Eigen::StrictlyLower>();//Don't copy the diagonal
        for(int i=0; i<L.rows(); ++i)
        {
            if(L.coeff(i,i)!=0)
                L.coeffRef(i,i)=0;
        }
        Dinv=D=A.diagonal();

        // for k=1:size(L,2)
        for(int k=0; k<A.rows(); ++k)//k is the column that we're infecting the remaining columns with
        {//L(:,k)

            //Solidify the current column values
            //==================================
            if(D(k)==0) continue;
            if(Dinv(k)<0.25*D(k))//If D has shrunk too much since it started
                Dinv(k)=1/D(k);
            else
                Dinv(k)=1/Dinv(k);
            L.innerVector(k) *= Dinv(k);

            //Add k terms to all of the following columns
            //===========================================
            for(typename Matrix::InnerIterator it(L,k); it; ++it)// -L(i,k)*D(k)*L(j,k)
            {
                int j = it.row();//j>k
                if(j<=k) continue;
                Scalar missing=0;
                Scalar multiplier=it.value();//L(j,k)*D(k)

                typename Matrix::InnerIterator k_it(L,k);
                typename Matrix::InnerIterator j_it(L,j);
                //move down teh column of L(:,k) to collect missing elements in the match with A(:,j)
                //i=k_it.row()

                while (k_it && k_it.row()<j){//L(i,k)
                    while(j_it)//L(i,j) occasionally
                    {
                        if(j_it.row() < k_it.row())
                            ++j_it;
                        else if(j_it.row() == k_it.row())//L(i,k) are L(i,j) are nonzero
                            break;
                        else
                        {
                            missing += k_it.value();//L(i,k) will fill something not in L(i,j)
                            break;
                        }
                    }
                    ++k_it;
                }

                if(k_it && j_it.row() == j)
                {
                    Dinv(j) -= it.value() * multiplier;
                }

                typename Matrix::InnerIterator j_it2(L,j);
                while(k_it && j_it2)
                {
                    if(j_it2.row() < k_it.row())
                        ++j_it2;
                    else if(j_it2.row() == k_it.row())//L(i,k) and L(i,j) are both nonzero, -=L(i,k)*L(j,k)*D(k)
                    {
                        j_it2.valueRef() -= multiplier * k_it.value() ;//k_it.value()=L(i,k)
                        ++j_it2;
                        ++k_it;
                    }
                    else
                    {
                        missing+=k_it.value();
                        ++k_it;
                    }
                }

                while(k_it)
                {
                    missing+=k_it.value();
                    ++k_it;
                }
                Dinv(j)-=0.97*missing*multiplier;
            }
        }

        /*
           std::cout << L << std::endl;
           */

    }
    void solve(const Vector & b, Vector & x) const
    {
        x = L.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseProduct(Dinv);//safe beacuse it's a dot
        L.transpose().template triangularView<Eigen::UnitUpper>().solveInPlace(x);
    }
    Matrix getA()
    {
        Matrix
                A = L.template triangularView<Eigen::UnitLower>();
        A = A * D.asDiagonal();
        A = A * L.template triangularView<Eigen::UnitLower>().transpose();

        return A;
    }
private:
    Matrix L;
    Vector D,Dinv;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PreconditionedConjugateGradient
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    PreconditionedConjugateGradient(const Matrix & A): A(A)
    {
        mtao::logging::timer("Preconditioner",false);
        precond = Preconditioner(A);
    }
    PreconditionedConjugateGradient(PreconditionedConjugateGradient&&) = default;
    auto solve(const Vector& b) const {
        mtao::logging::timer("Solve",false);
        auto x = b.eval();
        x.setZero();
        Vector r = b-A*x;
        Vector z;
        precond.solve(r,z);
        Vector p = z;
        Vector Ap = A*p;
        Scalar rdz = r.dot(z);
        Scalar alpha, beta;
        auto error = [&]() { return r.template lpNorm<Eigen::Infinity>(); };

        uint iterations = 0;
        Scalar eps = error() * epsilon;
        while(++iterations < 10 &&
        //while(++iterations < b.rows() &&
                error() > eps)
        {
            alpha = (rdz)/(p.dot(Ap));
            x+=alpha * p;
            r-=alpha * Ap;
            precond.solve(r,z);
            beta=1/rdz;
            rdz = r.dot(z);
            beta*=rdz;
            p=z+beta*p;
            Ap=A*p;
        }
        return x;
    }
    Scalar error()
    {
    }
private:
    const Matrix A;
    Preconditioner precond;

    Scalar epsilon = 1e-5;

};

template <typename Matrix, typename Vector>
auto ldlt_pcg_solver(const Matrix & A, const Vector& b)
{
    return PreconditionedConjugateGradient<Matrix,Vector, SparseLDLT<Matrix, mtao::Vector<typename Vector::Scalar, Vector::RowsAtCompileTime>>>(A);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
}
template <typename Matrix, typename Vector>
auto ldlt_pcg_solve(const Matrix & A, const Vector & b)
{
    auto solver = ldlt_pcg_solver(A,b);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    return solver.solve(b);
}

#endif


/*************************************************************************
 * SceneML, Copyright (C) 2007, 2008  J.D. Yamokoski
 * All rights reserved.
 * Email: yamokosk at gmail dot com
 *
 * This library 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 2.1 of the License,
 * or (at your option) any later version. The text of the GNU Lesser General
 * Public License is included with this library in the file LICENSE.TXT.
 *
 * This library 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 file LICENSE.TXT for
 * more details.
 *
 *************************************************************************/

#include "transform.h"
#include "pose_estimation.h"
#include "matrix.h"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
using namespace sceneml;

CoordinateTransform::CoordinateTransform() :
	pos_(3),
	ori_(0,0,0,0)
{ }

SimpleTransform::~SimpleTransform()
{ }

//const dReal* SimpleTransform::compute()
const dReal* SimpleTransform::compute()
{
	//using namespace boost::numeric::ublas;

	//identity_matrix<double> eye(4);
	//matrix<double> tmatrix(4,4);
	dTSetIdentity(tmatrix_);

	// Compute transform - using pos and quaternion
	if ( !type_.compare("translation") ) {
		dTFromTrans(tmatrix_, (data_.get())[0], (data_.get())[1], (data_.get())[2]);
		//for (int n=0; n < 3; ++n) tmatrix(n,3) = pos_(n);
		//subrange(tmatrix, 0, 2, 3, 3) = pos_;
	} else if ( !type_.compare("rotation") ) {
		if ( !subtype_.compare("x") ) 		dTFromAxisAndAngle(tmatrix_, REAL(1.0), REAL(0.0), REAL(0.0), (data_.get())[0]);
		else if ( !subtype_.compare("y") ) 	dTFromAxisAndAngle(tmatrix_, REAL(0.0), REAL(1.0), REAL(0.0), (data_.get())[0]);
		else if ( !subtype_.compare("z") ) 	dTFromAxisAndAngle(tmatrix_, REAL(0.0), REAL(0.0), REAL(1.0), (data_.get())[0]);
		else if ( !subtype_.compare("e123") )	dTFromEuler123(tmatrix_, (data_.get())[0], (data_.get())[1], (data_.get())[2]);
		else if ( !subtype_.compare("t123") )	dTFromEuler123(tmatrix_, -(data_.get())[0], -(data_.get())[1], -(data_.get())[2]);
		else throw std::runtime_error("");
	} else {

	}
	return tmatrix_;
	/*if ( !type_.compare("translation") ) {
		dTFromTrans(tmatrix_, (data_.get())[0], (data_.get())[1], (data_.get())[2]);
	} else if ( !type_.compare("rotation") ) {
		if ( !subtype_.compare("x") ) 		dTFromAxisAndAngle(tmatrix_, REAL(1.0), REAL(0.0), REAL(0.0), (data_.get())[0]);
		else if ( !subtype_.compare("y") ) 	dTFromAxisAndAngle(tmatrix_, REAL(0.0), REAL(1.0), REAL(0.0), (data_.get())[0]);
		else if ( !subtype_.compare("z") ) 	dTFromAxisAndAngle(tmatrix_, REAL(0.0), REAL(0.0), REAL(1.0), (data_.get())[0]);
		else if ( !subtype_.compare("e123") )	dTFromEuler123(tmatrix_, (data_.get())[0], (data_.get())[1], (data_.get())[2]);
		else if ( !subtype_.compare("t123") )	dTFromEuler123(tmatrix_, -(data_.get())[0], -(data_.get())[1], -(data_.get())[2]);
		else throw std::runtime_error("");
	} else  {
		throw std::runtime_error("");
	}

	return tmatrix_;*/
}

MarkerTransform::~MarkerTransform()
{
	/*for (unsigned int n=0; n < localCoords_.size(); ++n)
	{
		delete [] localCoords_[n];
		delete [] globalCoords_[n];
	}*/
}

const dReal* MarkerTransform::compute()
{
	// Number of local and global coords is ASSUMED to be the same.. no error
	// checking done here
	int nNumCoords = localCoords_.size();

	// Allocate temp storage for markers and fill up with data
	dRealPtr lCoords( new dReal[3 * nNumCoords] );
	dRealPtr gCoords( new dReal[3 * nNumCoords] );

	for (int n=0; n < nNumCoords; ++n)
	{
		//dReal* lCoord = localCoords_[n], gCoord = globalCoords[n];

		memcpy( (lCoords.get()+n*3), localCoords_[n].get(), 3*sizeof(dReal) );
		memcpy( (gCoords.get()+n*3), globalCoords_[n].get(), 3*sizeof(dReal) );
	}

	// Do estimation and get answer
	SVDEstimator estimator;
	estimator.estimate(gCoords.get(), lCoords.get(), nNumCoords);
	estimator.getPose(tmatrix_);

	return tmatrix_;
}

CompositeTransform::~CompositeTransform()
{
	//CoordinateTransformList_t::iterator it = childTransforms_.begin();
	//for (; it != childTransforms_.end(); ++it) delete (*it);
}

const dReal* CompositeTransform::compute()
{
	dTSetIdentity(tmatrix_);
	CoordinateTransformList_t::iterator it = childTransforms_.begin();
	for (; it != childTransforms_.end(); ++it)
	{
		dMatrix4 Tr;
		dMultiply0(Tr, tmatrix_, (*it)->compute(), 4, 4, 4);
		memcpy(tmatrix_, Tr, sizeof(dMatrix4));
	}
	mNeedsUpdate = false;
	return tmatrix_;
};


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <Eigen/LU>

#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h"
#include "Tudat/External/SofaInterface/fundamentalArguments.h"
#include "Tudat/External/SofaInterface/earthOrientation.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::sofa_interface;

BOOST_AUTO_TEST_SUITE( test_sofa_fundamental_arguments )

//! Test calculation of Delaunay/Doodson arguments used for calculation of e.g. Earth-tide effects
BOOST_AUTO_TEST_CASE( testSofaFundamentalArguments )
{
    // Test whether Doodson <-> Delaunay argument conversion matrices are each other's inverse.
    Eigen::Matrix< double, 5, 5 > conversionMatrixComparison =
            delaunayToDoodsonArguments - doodsonToDelaunayArguments.inverse( );
    for( unsigned int i = 0; i < 5; i++ )
    {
        for( unsigned int j = 0; j < 5; j++ )
        {
            BOOST_CHECK_EQUAL( conversionMatrixComparison( i, j ), 0.0 );
        }
    }

    // Use test case from FUNDARG.F code (provided with IERS 2010 conventions) to calculate Delaunay arguments
    double testModifiedJulianDay1 = 54465.0;
    double testSecondsSinceJ2000 = ( testModifiedJulianDay1 -
            ( - basic_astrodynamics::JULIAN_DAY_AT_0_MJD + basic_astrodynamics::JULIAN_DAY_ON_J2000 ) ) *
            physical_constants::JULIAN_DAY;
    Eigen::Matrix< double, 5, 1 > expectedFundamentalArgumentValues;
    expectedFundamentalArgumentValues << 2.291187512612069099, 6.212931111003726414, 3.658025792050572989,
            4.554139562402433228, -0.5167379217231804489;

    // Calculate Delaunay arguments.
    Eigen::Matrix< double, 5, 1 > fundamentalArgumentValues =
            calculateDelaunayFundamentalArguments( testSecondsSinceJ2000 );

    // Compare against IERS results.
    for( unsigned int i = 0; i < 5; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( expectedFundamentalArgumentValues( i ) - fundamentalArgumentValues( i ) ), 2.0E-13  );
    }

    // Calculate Delaunay arguments with GMST.
    Eigen::Matrix< double, 6, 1 > fundamentalArgumentValuesWithGmst =
            calculateApproximateDelaunayFundamentalArgumentsWithGmst( testSecondsSinceJ2000 );
    for( unsigned int i = 0; i < 5; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( fundamentalArgumentValuesWithGmst( i + 1 ) - fundamentalArgumentValues( i ) ), 1.0E-15  );
    }

    // Manually compute GMST
    double expectedGmst = calculateGreenwichMeanSiderealTime(
                testSecondsSinceJ2000,
                convertTTtoUTC( testSecondsSinceJ2000 ),
                basic_astrodynamics::JULIAN_DAY_ON_J2000, basic_astrodynamics::iau_2006 );

    BOOST_CHECK_SMALL( std::fabs( expectedGmst + mathematical_constants::PI - fundamentalArgumentValuesWithGmst( 0 ) ), 1.0E-15  );

    // Calculate Doodson arguments directly and from Delaunay arguments and compare.
    Eigen::Matrix< double, 6, 1 > doodsonArguments = calculateDoodsonFundamentalArguments( testSecondsSinceJ2000 );
    Eigen::Matrix< double, 5, 1 > reconstructedFundamentalArguments =
            doodsonToDelaunayArguments * doodsonArguments.segment( 1, 5 );
    for( unsigned int i = 0; i < 5; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( fundamentalArgumentValues( i ) - reconstructedFundamentalArguments( i ) ), 1.0E-15  );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat