#ifndef __multicontact_api_scenario_contact_phase_hpp__
#define __multicontact_api_scenario_contact_phase_hpp__

#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
#include "multicontact-api/geometry/curve-map.hpp"

#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/spatial.hpp"

#include <curves/fwd.h>
#include <curves/piecewise_curve.h>
#include <curves/serialization/curves.hpp>
#include <map>
#include <vector>
#include <set>
#include <string>
#include <sstream>
#include <boost/serialization/string.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>

namespace multicontact_api {
namespace scenario {

template <typename _Scalar>
struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Scalar> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;

  // Eigen types
  typedef curves::pointX_t pointX_t;
  typedef curves::point3_t point3_t;
  typedef curves::point6_t point6_t;
  typedef curves::t_point3_t t_point3_t;
  typedef curves::t_pointX_t t_pointX_t;
  typedef curves::transform_t transform_t;

  // Curves types
  typedef curves::curve_abc_t curve_t;
  // typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
  typedef curves::curve_SE3_t curve_SE3_t;
  typedef curves::curve_ptr_t curve_ptr;
  // typedef boost::shared_ptr<curve_3_t> curve_3_ptr;
  typedef curves::curve_SE3_ptr_t curve_SE3_ptr;
  typedef curves::piecewise3_t piecewise3_t;
  typedef curves::piecewise_t piecewise_t;
  typedef piecewise_t::t_time_t t_time_t;

  typedef std::vector<std::string> t_strings;
  typedef ContactPatchTpl<Scalar> ContactPatch;
  typedef typename ContactPatch::SE3 SE3;
  typedef std::map<std::string, ContactPatch> ContactPatchMap;
  typedef CurveMap<curve_ptr> CurveMap_t;
  typedef CurveMap<curve_SE3_ptr> CurveSE3Map_t;

  /// \brief Default constructor
  ContactPhaseTpl()
      : m_c_init(point3_t::Zero()),
        m_dc_init(point3_t::Zero()),
        m_ddc_init(point3_t::Zero()),
        m_L_init(point3_t::Zero()),
        m_dL_init(point3_t::Zero()),
        m_q_init(),
        m_c_final(point3_t::Zero()),
        m_dc_final(point3_t::Zero()),
        m_ddc_final(point3_t::Zero()),
        m_L_final(point3_t::Zero()),
        m_dL_final(point3_t::Zero()),
        m_q_final(),
        m_q(),
        m_dq(),
        m_ddq(),
        m_tau(),
        m_c(),
        m_dc(),
        m_ddc(),
        m_L(),
        m_dL(),
        m_wrench(),
        m_zmp(),
        m_root(),
        m_contact_forces(),
        m_contact_normal_force(),
        m_effector_trajectories(),
        m_effector_in_contact(),
        m_contact_patches(),
        m_t_init(-1),
        m_t_final(-1) {}

  /**
   * @brief ContactPhaseTpl Constructor with time interval
   * @param t_init the time at the beginning of this contact phase
   * @param t_final the time at the end of this contact phase
   * @throw invalid_argument if t_final < t_init
   */
  ContactPhaseTpl(const Scalar t_init, const Scalar t_final)
      : m_c_init(point3_t::Zero()),
        m_dc_init(point3_t::Zero()),
        m_ddc_init(point3_t::Zero()),
        m_L_init(point3_t::Zero()),
        m_dL_init(point3_t::Zero()),
        m_q_init(),
        m_c_final(point3_t::Zero()),
        m_dc_final(point3_t::Zero()),
        m_ddc_final(point3_t::Zero()),
        m_L_final(point3_t::Zero()),
        m_dL_final(point3_t::Zero()),
        m_q_final(),
        m_q(),
        m_dq(),
        m_ddq(),
        m_tau(),
        m_c(),
        m_dc(),
        m_ddc(),
        m_L(),
        m_dL(),
        m_wrench(),
        m_zmp(),
        m_root(),
        m_contact_forces(),
        m_contact_normal_force(),
        m_effector_trajectories(),
        m_effector_in_contact(),
        m_contact_patches(),
        m_t_init(t_init),
        m_t_final(t_final) {
    if (t_final < t_init) throw std::invalid_argument("t_final cannot be inferior to t_initial");
  }

  /// \brief Copy constructor
  template <typename S2>
  ContactPhaseTpl(const ContactPhaseTpl<S2>& other)
      : m_c_init(other.m_c_init),
        m_dc_init(other.m_dc_init),
        m_ddc_init(other.m_ddc_init),
        m_L_init(other.m_L_init),
        m_dL_init(other.m_dL_init),
        m_q_init(other.m_q_init),
        m_c_final(other.m_c_final),
        m_dc_final(other.m_dc_final),
        m_ddc_final(other.m_ddc_final),
        m_L_final(other.m_L_final),
        m_dL_final(other.m_dL_final),
        m_q_final(other.m_q_final),
        m_q(other.m_q),
        m_dq(other.m_dq),
        m_ddq(other.m_ddq),
        m_tau(other.m_tau),
        m_c(other.m_c),
        m_dc(other.m_dc),
        m_ddc(other.m_ddc),
        m_L(other.m_L),
        m_dL(other.m_dL),
        m_wrench(other.m_wrench),
        m_zmp(other.m_zmp),
        m_root(other.m_root),
        m_contact_forces(other.m_contact_forces),
        m_contact_normal_force(other.m_contact_normal_force),
        m_effector_trajectories(other.m_effector_trajectories),
        m_effector_in_contact(other.m_effector_in_contact),
        m_contact_patches(other.m_contact_patches),
        m_t_init(other.m_t_init),
        m_t_final(other.m_t_final) {}

  template <typename S2>
  bool operator==(const ContactPhaseTpl<S2>& other) const {
    return m_c_init == other.m_c_init && m_dc_init == other.m_dc_init && m_ddc_init == other.m_ddc_init &&
           m_L_init == other.m_L_init && m_dL_init == other.m_dL_init &&
           (m_q_init.rows() == other.m_q_init.rows() && m_q_init.cols() == other.m_q_init.cols() &&
            m_q_init == other.m_q_init) &&
           m_c_final == other.m_c_final && m_dc_final == other.m_dc_final && m_ddc_final == other.m_ddc_final &&
           m_L_final == other.m_L_final && m_dL_final == other.m_dL_final &&
           (m_q_final.rows() == other.m_q_final.rows() && m_q_final.cols() == other.m_q_final.cols() &&
            m_q_final == other.m_q_final) &&
           (m_q == other.m_q || (m_q && other.m_q && m_q->isApprox(other.m_q.get()))) &&
           (m_dq == other.m_dq || (m_dq && other.m_dq && m_dq->isApprox(other.m_dq.get()))) &&
           (m_ddq == other.m_ddq || (m_ddq && other.m_ddq && m_ddq->isApprox(other.m_ddq.get()))) &&
           (m_tau == other.m_tau || (m_tau && other.m_tau && m_tau->isApprox(other.m_tau.get()))) &&
           (m_c == other.m_c || (m_c && other.m_c && m_c->isApprox(other.m_c.get()))) &&
           (m_dc == other.m_dc || (m_dc && other.m_dc && m_dc->isApprox(other.m_dc.get()))) &&
           (m_ddc == other.m_ddc || (m_ddc && other.m_ddc && m_ddc->isApprox(other.m_ddc.get()))) &&
           (m_L == other.m_L || (m_L && other.m_L && m_L->isApprox(other.m_L.get()))) &&
           (m_dL == other.m_dL || (m_dL && other.m_dL && m_dL->isApprox(other.m_dL.get()))) &&
           (m_wrench == other.m_wrench || (m_wrench && other.m_wrench && m_wrench->isApprox(other.m_wrench.get()))) &&
           (m_zmp == other.m_zmp || (m_zmp && other.m_zmp && m_zmp->isApprox(other.m_zmp.get()))) &&
           (m_root == other.m_root || (m_root && other.m_root && m_root->isApprox(other.m_root.get()))) &&
           m_contact_forces == other.m_contact_forces && m_contact_normal_force == other.m_contact_normal_force &&
           m_effector_trajectories == other.m_effector_trajectories &&
           m_effector_in_contact == other.m_effector_in_contact && m_contact_patches == other.m_contact_patches &&
           m_t_init == other.m_t_init && m_t_final == other.m_t_final;
  }

  template <typename S2>
  bool operator!=(const ContactPhaseTpl<S2>& other) const {
    return !(*this == other);
  }

  // public members :
  /// \brief Initial position of the center of mass for this contact phase
  point3_t m_c_init;
  /// \brief Initial velocity of the center of mass for this contact phase
  point3_t m_dc_init;
  /// \brief Initial acceleration of the center of mass for this contact phase
  point3_t m_ddc_init;
  /// \brief Initial angular momentum for this contact phase
  point3_t m_L_init;
  /// \brief Initial angular momentum derivative for this contact phase
  point3_t m_dL_init;
  /// \brief Initial whole body configuration of this phase
  pointX_t m_q_init;
  /// \brief Final position of the center of mass for this contact phase
  point3_t m_c_final;
  /// \brief Final velocity of the center of mass for this contact phase
  point3_t m_dc_final;
  /// \brief Final acceleration of the center of mass for this contact phase
  point3_t m_ddc_final;
  /// \brief Final angular momentum for this contact phase
  point3_t m_L_final;
  /// \brief Final angular momentum derivative for this contact phase
  point3_t m_dL_final;
  /// \brief Final whole body configuration of this phase
  pointX_t m_q_final;

  /// \brief trajectory for the joint positions
  curve_ptr m_q;
  /// \brief trajectory for the joint velocities
  curve_ptr m_dq;
  /// \brief trajectory for the joint accelerations
  curve_ptr m_ddq;
  /// \brief trajectory for the joint torques
  curve_ptr m_tau;
  /// \brief trajectory for the center of mass position
  curve_ptr m_c;
  /// \brief trajectory for the center of mass velocity
  curve_ptr m_dc;
  /// \brief trajectory for the center of mass acceleration
  curve_ptr m_ddc;
  /// \brief trajectory for the angular momentum
  curve_ptr m_L;
  /// \brief trajectory for the angular momentum derivative
  curve_ptr m_dL;
  /// \brief trajectory for the centroidal wrench
  curve_ptr m_wrench;
  /// \brief trajectory for the zmp
  curve_ptr m_zmp;
  /// \brief SE3 trajectory of the root of the robot
  curve_SE3_ptr m_root;

  // getter and setter for the timings
  Scalar timeInitial() const { return m_t_init; }
  void timeInitial(const Scalar t) { m_t_init = t; }
  Scalar timeFinal() const { return m_t_final; }
  void timeFinal(const Scalar t) {
    if (t < m_t_init) throw std::invalid_argument("t_final cannot be inferior to t_initial");
    m_t_final = t;
  }
  Scalar duration() const { return m_t_final - m_t_init; }
  void duration(const Scalar d) {
    if (d <= 0) throw std::invalid_argument("Duration of the phase cannot be negative.");
    m_t_final = m_t_init + d;
  }

  // getter for the map trajectories
  CurveMap_t contactForces() const { return m_contact_forces; }
  CurveMap_t contactNormalForces() const { return m_contact_normal_force; }
  CurveSE3Map_t effectorTrajectories() const { return m_effector_trajectories; }
  curve_ptr contactForces(const std::string& eeName) {
    if (m_contact_forces.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any contact force trajectory for the effector " +
                                  eeName);
    } else {
      return m_contact_forces.at(eeName);
    }
  }
  curve_ptr contactNormalForces(const std::string& eeName) {
    if (m_contact_normal_force.count(eeName) == 0) {
      throw std::invalid_argument(
          "This contact phase do not contain any normal contact force trajectory for the effector " + eeName);
    } else {
      return m_contact_normal_force.at(eeName);
    }
  }
  curve_SE3_ptr effectorTrajectories(const std::string& eeName) {
    if (m_effector_trajectories.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any effector trajectory for the effector " +
                                  eeName);
    } else {
      return m_effector_trajectories.at(eeName);
    }
  }
  /**
   * @brief addContactForceTrajectory add a trajectory to the map of contact forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is not defined in contact for this phase
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
  bool addContactForceTrajectory(const std::string& eeName, const curve_ptr trajectory) {
    if (!isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add a contact force trajectory for effector " + eeName +
                                  " as it is not in contact for the current phase.");
    bool alreadyExist(m_contact_forces.count(eeName));
    if (alreadyExist) m_contact_forces.erase(eeName);
    m_contact_forces.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
    return !alreadyExist;
  }
  /**
   * @brief addContactNormalForceTrajectory add a trajectory to the map of contact normal forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is not defined in contact for this phase
   * @throw invalid_argument if trajectory is not of dimension 1
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
  bool addContactNormalForceTrajectory(const std::string& eeName, const curve_ptr trajectory) {
    if (!isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add a contact normal trajectory for effector " + eeName +
                                  " as it is not in contact for the current phase.");
    if (trajectory->dim() != 1) throw std::invalid_argument("Contact normal force trajectory must be of dimension 1");
    bool alreadyExist(m_contact_normal_force.count(eeName));
    if (alreadyExist) m_contact_normal_force.erase(eeName);
    m_contact_normal_force.insert(std::pair<std::string, curve_ptr>(eeName, trajectory));
    return !alreadyExist;
  }
  /**
   * @brief adEffectorTrajectory add a trajectory to the map of contact forces.
   * If a trajectory already exist for this effector, it is overwritted.
   * @param eeName the name of the effector (key of the map)
   * @param trajectory the trajectory to add
   * @throw invalid_argument if eeName is defined in contact for this phase
   * @return false if a trajectory already existed (and have been overwrited) true otherwise
   */
  bool addEffectorTrajectory(const std::string& eeName, const curve_SE3_ptr trajectory) {
    if (isEffectorInContact(eeName))
      throw std::invalid_argument("Cannot add an effector trajectory for effector " + eeName +
                                  " as it is in contact for the current phase.");
    bool alreadyExist(m_effector_trajectories.count(eeName));
    if (alreadyExist) m_effector_trajectories.erase(eeName);
    m_effector_trajectories.insert(std::pair<std::string, curve_SE3_ptr>(eeName, trajectory));
    return !alreadyExist;
  }

  ContactPatchMap contactPatches() const { return m_contact_patches; }
  ContactPatch& contactPatch(const std::string& eeName) {
    if (m_contact_patches.count(eeName) == 0) {
      throw std::invalid_argument("This contact phase do not contain any contact patch for the effector " + eeName);
    } else {
      return m_contact_patches.at(eeName);
    }
  }
  /**
   * @brief addContact add a new contact patch to this contact phase
   * If a contact phase already exist for this effector, it is overwritted.
   * If an end effector trajectory exist for this contact, it is removed.
   * @param eeName the name of the effector in contact
   * @param patch the contact patch
   * @return false if a contact for this effector already existed (and have been overwrited) true otherwise
   */
  bool addContact(const std::string& eeName, const ContactPatch& patch) {
    bool alreadyExist(isEffectorInContact(eeName));
    if (m_contact_patches.count(eeName))
      m_contact_patches.erase(eeName);
    else
      m_effector_in_contact.push_back(eeName);
    m_contact_patches.insert(std::pair<std::string, ContactPatch>(eeName, patch));
    m_effector_trajectories.erase(eeName);
    return !alreadyExist;
  }

  /**
   * @brief removeContact remove the given contact
   * This will also remove the contact_patch all the contact_forces and contact_normal_forces related to this contact
   * @param eeName the name of the effector to remove
   * @return true if the effector was in contact, false otherwise
   */
  bool removeContact(const std::string& eeName) {
    bool existed(isEffectorInContact(eeName));
    if (existed)
      m_effector_in_contact.erase(std::find(m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName));
    m_contact_patches.erase(eeName);
    m_contact_forces.erase(eeName);
    m_contact_normal_force.erase(eeName);
    return existed;
  }

  std::size_t numContacts() const { return m_effector_in_contact.size(); }

  t_strings effectorsInContact() const { return m_effector_in_contact; }

  bool isEffectorInContact(const std::string& eeName) const {
    if (m_effector_in_contact.empty())
      return false;
    else
      return (std::find(m_effector_in_contact.begin(), m_effector_in_contact.end(), eeName) !=
              m_effector_in_contact.end());
  }

  /**
   * @brief effectorsWithTrajectory return a set of all effectors for which an effector trajectory have been defined
   * @return a set of all effectors for which an effector trajectory have been defined
   */
  t_strings effectorsWithTrajectory() const {
    t_strings effectors;
    for (typename CurveSE3Map_t::const_iterator mit = m_effector_trajectories.begin();
         mit != m_effector_trajectories.end(); ++mit) {
      effectors.push_back(mit->first);
    }
    return effectors;
  }

  /**
   * @brief effectorHaveAtrajectory check if an end effector trajectory have been defined for a given effector
   * @param eeName the effector name
   * @return true if there is a trajectory defined, false otherwise
   */
  bool effectorHaveAtrajectory(const std::string& eeName) const { return m_effector_trajectories.count(eeName); }

  /* Helpers */

  /**
   * @brief isConsistent check if all the members of the phase are consistent together:
   * - There is a contact patch defined for all effector in contact
   * - There is only contact forces for the effector defined in contact
   * - If a trajectory is defined, it is defined between t_begin and t_final
   * - If init/end values are defined and a trajectory for this values is also defined, check that they match
   * - The times are positives and tbegin <= tmax
   * @param throw_if_inconsistent if true, throw an runtime_error if not consistent
   * @return true if consistent, false otherwise
   */
  bool isConsistent(const bool throw_if_inconsistent = false) const {
    std::cout << "WARNING : not implemented yet, return True" << std::endl;
    (void)throw_if_inconsistent;
    return true;
  }

  /**
   * @brief setCOMtrajectoryFromPoints set the c,dc and ddc curves from a list of discrete
   * COM positions, velocity and accelerations.
   * The trajectories are build with first order polynomials connecting each discrete points given.
   * this method also set the initial/final values for c, dc and ddc from the first and last discrete point given.
   * @param points list of discrete CoM positions
   * @param points_derivative list of discrete CoM velocities
   * @param points_second_derivative list of discrete CoM accelerations
   * @param time_points list of times corresponding to each discrete point given.
   */
  void setCOMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                  const t_pointX_t& points_second_derivative, const t_time_t& time_points) {
    /*
    piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, points_second_derivative, time_points);
    if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
    m_c = curve_ptr(new piecewise_t(c_t));
    m_dc = curve_ptr(c_t.compute_derivate_ptr(1));
    m_ddc = curve_ptr(c_t.compute_derivate_ptr(2));
    */
    m_c = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dc = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
    m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points_second_derivative, time_points)));
    if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3)
      throw std::invalid_argument("Dimension of the points must be 3.");

    m_c_init = point3_t(points.front());
    m_c_final = point3_t(points.back());
    m_dc_init = point3_t(points_derivative.front());
    m_dc_final = point3_t(points_derivative.back());
    m_ddc_init = point3_t(points_second_derivative.front());
    m_ddc_final = point3_t(points_second_derivative.back());
    return;
  }

  /**
   * @brief setAMtrajectoryFromPoints set the L and d_L curves from a list of discrete
   * Angular velocity values and their derivatives
   * The trajectories are build with first order polynomials connecting each discrete points given.
   * This method also set the initial/final values for L, and dL from the first and last discrete point given.
   * @param points list of discrete Angular Momentum values
   * @param points_derivative list of discrete Angular momentum derivative
   * @param time_points list of times corresponding to each discrete point given.
   */
  void setAMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                 const t_time_t& time_points) {
    /*
    piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, time_points);
    if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");
    m_L = curve_ptr(new piecewise_t(L_t));
    m_dL = curve_ptr(L_t.compute_derivate_ptr(1));
    */
    m_L = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dL = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
    if (m_L->dim() != 3 || m_dL->dim() != 3) throw std::invalid_argument("Dimension of the points must be 3.");

    m_L_init = point3_t(points.front());
    m_L_final = point3_t(points.back());
    m_dL_init = point3_t(points_derivative.front());
    m_dL_final = point3_t(points_derivative.back());
    return;
  }

  /**
   * @brief setJointsTrajectoryFromPoints set the q,dq and ddq curves from a list of discrete
   * joints positions, velocity and accelerations.
   * The trajectories are build with first order polynomials connecting each discrete points given.
   * This method also set initial/final values for q from the first and last discrete point given.
   * @param points list of discrete joints positions
   * @param points_derivative list of discrete joints velocities
   * @param points_second_derivative list of discrete joints accelerations
   * @param time_points list of times corresponding to each discrete point given.
   */
  void setJointsTrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative,
                                     const t_pointX_t& points_second_derivative, const t_time_t& time_points) {
    /*
    piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points, points_derivative, points_second_derivative, time_points);
    m_q = curve_ptr(new piecewise_t(q_t));
    m_dq = curve_ptr(q_t.compute_derivate_ptr(1));
    m_ddq = curve_ptr(q_t.compute_derivate_ptr(2));
    */
    m_q = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points)));
    m_dq = curve_ptr(new piecewise_t(
        piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points)));
    m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(
        points_second_derivative, time_points)));
    m_q_init = points.front();
    m_q_final = points.back();
    return;
  }

  /**
   * @brief getContactsBroken return the list of effectors in contact in '*this' but not in contact in 'to'
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsBroken(const ContactPhase& to) const {
    t_strings res;
    for (std::string eeName : m_effector_in_contact) {
      if (!to.isEffectorInContact(eeName)) res.push_back(eeName);
    }
    return res;
  }

  /**
   * @brief getContactsCreated return the list of effectors in contact in 'to' but not in contact in '*this'
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsCreated(const ContactPhase& to) const {
    t_strings res;
    for (std::string eeName : to.effectorsInContact()) {
      if (!isEffectorInContact(eeName)) res.push_back(eeName);
    }
    return res;
  }

  /**
   * @brief getContactsRepositioned return the list of effectors in contact both in 'to' and '*this'
   * but not at the same placement
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsRepositioned(const ContactPhase& to) const {
    t_strings res;
    const ContactPatchMap& to_patch_map = to.contactPatches();
    for (std::string eeName : m_effector_in_contact) {
      if (to.isEffectorInContact(eeName))
        if (m_contact_patches.at(eeName).placement() != to_patch_map.at(eeName).placement()) res.push_back(eeName);
    }
    return res;
  }

  /**
   * @brief getContactsVariations return the list of all the effectors whose contacts differ between *this and to
   * @param to the other ContactPhase
   * @return a list of string containing the effectors names
   */
  t_strings getContactsVariations(const ContactPhase& to) const {
    std::set<std::string> set_res;  // use intermediate set to guarantee uniqueness of element
    for (std::string eeName : getContactsBroken(to)) {
      set_res.insert(eeName);
    }
    for (std::string eeName : getContactsCreated(to)) {
      set_res.insert(eeName);
    }
    for (std::string eeName : getContactsRepositioned(to)) {
      set_res.insert(eeName);
    }
    return t_strings(set_res.begin(), set_res.end());
  }

  /* End Helpers */

  void disp(std::ostream& os) const {
    Eigen::Matrix<Scalar, 3, 5> state0(Eigen::Matrix<Scalar, 3, 5>::Zero());
    Eigen::Matrix<Scalar, 3, 5> state1(Eigen::Matrix<Scalar, 3, 5>::Zero());
    state0.block(0, 0, 3, 1) = m_c_init;
    state0.block(0, 1, 3, 1) = m_dc_init;
    state0.block(0, 2, 3, 1) = m_ddc_init;
    state0.block(0, 3, 3, 1) = m_L_init;
    state0.block(0, 4, 3, 1) = m_dL_init;
    state1.block(0, 0, 3, 1) = m_c_final;
    state1.block(0, 1, 3, 1) = m_dc_final;
    state1.block(0, 2, 3, 1) = m_ddc_final;
    state1.block(0, 3, 3, 1) = m_L_final;
    state1.block(0, 4, 3, 1) = m_dL_final;

    os << "Contact phase defined for t \\in [" << m_t_init << ";" << m_t_final << "]" << std::endl
       << "Conecting (c0,dc0,ddc0,L0,dL0) = " << std::endl
       << state0 << std::endl
       << "to        (c0,dc0,ddc0,L0,dL0) = " << std::endl
       << state1 << std::endl;
    os << "Effectors in contact " << m_effector_in_contact.size() << " : " << std::endl;
    for (t_strings::const_iterator ee = m_effector_in_contact.begin(); ee != m_effector_in_contact.end(); ++ee) {
      os << "______________________________________________" << std::endl
         << "Effector " << *ee << " contact patch:" << std::endl
         << m_contact_patches.at(*ee) << std::endl
         << "Has contact force trajectory : " << bool(m_contact_forces.count(*ee)) << std::endl
         << "Has contact normal force trajectory : " << bool(m_contact_normal_force.count(*ee)) << std::endl;
    }
  }

  template <typename S2>
  friend std::ostream& operator<<(std::ostream& os, const ContactPhaseTpl<S2>& cp) {
    cp.disp(os);
    return os;
  }

 protected:
  // private members
  /// \brief map with keys : effector name containing the contact forces
  CurveMap_t m_contact_forces;
  /// \brief map with keys : effector name containing the contact normal force
  CurveMap_t m_contact_normal_force;
  /// \brief map with keys : effector name containing the end effector trajectory
  CurveSE3Map_t m_effector_trajectories;
  /// \brief set of the name of all effector in contact for this phase
  t_strings m_effector_in_contact;
  /// \brief map effector name : contact patches. All the patches are actives
  ContactPatchMap m_contact_patches;
  /// \brief time at the beginning of the contact phase
  Scalar m_t_init;
  /// \brief time at the end of the contact phase
  Scalar m_t_final;

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    // ar& boost::serialization::make_nvp("placement", m_placement);
    unsigned int curve_version; // Curves API version related to the archive multicontact-api API version
    if(version <2){
      curve_version = 0;
    }else{
      curve_version = 1;
    }
    curves::serialization::register_types<Archive>(ar, curve_version);
    ar& boost::serialization::make_nvp("c_init", m_c_init);
    ar& boost::serialization::make_nvp("dc_init", m_dc_init);
    ar& boost::serialization::make_nvp("ddc_init", m_ddc_init);
    ar& boost::serialization::make_nvp("L_init", m_L_init);
    ar& boost::serialization::make_nvp("dL_init", m_dL_init);
    ar& boost::serialization::make_nvp("q_init", m_q_init);
    ar& boost::serialization::make_nvp("c_final", m_c_final);
    ar& boost::serialization::make_nvp("dc_final", m_dc_final);
    ar& boost::serialization::make_nvp("ddc_final", m_ddc_final);
    ar& boost::serialization::make_nvp("L_final", m_L_final);
    ar& boost::serialization::make_nvp("dL_final", m_dL_final);
    ar& boost::serialization::make_nvp("q_final", m_q_final);
    ar& boost::serialization::make_nvp("q", m_q);
    ar& boost::serialization::make_nvp("dq", m_dq);
    ar& boost::serialization::make_nvp("ddq", m_ddq);
    ar& boost::serialization::make_nvp("tau", m_tau);
    ar& boost::serialization::make_nvp("c", m_c);
    ar& boost::serialization::make_nvp("dc", m_dc);
    ar& boost::serialization::make_nvp("ddc", m_ddc);
    ar& boost::serialization::make_nvp("L", m_L);
    ar& boost::serialization::make_nvp("dL", m_dL);
    ar& boost::serialization::make_nvp("wrench", m_wrench);
    ar& boost::serialization::make_nvp("zmp", m_zmp);
    ar& boost::serialization::make_nvp("root", m_root);
    ar& boost::serialization::make_nvp("contact_forces", m_contact_forces);
    ar& boost::serialization::make_nvp("contact_normal_force", m_contact_normal_force);
    ar& boost::serialization::make_nvp("effector_trajectories", m_effector_trajectories);
    ar& boost::serialization::make_nvp("effector_in_contact", m_effector_in_contact);
    ar& boost::serialization::make_nvp("contact_patches", m_contact_patches);
    ar& boost::serialization::make_nvp("t_init", m_t_init);
    ar& boost::serialization::make_nvp("t_final", m_t_final);
  }

};  // struct contact phase

}  // namespace scenario
}  // namespace multicontact_api

DEFINE_CLASS_TEMPLATE_VERSION(typename Scalar, multicontact_api::scenario::ContactPhaseTpl<Scalar>)

#endif  // CONTACTPHASE_HPP


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "lapack_common.h"
#include <Eigen/Eigenvalues>

// computes eigen values and vectors of a general N-by-N matrix A
EIGEN_LAPACK_FUNC(syev,(char *jobz, char *uplo, int* n, Scalar* a, int *lda, Scalar* w, Scalar* /*work*/, int* lwork, int *info))
{
  // TODO exploit the work buffer
  bool query_size = *lwork==-1;

  *info = 0;
        if(*jobz!='N' && *jobz!='V')                    *info = -1;
  else  if(UPLO(*uplo)==INVALID)                        *info = -2;
  else  if(*n<0)                                        *info = -3;
  else  if(*lda<std::max(1,*n))                         *info = -5;
  else  if((!query_size) && *lwork<std::max(1,3**n-1))  *info = -8;

  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"SYEV ", &e, 6);
  }

  if(query_size)
  {
    *lwork = 0;
    return 0;
  }

  if(*n==0)
    return 0;

  PlainMatrixType mat(*n,*n);
  if(UPLO(*uplo)==UP) mat = matrix(a,*n,*n,*lda).adjoint();
  else                mat = matrix(a,*n,*n,*lda);

  bool computeVectors = *jobz=='V' || *jobz=='v';
  SelfAdjointEigenSolver<PlainMatrixType> eig(mat,computeVectors?ComputeEigenvectors:EigenvaluesOnly);

  if(eig.info()==NoConvergence)
  {
    make_vector(w,*n).setZero();
    if(computeVectors)
      matrix(a,*n,*n,*lda).setIdentity();
    //*info = 1;
    return 0;
  }

  make_vector(w,*n) = eig.eigenvalues();
  if(computeVectors)
    matrix(a,*n,*n,*lda) = eig.eigenvectors();

  return 0;
}


#include <boost/test/unit_test.hpp>

#include <physics/objects/Ground.h>
#include <physics/World.h>
#include <physics/objects/ObjectsFactory.h>

using namespace Physics;
using namespace Objects;

struct GroundTestFixture {
	GroundTestFixture() :
		w(b2Vec2(0.0f, 9.8f), 4, 8),
		obj(ObjectsFactory::init(w.getWorld())) {
	}
	World w;
	ObjectsFactory& obj;
	std::vector<float> heights = { 0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f, 0.8f };
};

BOOST_FIXTURE_TEST_SUITE(GroundTests, GroundTestFixture);

BOOST_AUTO_TEST_CASE(check_returs_valid_x_distance)
{
	Ground g(Vector2(0.0f, 0.0f), 1.1f, heights);
	BOOST_CHECK_EQUAL(g.getXDistance(), 1.1f);
}

BOOST_AUTO_TEST_CASE(check_returs_valid_heights)
{
	Ground g(Vector2(0.0f, 0.0f), 1.0f, heights);
	for (std::size_t i = 0; i < heights.size(); ++i) {
		BOOST_CHECK_EQUAL(g.getHeights()[i], heights[i]);
	}
}

BOOST_AUTO_TEST_CASE(check_returs_valid_vertices)
{
	Ground g(Vector2(0.0f, 0.0f), 1.1f, heights);
	for (std::size_t i = 0; i < heights.size(); ++i) {
		BOOST_CHECK_EQUAL(g.getVertices()[i].x, 1.1f*i);
		BOOST_CHECK_EQUAL(g.getVertices()[i].y, heights[i]);
	}
}

BOOST_AUTO_TEST_SUITE_END();


#include "drake/solvers/fbstab/components/mpc_residual.h"

#include <algorithm>
#include <cmath>
#include <stdexcept>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/mpc_data.h"
#include "drake/solvers/fbstab/components/mpc_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {

MpcResidual::MpcResidual(int N, int nx, int nu, int nc) {
  if (N <= 0 || nx <= 0 || nu <= 0 || nc <= 0) {
    throw std::runtime_error(
        "All inputs to MpcResidual::MpcResidual must be >= 1.");
  }
  N_ = N;
  nx_ = nx;
  nu_ = nu;
  nc_ = nc;
  nz_ = (N_ + 1) * (nx_ + nu_);
  nl_ = (N_ + 1) * nx_;
  nv_ = (N_ + 1) * nc_;

  z_.resize(nz_);
  l_.resize(nl_);
  v_.resize(nv_);

  Fill(0.0);
}

void MpcResidual::Fill(double a) {
  z_.setConstant(a);
  l_.setConstant(a);
  v_.setConstant(a);
}

void MpcResidual::Negate() {
  z_ *= -1;
  l_ *= -1;
  v_ *= -1;
}

double MpcResidual::Norm() const {
  return sqrt(znorm_ * znorm_ + lnorm_ * lnorm_ + vnorm_ * vnorm_);
}

double MpcResidual::Merit() const {
  const double temp = this->Norm();
  return 0.5 * temp * temp;
}

void MpcResidual::InnerResidual(const MpcVariable& x, const MpcVariable& xbar,
                                double sigma) {
  const MpcData* const data = x.data();
  if (xbar.data_ != data) {
    throw std::runtime_error(
        "In MpcResidual::InnerResidual: x and xbar have mismatched problem "
        "data.");
  }

  // r.z = H*z + f + G'*l + A'*v + sigma*(z-zbar)
  z_.setConstant(0.0);
  data->axpyf(1.0, &z_);
  data->gemvH(x.z(), 1.0, 1.0, &z_);
  data->gemvGT(x.l(), 1.0, 1.0, &z_);
  data->gemvAT(x.v(), 1.0, 1.0, &z_);
  z_.noalias() += sigma * (x.z() - xbar.z());

  // r.l = h - G*z + sigma(l - lbar)
  l_.setConstant(0.0);
  data->axpyh(1.0, &l_);
  data->gemvG(x.z(), -1.0, 1.0, &l_);
  l_.noalias() += sigma * (x.l() - xbar.l());

  // rv = phi(y + sigma*(v-vbar),v)
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y()(i) + sigma * (x.v()(i) - xbar.v()(i));
    v_(i) = pfb(ys, x.v()(i), alpha_);
  }
  znorm_ = z_.norm();
  lnorm_ = l_.norm();
  vnorm_ = v_.norm();
}

void MpcResidual::NaturalResidual(const MpcVariable& x) {
  const MpcData* const data = x.data();
  // r.z = H*z + f + G'*l + A'*v
  z_.setConstant(0.0);
  data->axpyf(1.0, &z_);
  data->gemvH(x.z(), 1.0, 1.0, &z_);
  data->gemvGT(x.l(), 1.0, 1.0, &z_);
  data->gemvAT(x.v(), 1.0, 1.0, &z_);

  // r.l = h - G*z + sigma(l - lbar)
  l_.setConstant(0.0);
  data->axpyh(1.0, &l_);
  data->gemvG(x.z(), -1.0, 1.0, &l_);

  // rv = min(y,v)
  for (int i = 0; i < nv_; i++) {
    v_(i) = std::min(x.y()(i), x.v()(i));
  }
  znorm_ = z_.norm();
  lnorm_ = l_.norm();
  vnorm_ = v_.norm();
}

void MpcResidual::PenalizedNaturalResidual(const MpcVariable& x) {
  NaturalResidual(x);

  for (int i = 0; i < nv_; i++) {
    v_(i) = alpha_ * v_(i) +
            (1 - alpha_) * std::max(0.0, x.y()(i)) * std::max(0.0, x.v()(i));
  }
  znorm_ = z_.norm();
  lnorm_ = l_.norm();
  vnorm_ = v_.norm();
}

double MpcResidual::pfb(double a, double b, double alpha) {
  double fb = a + b - sqrt(a * a + b * b);
  return alpha * fb + (1.0 - alpha) * std::max(0.0, a) * std::max(0.0, b);
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


/***************************************************************************************************************************
 * circular.cpp
 *
 * Author: Ydm
 *
 * Update Time: 2020.8.14
 *
 * descripation: demostration of circular flying using mavros
 *      1.
 *      2.
 *      3.
***************************************************************************************************************************/

#include <ros/ros.h>
#include <fstream>
#include <math.h>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <std_msgs/Bool.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>

using namespace std;

float desire_z = 5.0;				//desired altitude
float desire_Radius = 5.0;		//desired radius of circle
float MoveTimeCnt = 0.0;
float priod = 2000.0;			//to change velocity of flying using it

Eigen::Vector3d pos_target; 	//desired value in offboard mode
Eigen::Vector3d temp_pos_drone;
Eigen::Vector3d temp_pos_target;
mavros_msgs::SetMode mode_cmd;
ros::Publisher setpoint_raw_local_pub;
ros::ServiceClient set_mode_client;

enum
{
	WATTING,
	CHECKING,
	PREPARE,
	REST,
	FLY,
	FLYOVER,
}FlyState = WATTING;		//the initial state

//receive the current position of drone from controller
Eigen::Vector3d pos_drone;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	//read the drone postion from the Mavros Package [Frame: ENU]
	Eigen::Vector3d pos_drone_fcu_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);

	pos_drone = pos_drone_fcu_enu;
}

//receive the current position of drone from controller
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
	current_state = *msg;
}

//send the desired position to controller (xyz, yaw)
void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
	mavros_msgs::PositionTarget pos_setpoint;
	pos_setpoint.type_mask = 0b100111111000;	//0b 100 111 111 000 xyz + yaw

	//Bitmask to indicate which dimensions should be ignored (1 means ignore, 0 means not ignore; Bit 10 must set to 0)
	//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7: ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11: yaw, bit 12:yaw_rate
	//Bit 10 should set to 0, means no force sp

	pos_setpoint.coordinate_frame = 1;

	pos_setpoint.position.x = pos_sp[0];
	pos_setpoint.position.y = pos_sp[1];
	pos_setpoint.position.z = pos_sp[2];

	pos_setpoint.yaw = yaw_sp;

	setpoint_raw_local_pub.publish(pos_setpoint);
}

void FlyState_update(void)
{
	switch(FlyState)
	{
		case WATTING:
			if(current_state.mode != "OFFBOARD")	//waitting for offboard mode
			{
				pos_target[0] = pos_drone[0];
				pos_target[1] = pos_drone[1];
				pos_target[2] = pos_drone[2];
				temp_pos_drone[0] = pos_drone[0];
				temp_pos_drone[1] = pos_drone[1];
				temp_pos_drone[2] = pos_drone[2];
				send_pos_setpoint(pos_target, 0);
			}
			else
			{
				pos_target[0] = temp_pos_drone[0];
				pos_target[1] = temp_pos_drone[1];
				pos_target[2] = temp_pos_drone[2];
				send_pos_setpoint(pos_target, 0);
				FlyState = CHECKING;
			}
			//cout << "WATTING" << endl;
			break;

		case CHECKING:
			if(pos_drone[0] == 0 && pos_drone[1] == 0)	//no position information, land
			{
				cout << "Check error, make sure have local location" <<endl;
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			else
			{
				FlyState = PREPARE;
				MoveTimeCnt = 0;
			}
			//cout << "CHECKING" << endl;
			break;
		case PREPARE:									//fly to the first point located in x negative axis
			temp_pos_target[0] = temp_pos_drone[0] - desire_Radius;
			temp_pos_target[1] = temp_pos_drone[1];
			temp_pos_target[2] = desire_z;
			MoveTimeCnt += 2;
			if(MoveTimeCnt >= 500)
			{
				FlyState = REST;
				MoveTimeCnt = 0;
			}
			pos_target[0] = temp_pos_drone[0] + (temp_pos_target[0] - temp_pos_drone[0]) * (MoveTimeCnt/500);
			pos_target[1] = temp_pos_drone[1] + (temp_pos_target[1] - temp_pos_drone[1]) * (MoveTimeCnt/500);
			pos_target[2] = temp_pos_drone[2] + (temp_pos_target[2] - temp_pos_drone[2]) * (MoveTimeCnt/500);
			send_pos_setpoint(pos_target, 0);
			if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
			{
				FlyState = WATTING;
			}
			//cout << "PREPARE" << endl;
			break;
		case REST:
			pos_target[0] = temp_pos_drone[0] - desire_Radius;
			pos_target[1] = temp_pos_drone[1];
			pos_target[2] = desire_z;
			send_pos_setpoint(pos_target, 0);
			MoveTimeCnt += 1;
			if(MoveTimeCnt >= 100)
			{
				MoveTimeCnt = 0;
				FlyState = FLY;
			}
			if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
			{
				FlyState = WATTING;
			}
			//cout << "REST" << endl;
			break;
		case FLY:
			{
				float phase = 3.1415926;
				float Omega = 2.0 * 3.14159 * MoveTimeCnt / priod;  //0~2pi
				MoveTimeCnt += 3;
				if(MoveTimeCnt >= priod)
				{
					FlyState = FLYOVER;
				}
				pos_target[0] = temp_pos_drone[0] + desire_Radius * cos(Omega + phase);
				pos_target[1] = temp_pos_drone[1] + desire_Radius * sin(Omega + phase);
				pos_target[2] = desire_z;
				send_pos_setpoint(pos_target, 0);
				if(current_state.mode != "OFFBOARD")			//if it is switched to "onboard" mode, jump to the "WATTING"
				{
					FlyState = WATTING;
				}
			}
			//cout << "FLY" << endl;
			break;
		case FLYOVER:
			{
				mode_cmd.request.custom_mode = "AUTO.LAND";
				set_mode_client.call(mode_cmd);
				FlyState = WATTING;
			}
			cout << "FLYOVER" << endl;
			break;
		default:
			cout << "ERROR!!!" << endl;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "circular_offboard");
	ros::NodeHandle nh("~");
	ros::Rate rate(20.0);

	ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);
	ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);			//handle return function

	setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);

	set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");			//little question
		nh.param<float>("desire_z", desire_z, 5.0);
		nh.param<float>("desire_Radius", desire_Radius, 5.0);

	cout << "circular node started!!!" << endl;

	while(ros::ok())
	{
		FlyState_update();
		ros::spinOnce();
		rate.sleep();

	}
	return 0;
}


#ifndef VIENNACL_LINALG_AMG_HPP_
#define VIENNACL_LINALG_AMG_HPP_

/* =========================================================================
   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
============================================================================= */

/** @file viennacl/linalg/amg.hpp
    @brief Main include file for algebraic multigrid (AMG) preconditioners.  Experimental.

    Implementation contributed by Markus Wagner
*/

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <vector>
#include <cmath>
#include "viennacl/forwards.h"
#include "viennacl/tools/tools.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/direct_solve.hpp"

#include "viennacl/linalg/detail/amg/amg_base.hpp"
#include "viennacl/linalg/detail/amg/amg_coarse.hpp"
#include "viennacl/linalg/detail/amg/amg_interpol.hpp"

#include <map>

#ifdef VIENNACL_WITH_OPENMP
 #include <omp.h>
#endif

#include "viennacl/linalg/detail/amg/amg_debug.hpp"

#define VIENNACL_AMG_COARSE_LIMIT 50
#define VIENNACL_AMG_MAX_LEVELS 100

namespace viennacl
{
namespace linalg
{

typedef detail::amg::amg_tag          amg_tag;

/** @brief Setup AMG preconditioner
*
* @param A            Operator matrices on all levels
* @param P            Prolongation/Interpolation operators on all levels
* @param pointvector  Vector of points on all levels
* @param tag          AMG preconditioner tag
*/
template<typename InternalT1, typename InternalT2>
void amg_setup(InternalT1 & A, InternalT1 & P, InternalT2 & pointvector, amg_tag & tag)
{
  typedef typename InternalT2::value_type      PointVectorType;

  unsigned int i, iterations, c_points, f_points;
  detail::amg::amg_slicing<InternalT1,InternalT2> slicing;

  // Set number of iterations. If automatic coarse grid construction is chosen (0), then set a maximum size and stop during the process.
  iterations = tag.get_coarselevels();
  if (iterations == 0)
    iterations = VIENNACL_AMG_MAX_LEVELS;

  // For parallel coarsenings build data structures (number of threads set automatically).
  if (tag.get_coarse() == VIENNACL_AMG_COARSE_RS0 || tag.get_coarse() == VIENNACL_AMG_COARSE_RS3)
    slicing.init(iterations);

  for (i=0; i<iterations; ++i)
  {
    // Initialize Pointvector on level i and construct points.
    pointvector[i] = PointVectorType(static_cast<unsigned int>(A[i].size1()));
    pointvector[i].init_points();

    // Construct C and F points on coarse level (i is fine level, i+1 coarse level).
    detail::amg::amg_coarse (i, A, pointvector, slicing, tag);

    // Calculate number of C and F points on level i.
    c_points = pointvector[i].get_cpoints();
    f_points = pointvector[i].get_fpoints();

    #if defined (VIENNACL_AMG_DEBUG) //or defined(VIENNACL_AMG_DEBUGBENCH)
    std::cout << "Level " << i << ": ";
    std::cout << "No of C points = " << c_points << ", ";
    std::cout << "No of F points = " << f_points << std::endl;
    #endif

    // Stop routine when the maximal coarse level is found (no C or F point). Coarsest level is level i.
    if (c_points == 0 || f_points == 0)
      break;

    // Construct interpolation matrix for level i.
    detail::amg::amg_interpol (i, A, P, pointvector, tag);

    // Compute coarse grid operator (A[i+1] = R * A[i] * P) with R = trans(P).
    detail::amg::amg_galerkin_prod(A[i], P[i], A[i+1]);

    // Test triple matrix product. Very slow for large matrix sizes (ublas).
    // test_triplematprod(A[i],P[i],A[i+1]);

    pointvector[i].delete_points();

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "Coarse Grid Operator Matrix:" << std::endl;
    printmatrix (A[i+1]);
    #endif

    // If Limit of coarse points is reached then stop. Coarsest level is level i+1.
    if (tag.get_coarselevels() == 0 && c_points <= VIENNACL_AMG_COARSE_LIMIT)
    {
      tag.set_coarselevels(i+1);
      return;
    }
  }
  tag.set_coarselevels(i);
}

/** @brief Initialize AMG preconditioner
*
* @param mat          System matrix
* @param A            Operator matrices on all levels
* @param P            Prolongation/Interpolation operators on all levels
* @param pointvector  Vector of points on all levels
* @param tag          AMG preconditioner tag
*/
template<typename MatrixT, typename InternalT1, typename InternalT2>
void amg_init(MatrixT const & mat, InternalT1 & A, InternalT1 & P, InternalT2 & pointvector, amg_tag & tag)
{
  //typedef typename MatrixType::value_type ScalarType;
  typedef typename InternalT1::value_type SparseMatrixType;

  if (tag.get_coarselevels() > 0)
  {
    A.resize(tag.get_coarselevels()+1);
    P.resize(tag.get_coarselevels());
    pointvector.resize(tag.get_coarselevels());
  }
  else
  {
    A.resize(VIENNACL_AMG_MAX_LEVELS+1);
    P.resize(VIENNACL_AMG_MAX_LEVELS);
    pointvector.resize(VIENNACL_AMG_MAX_LEVELS);
  }

  // Insert operator matrix as operator for finest level.
  SparseMatrixType A0(mat);
  A.insert_element(0, A0);
}

/** @brief Save operators after setup phase for CPU computation.
*
* @param A      Operator matrices on all levels on the CPU
* @param P      Prolongation/Interpolation operators on all levels on the CPU
* @param R      Restriction operators on all levels on the CPU
* @param A_setup    Operators matrices on all levels from setup phase
* @param P_setup    Prolongation/Interpolation operators on all levels from setup phase
* @param tag    AMG preconditioner tag
*/
template<typename InternalT1, typename InternalT2>
void amg_transform_cpu(InternalT1 & A, InternalT1 & P, InternalT1 & R, InternalT2 & A_setup, InternalT2 & P_setup, amg_tag & tag)
{
  //typedef typename InternalType1::value_type MatrixType;

  // Resize internal data structures to actual size.
  A.resize(tag.get_coarselevels()+1);
  P.resize(tag.get_coarselevels());
  R.resize(tag.get_coarselevels());

  // Transform into matrix type.
  for (unsigned int i=0; i<tag.get_coarselevels()+1; ++i)
  {
    A[i].resize(A_setup[i].size1(),A_setup[i].size2(),false);
    A[i] = A_setup[i];
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    P[i].resize(P_setup[i].size1(),P_setup[i].size2(),false);
    P[i] = P_setup[i];
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    R[i].resize(P_setup[i].size2(),P_setup[i].size1(),false);
    P_setup[i].set_trans(true);
    R[i] = P_setup[i];
    P_setup[i].set_trans(false);
  }
}

/** @brief Save operators after setup phase for GPU computation.
*
* @param A          Operator matrices on all levels on the GPU
* @param P          Prolongation/Interpolation operators on all levels on the GPU
* @param R          Restriction operators on all levels on the GPU
* @param A_setup    Operators matrices on all levels from setup phase
* @param P_setup    Prolongation/Interpolation operators on all levels from setup phase
* @param tag        AMG preconditioner tag
* @param ctx        Optional context in which the auxiliary objects are created (one out of multiple OpenCL contexts, CUDA, host)
*/
template<typename InternalT1, typename InternalT2>
void amg_transform_gpu(InternalT1 & A, InternalT1 & P, InternalT1 & R, InternalT2 & A_setup, InternalT2 & P_setup, amg_tag & tag, viennacl::context ctx)
{
  typedef typename InternalT2::value_type::value_type    NumericType;

  // Resize internal data structures to actual size.
  A.resize(tag.get_coarselevels()+1);
  P.resize(tag.get_coarselevels());
  R.resize(tag.get_coarselevels());

  // Copy to GPU using the internal sparse matrix structure: std::vector<std::map>.
  for (unsigned int i=0; i<tag.get_coarselevels()+1; ++i)
  {
    viennacl::switch_memory_context(A[i], ctx);
    //A[i].resize(A_setup[i].size1(),A_setup[i].size2(),false);
    viennacl::copy(*(A_setup[i].get_internal_pointer()),A[i]);
  }
  for (unsigned int i=0; i<tag.get_coarselevels(); ++i)
  {
    // find number of nonzeros in P:
    vcl_size_t nonzeros = 0;
    for (vcl_size_t j=0; j<P_setup[i].get_internal_pointer()->size(); ++j)
      nonzeros += (*P_setup[i].get_internal_pointer())[j].size();

    viennacl::switch_memory_context(P[i], ctx);
    //P[i].resize(P_setup[i].size1(),P_setup[i].size2(),false);
    viennacl::detail::copy_impl(tools::const_sparse_matrix_adapter<NumericType>(*(P_setup[i].get_internal_pointer()), P_setup[i].size1(), P_setup[i].size2()), P[i], nonzeros);
    //viennacl::copy((boost::numeric::ublas::compressed_matrix<ScalarType>)P_setup[i],P[i]);

    viennacl::switch_memory_context(R[i], ctx);
    //R[i].resize(P_setup[i].size2(),P_setup[i].size1(),false);
    P_setup[i].set_trans(true);
    viennacl::detail::copy_impl(tools::const_sparse_matrix_adapter<NumericType>(*(P_setup[i].get_internal_pointer()), P_setup[i].size1(), P_setup[i].size2()), R[i], nonzeros);
    P_setup[i].set_trans(false);
  }
}

/** @brief Setup data structures for precondition phase.
*
* @param result      Result vector on all levels
* @param rhs         RHS vector on all levels
* @param residual    Residual vector on all levels
* @param A           Operators matrices on all levels from setup phase
* @param tag         AMG preconditioner tag
*/
template<typename InternalVectorT, typename SparseMatrixT>
void amg_setup_apply(InternalVectorT & result, InternalVectorT & rhs, InternalVectorT & residual, SparseMatrixT const & A, amg_tag const & tag)
{
  typedef typename InternalVectorT::value_type VectorType;

  result.resize(tag.get_coarselevels()+1);
  rhs.resize(tag.get_coarselevels()+1);
  residual.resize(tag.get_coarselevels());

  for (unsigned int level=0; level < tag.get_coarselevels()+1; ++level)
  {
    result[level] = VectorType(A[level].size1());
    result[level].clear();
    rhs[level] = VectorType(A[level].size1());
    rhs[level].clear();
  }
  for (unsigned int level=0; level < tag.get_coarselevels(); ++level)
  {
    residual[level] = VectorType(A[level].size1());
    residual[level].clear();
  }
}

/** @brief Setup data structures for precondition phase for later use on the GPU
*
* @param result      Result vector on all levels
* @param rhs         RHS vector on all levels
* @param residual    Residual vector on all levels
* @param A           Operators matrices on all levels from setup phase
* @param tag         AMG preconditioner tag
* @param ctx         Optional context in which the auxiliary objects are created (one out of multiple OpenCL contexts, CUDA, host)
*/
template<typename InternalVectorT, typename SparseMatrixT>
void amg_setup_apply(InternalVectorT & result, InternalVectorT & rhs, InternalVectorT & residual, SparseMatrixT const & A, amg_tag const & tag, viennacl::context ctx)
{
  typedef typename InternalVectorT::value_type VectorType;

  result.resize(tag.get_coarselevels()+1);
  rhs.resize(tag.get_coarselevels()+1);
  residual.resize(tag.get_coarselevels());

  for (unsigned int level=0; level < tag.get_coarselevels()+1; ++level)
  {
    result[level] = VectorType(A[level].size1(), ctx);
      rhs[level] = VectorType(A[level].size1(), ctx);
  }
  for (unsigned int level=0; level < tag.get_coarselevels(); ++level)
  {
    residual[level] = VectorType(A[level].size1(), ctx);
  }
}

/** @brief Pre-compute LU factorization for direct solve (ublas library).
 *  @brief Speeds up precondition phase as this is computed only once overall instead of once per iteration.
*
* @param op           Operator matrix for direct solve
* @param permutation  Permutation matrix which saves the factorization result
* @param A            Operator matrix on coarsest level
*/
template<typename NumericT, typename SparseMatrixT>
void amg_lu(boost::numeric::ublas::compressed_matrix<NumericT> & op, boost::numeric::ublas::permutation_matrix<> & permutation, SparseMatrixT const & A)
{
  typedef typename SparseMatrixT::const_iterator1 ConstRowIterator;
  typedef typename SparseMatrixT::const_iterator2 ConstColIterator;

  // Copy to operator matrix. Needed
  op.resize(A.size1(),A.size2(),false);
  for (ConstRowIterator row_iter = A.begin1(); row_iter != A.end1(); ++row_iter)
    for (ConstColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      op (col_iter.index1(), col_iter.index2()) = *col_iter;

  // Permutation matrix has to be reinitialized with actual size. Do not clear() or resize()!
  permutation = boost::numeric::ublas::permutation_matrix<> (op.size1());
  boost::numeric::ublas::lu_factorize(op, permutation);
}

/** @brief AMG preconditioner class, can be supplied to solve()-routines
*/
template<typename MatrixT>
class amg_precond
{
  typedef typename MatrixT::value_type                NumericType;
  typedef boost::numeric::ublas::vector<NumericType>  VectorType;
  typedef detail::amg::amg_sparsematrix<NumericType>  SparseMatrixType;
  typedef detail::amg::amg_pointvector                PointVectorType;

  typedef typename SparseMatrixType::const_iterator1  InternalConstRowIterator;
  typedef typename SparseMatrixType::const_iterator2  InternalConstColIterator;
  typedef typename SparseMatrixType::iterator1        InternalRowIterator;
  typedef typename SparseMatrixType::iterator2        InternalColIterator;

  boost::numeric::ublas::vector<SparseMatrixType> A_setup_;
  boost::numeric::ublas::vector<SparseMatrixType> P_setup_;
  boost::numeric::ublas::vector<MatrixT>          A_;
  boost::numeric::ublas::vector<MatrixT>          P_;
  boost::numeric::ublas::vector<MatrixT>          R_;
  boost::numeric::ublas::vector<PointVectorType>  pointvector_;

  mutable boost::numeric::ublas::compressed_matrix<NumericType> op_;
  mutable boost::numeric::ublas::permutation_matrix<>           permutation_;

  mutable boost::numeric::ublas::vector<VectorType> result_;
  mutable boost::numeric::ublas::vector<VectorType> rhs_;
  mutable boost::numeric::ublas::vector<VectorType> residual_;

  mutable bool done_init_apply_;

  amg_tag tag_;
public:

  amg_precond(): permutation_(0) {}
  /** @brief The constructor. Saves system matrix, tag and builds data structures for setup.
  *
  * @param mat  System matrix
  * @param tag  The AMG tag
  */
  amg_precond(MatrixT const & mat, amg_tag const & tag): permutation_(0)
  {
    tag_ = tag;
    // Initialize data structures.
    amg_init (mat, A_setup_, P_setup_, pointvector_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Start setup phase for this class and copy data structures.
  */
  void setup()
  {
    // Start setup phase.
    amg_setup(A_setup_, P_setup_, pointvector_, tag_);
    // Transform to CPU-Matrixtype for precondition phase.
    amg_transform_cpu(A_, P_, R_, A_setup_, P_setup_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Prepare data structures for preconditioning:
   *  Build data structures for precondition phase.
   *  Do LU factorization on coarsest level.
  */
  void init_apply() const
  {
    // Setup precondition phase (Data structures).
    amg_setup_apply(result_, rhs_, residual_, A_setup_, tag_);
    // Do LU factorization for direct solve.
    amg_lu(op_, permutation_, A_setup_[tag_.get_coarselevels()]);

    done_init_apply_ = true;
  }

  /** @brief Returns complexity measures.
  *
  * @param avgstencil  Average stencil sizes on all levels
  * @return            Operator complexity of AMG method
  */
  template<typename VectorT>
  NumericType calc_complexity(VectorT & avgstencil)
  {
    avgstencil = VectorT(tag_.get_coarselevels()+1);
    unsigned int nonzero=0, systemmat_nonzero=0, level_coefficients=0;

    for (unsigned int level=0; level < tag_.get_coarselevels()+1; ++level)
    {
      level_coefficients = 0;
      for (InternalRowIterator row_iter = A_setup_[level].begin1(); row_iter != A_setup_[level].end1(); ++row_iter)
      {
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (level == 0)
            systemmat_nonzero++;
          nonzero++;
          level_coefficients++;
        }
      }
      avgstencil[level] = static_cast<NumericType>(level_coefficients)/static_cast<NumericType>(A_setup_[level].size1());
    }
    return static_cast<NumericType>(nonzero) / static_cast<NumericType>(systemmat_nonzero);
  }

  /** @brief Precondition Operation
  *
  * @param vec The vector to which preconditioning is applied to (ublas version)
  */
  template<typename VectorT>
  void apply(VectorT & vec) const
  {
    // Build data structures and do lu factorization before first iteration step.
    if (!done_init_apply_)
      init_apply();

    int level;

    // Precondition operation (Yang, p.3)
    rhs_[0] = vec;
    for (level=0; level<static_cast<int>(tag_.get_coarselevels()); level++)
    {
      result_[level].clear();

      // Apply Smoother presmooth_ times.
      smooth_jacobi (level, tag_.get_presmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After presmooth:" << std::endl;
      printvector(result_[level]);
      #endif

      // Compute residual.
      residual_[level] = rhs_[level] - boost::numeric::ublas::prod(A_[level], result_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Residual:" << std::endl;
      printvector(residual_[level]);
      #endif

      // Restrict to coarse level. Restricted residual is RHS of coarse level.
      rhs_[level+1] = boost::numeric::ublas::prod(R_[level], residual_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Restricted Residual: " << std::endl;
      printvector(rhs_[level+1]);
      #endif
    }

    // On highest level use direct solve to solve equation.
    result_[level] = rhs_[level];
    boost::numeric::ublas::lu_substitute(op_, permutation_, result_[level]);

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "After direct solve: " << std::endl;
    printvector(result_[level]);
    #endif

    for (level=tag_.get_coarselevels()-1; level >= 0; level--)
    {
      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Coarse Error: " << std::endl;
      printvector(result_[level+1]);
      #endif

      // Interpolate error to fine level. Correct solution by adding error.
      result_[level] += boost::numeric::ublas::prod(P_[level], result_[level+1]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Corrected Result: " << std::endl;
      printvector(result_[level]);
      #endif

      // Apply Smoother postsmooth_ times.
      smooth_jacobi(level, tag_.get_postsmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After postsmooth: " << std::endl;
      printvector(result_[level]);
      #endif
    }
    vec = result_[0];
  }

  /** @brief (Weighted) Jacobi Smoother (CPU version)
  * @param level       Coarse level to which smoother is applied to
  * @param iterations  Number of smoother iterations
  * @param x           The vector smoothing is applied to
  * @param rhs_smooth  The right hand side of the equation for the smoother
  */
  template<typename VectorT>
  void smooth_jacobi(int level, int const iterations, VectorT & x, VectorT const & rhs_smooth) const
  {
    VectorT old_result(x.size());
    long index;

    for (int i=0; i<iterations; ++i)
    {
      old_result = x;
      x.clear();
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for
#endif
      for (index=0; index < static_cast<long>(A_setup_[level].size1()); ++index)
      {
        InternalConstRowIterator row_iter = A_setup_[level].begin1();
        row_iter += index;
        NumericType sum  = 0;
        NumericType diag = 1;
        for (InternalConstColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (col_iter.index1() == col_iter.index2())
            diag = *col_iter;
          else
            sum += *col_iter * old_result[col_iter.index2()];
        }
        x[index]= static_cast<NumericType>(tag_.get_jacobiweight()) * (rhs_smooth[index] - sum) / diag + (1-static_cast<NumericType>(tag_.get_jacobiweight())) * old_result[index];
      }
    }
  }

  amg_tag & tag() { return tag_; }
};

/** @brief AMG preconditioner class, can be supplied to solve()-routines.
*
*  Specialization for compressed_matrix
*/
template<typename NumericT, unsigned int AlignmentV>
class amg_precond< compressed_matrix<NumericT, AlignmentV> >
{
  typedef viennacl::compressed_matrix<NumericT, AlignmentV> MatrixType;
  typedef viennacl::vector<NumericT>                        VectorType;
  typedef detail::amg::amg_sparsematrix<NumericT>           SparseMatrixType;
  typedef detail::amg::amg_pointvector                      PointVectorType;

  typedef typename SparseMatrixType::const_iterator1   InternalConstRowIterator;
  typedef typename SparseMatrixType::const_iterator2   InternalConstColIterator;
  typedef typename SparseMatrixType::iterator1         InternalRowIterator;
  typedef typename SparseMatrixType::iterator2         InternalColIterator;

  boost::numeric::ublas::vector<SparseMatrixType> A_setup_;
  boost::numeric::ublas::vector<SparseMatrixType> P_setup_;
  boost::numeric::ublas::vector<MatrixType>       A_;
  boost::numeric::ublas::vector<MatrixType>       P_;
  boost::numeric::ublas::vector<MatrixType>       R_;
  boost::numeric::ublas::vector<PointVectorType>  pointvector_;

  mutable boost::numeric::ublas::compressed_matrix<NumericT>  op_;
  mutable boost::numeric::ublas::permutation_matrix<>         permutation_;

  mutable boost::numeric::ublas::vector<VectorType> result_;
  mutable boost::numeric::ublas::vector<VectorType> rhs_;
  mutable boost::numeric::ublas::vector<VectorType> residual_;

  viennacl::context ctx_;

  mutable bool done_init_apply_;

  amg_tag tag_;

public:

  amg_precond(): permutation_(0) {}

  /** @brief The constructor. Builds data structures.
  *
  * @param mat  System matrix
  * @param tag  The AMG tag
  */
  amg_precond(compressed_matrix<NumericT, AlignmentV> const & mat, amg_tag const & tag): permutation_(0), ctx_(viennacl::traits::context(mat))
  {
    tag_ = tag;

    // Copy to CPU. Internal structure of sparse matrix is used for copy operation.
    std::vector<std::map<unsigned int, NumericT> > mat2 = std::vector<std::map<unsigned int, NumericT> >(mat.size1());
    viennacl::copy(mat, mat2);

    // Initialize data structures.
    amg_init (mat2, A_setup_, P_setup_, pointvector_, tag_);

    done_init_apply_ = false;
  }

  /** @brief Start setup phase for this class and copy data structures.
  */
  void setup()
  {
    // Start setup phase.
    amg_setup(A_setup_, P_setup_, pointvector_, tag_);
    // Transform to GPU-Matrixtype for precondition phase.
    amg_transform_gpu(A_, P_, R_, A_setup_, P_setup_, tag_, ctx_);

    done_init_apply_ = false;
  }

  /** @brief Prepare data structures for preconditioning:
   *  Build data structures for precondition phase.
   *  Do LU factorization on coarsest level.
  */
  void init_apply() const
  {
    // Setup precondition phase (Data structures).
    amg_setup_apply(result_, rhs_, residual_, A_setup_, tag_, ctx_);
    // Do LU factorization for direct solve.
    amg_lu(op_, permutation_, A_setup_[tag_.get_coarselevels()]);

    done_init_apply_ = true;
  }

  /** @brief Returns complexity measures
  *
  * @param avgstencil  Average stencil sizes on all levels
  * @return     Operator complexity of AMG method
  */
  template<typename VectorT>
  NumericT calc_complexity(VectorT & avgstencil)
  {
    avgstencil = VectorT(tag_.get_coarselevels()+1);
    unsigned int nonzero=0, systemmat_nonzero=0, level_coefficients=0;

    for (unsigned int level=0; level < tag_.get_coarselevels()+1; ++level)
    {
      level_coefficients = 0;
      for (InternalRowIterator row_iter = A_setup_[level].begin1(); row_iter != A_setup_[level].end1(); ++row_iter)
      {
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          if (level == 0)
            systemmat_nonzero++;
          nonzero++;
          level_coefficients++;
        }
      }
      avgstencil[level] = level_coefficients/static_cast<double>(A_[level].size1());
    }
    return nonzero/static_cast<double>(systemmat_nonzero);
  }

  /** @brief Precondition Operation
  *
  * @param vec The vector to which preconditioning is applied to
  */
  template<typename VectorT>
  void apply(VectorT & vec) const
  {
    if (!done_init_apply_)
      init_apply();

    vcl_size_t level;

    // Precondition operation (Yang, p.3).
    rhs_[0] = vec;
    for (level=0; level < tag_.get_coarselevels(); level++)
    {
      result_[level].clear();

      // Apply Smoother presmooth_ times.
      smooth_jacobi(level, tag_.get_presmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After presmooth: " << std::endl;
      printvector(result_[level]);
      #endif

      // Compute residual.
      //residual[level] = rhs_[level] - viennacl::linalg::prod(A_[level], result_[level]);
      residual_[level] = viennacl::linalg::prod(A_[level], result_[level]);
      residual_[level] = rhs_[level] - residual_[level];

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Residual: " << std::endl;
      printvector(residual_[level]);
      #endif

      // Restrict to coarse level. Result is RHS of coarse level equation.
      //residual_coarse[level] = viennacl::linalg::prod(R[level],residual[level]);
      rhs_[level+1] = viennacl::linalg::prod(R_[level], residual_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Restricted Residual: " << std::endl;
      printvector(rhs_[level+1]);
      #endif
    }

    // On highest level use direct solve to solve equation (on the CPU)
    //TODO: Use GPU direct solve!
    result_[level] = rhs_[level];
    boost::numeric::ublas::vector<NumericT> result_cpu(result_[level].size());

    viennacl::copy(result_[level], result_cpu);
    boost::numeric::ublas::lu_substitute(op_, permutation_, result_cpu);
    viennacl::copy(result_cpu, result_[level]);

    #ifdef VIENNACL_AMG_DEBUG
    std::cout << "After direct solve: " << std::endl;
    printvector (result[level]);
    #endif

    for (int level2 = static_cast<int>(tag_.get_coarselevels()-1); level2 >= 0; level2--)
    {
      level = static_cast<vcl_size_t>(level2);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Coarse Error: " << std::endl;
      printvector(result[level+1]);
      #endif

      // Interpolate error to fine level and correct solution.
      result_[level] += viennacl::linalg::prod(P_[level], result_[level+1]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Corrected Result: " << std::endl;
      printvector(result_[level]);
      #endif

      // Apply Smoother postsmooth_ times.
      smooth_jacobi(level, tag_.get_postsmooth(), result_[level], rhs_[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "After postsmooth: " << std::endl;
      printvector(result_[level]);
      #endif
    }
    vec = result_[0];
  }

  /** @brief Jacobi Smoother (GPU version)
  * @param level       Coarse level to which smoother is applied to
  * @param iterations  Number of smoother iterations
  * @param x           The vector smoothing is applied to
  * @param rhs_smooth  The right hand side of the equation for the smoother
  */
  template<typename VectorT>
  void smooth_jacobi(vcl_size_t level, unsigned int iterations, VectorT & x, VectorT const & rhs_smooth) const
  {
    VectorType old_result = x;

    viennacl::ocl::context & ctx = const_cast<viennacl::ocl::context &>(viennacl::traits::opencl_handle(x).context());
    viennacl::linalg::opencl::kernels::compressed_matrix<NumericT>::init(ctx);
    viennacl::ocl::kernel & k = ctx.get_kernel(viennacl::linalg::opencl::kernels::compressed_matrix<NumericT>::program_name(), "jacobi");

    for (unsigned int i=0; i<iterations; ++i)
    {
      if (i > 0)
        old_result = x;
      x.clear();
      viennacl::ocl::enqueue(k(A_[level].handle1().opencl_handle(), A_[level].handle2().opencl_handle(), A_[level].handle().opencl_handle(),
                              static_cast<NumericT>(tag_.get_jacobiweight()),
                              viennacl::traits::opencl_handle(old_result),
                              viennacl::traits::opencl_handle(x),
                              viennacl::traits::opencl_handle(rhs_smooth),
                              static_cast<cl_uint>(rhs_smooth.size())));

    }
  }

  amg_tag & tag() { return tag_; }
};

}
}

#endif


#include <NDTMatcherF2F.hh>
#include <NDTMap.hh>
#include <NDTHistogram.hh>
#include <OctTree.hh>
#include <AdaptiveOctTree.hh>
#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include "pcl/registration/icp.h"
#include "pcl/filters/voxel_grid.h"
#include <LazzyGrid.hh>

#include <cstdio>
#include <Eigen/Eigen>
#include <PointCloudUtils.hh>
#include <fstream>

using namespace std;

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

    std::ofstream logger ("/home/tsv/ndt_tmp/results_histogram.txt");
    //cout.precision(15);

    pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3, cloud4, cloud5, cloud6, cloud7 ;
    double __res[] = {0.5, 1, 2, 4};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
    lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tin, Tout, Tndt;

    struct timeval tv_start, tv_end;

    double bestscore = INT_MAX;

    Tout.setIdentity();
    bool succ = false;
    if(argc == 3)
    {
        //we do a single scan to scan registration

        cloud1 = lslgeneric::readVRML(argv[1]);
        cloud2 = lslgeneric::readVRML(argv[2]);

        // lslgeneric::AdaptiveOctTree::MIN_CELL_SIZE = 0.01;
        lslgeneric::OctTree tr;
        //lslgeneric::LazzyGrid tr(0.5);
        lslgeneric::OctTree::BIG_CELL_SIZE = 1;
        lslgeneric::OctTree::SMALL_CELL_SIZE = 0.2;
        double finalscore;

        gettimeofday(&tv_start,NULL);

        lslgeneric::NDTMap fixed(&tr);
        fixed.loadPointCloud(cloud1);
        lslgeneric::NDTMap moving(&tr);
        moving.loadPointCloud(cloud2);
        lslgeneric::NDTMap moved(&tr);

        fixed.computeNDTCells();
        moving.computeNDTCells();

        lslgeneric::NDTHistogram fixedH(fixed);
        lslgeneric::NDTHistogram movingH(moving);

        //cout<<"1 =========== \n";
        //fixedH.printHistogram(true);
        //cout<<"2 =========== \n";
        //movingH.printHistogram(true);

        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;

        movingH.bestFitToHistogram(fixedH,T);

        //cout<<" ==================== \n Transform R "<<T.rotation()<<"\nt "<<T.translation().transpose()<<endl;

        for(int q=0; q<1; q++)
        {
            if(q!=3)
            {
                movingH.getTransform(q,T);
            }
            else
            {
                T.setIdentity();
            }
            cout<<"T init "<<T.translation().transpose()<<" r "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
            Tndt.setIdentity();
            cloud3 = lslgeneric::transformPointCloud(T,cloud2);
            bool ret = matcherF2F.match(cloud1,cloud3,Tndt);
            finalscore = matcherF2F.finalscore;
            cout<<"final score at "<<q<<" is "<<finalscore<<endl;
            if(finalscore < bestscore)
            {
                Tout = Tndt*T;
                bestscore = finalscore;
                cout<<"score = "<<bestscore<<"best is "<<q<<endl;
            }
            cout<<"T fin "<<Tout.translation().transpose()<<" r "<<Tout.rotation().eulerAngles(0,1,2).transpose()<<endl;
        }

        cloud4 = lslgeneric::transformPointCloud(Tout,cloud2);
        /*
        	//movingH.getTransform(1,T);
        	//moved.loadPointCloud(cloud3);
        	//moved.computeNDTCells();
        	//bool ret = matcherF2F.match(fixed,moved,Tout);
        	bool ret = matcherF2F.match(cloud1,cloud3,Tndt);
        	finalscore = matcherF2F.finalscore;
        	if(finalscore < bestscore) {
        	    Tout = Tndt;
        	    bestscore = finalscore;
        	    cout<<"score = "<<bestscore<<"best is 1!\n";
        	    cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
        	}

        	//option2
        	Tndt.setIdentity();
        	movingH.getTransform(1,T);
        	cloud3 = lslgeneric::transformPointCloud(T,cloud2);
        	ret = matcherF2F.match(cloud1,cloud3,Tndt);
        	finalscore = matcherF2F.finalscore;
        	if(finalscore < bestscore) {
        	    Tout = Tndt;
        	    bestscore = finalscore;
        	    cout<<"score = "<<bestscore<<"best is 2!\n";
        	    cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
        	}
        	//cloud5 = lslgeneric::transformPointCloud(Tndt,cloud3);

        	//option3
        	Tndt.setIdentity();
        	movingH.getTransform(2,T);
        	cloud3 = lslgeneric::transformPointCloud(T,cloud2);
        	ret = matcherF2F.match(cloud1,cloud3,Tndt);
        	finalscore = matcherF2F.finalscore;
        	if(finalscore < bestscore) {
        	    Tout = Tndt;
        	    bestscore = finalscore;
        	    cout<<"score = "<<bestscore<<"best is 3!\n";
        	    cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
        	}
        	//cloud6 = lslgeneric::transformPointCloud(Tndt,cloud3);

        	//option4
        	Tndt.setIdentity();
        	ret = matcherF2F.match(cloud1,cloud2,Tndt);
        	finalscore = matcherF2F.finalscore;
        	if(finalscore < bestscore) {
        	    Tout = Tndt;
        	    bestscore = finalscore;
        	    cout<<"score = "<<bestscore<<"best is 4!\n";
        	    cloud4 = lslgeneric::transformPointCloud(Tndt,cloud2);
        	}
        	//cloud7 = lslgeneric::transformPointCloud(Tndt,cloud2);
        */
        gettimeofday(&tv_end,NULL);

        cout<<" TIME: "<<
            (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.<<endl;

        char fname[50];
        snprintf(fname,49,"/home/tsv/ndt_tmp/c_offset.wrl");
        FILE *fout = fopen(fname,"w");
        fprintf(fout,"#VRML V2.0 utf8\n");
        //green = target
        lslgeneric::writeToVRML(fout,cloud1,Eigen::Vector3d(0,1,0));
        //red = before histogram
        lslgeneric::writeToVRML(fout,cloud2,Eigen::Vector3d(1,0,0));
        //blue = after histogram
        //lslgeneric::writeToVRML(fout,cloud3,Eigen::Vector3d(0,0,1));

        {
            Eigen::Vector3d out = Tout.rotation().eulerAngles(0,1,2);
            cout<<"rot: "<<out.transpose()<<endl;
            cout<<"translation "<<Tout.translation().transpose()<<endl;
            logger<<"rot: "<<out.transpose()<<endl;
            logger<<"translation "<<Tout.translation()<<endl;
            //white = after registration
            lslgeneric::writeToVRML(fout,cloud4,Eigen::Vector3d(1,1,1));
            //lslgeneric::writeToVRML(fout,cloud5,Eigen::Vector3d(0.6,0.6,0.6));
            //lslgeneric::writeToVRML(fout,cloud6,Eigen::Vector3d(1,1,1));
            //lslgeneric::writeToVRML(fout,cloud7,Eigen::Vector3d(0,1,1));
        }

        fclose(fout);

        succ = true;
    }
    return 0;
}