// Copyright 2004 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef _ALG_CTX_BWTC_H
#define _ALG_CTX_BWTC_H

#include <stack>
#include <vector>
#include <boost/graph/overloading.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/named_function_params.hpp>
#include <algorithm>

#include <functional>

using namespace boost;

namespace nctx {

namespace detail { namespace graph {

  /**
   * @brief Dijkstra visitor keeping track of vertices, shortest paths and updates due to decisions on constraints.
   *
   * This visitor is based on the Boost implementation. The corresponding documentation states:
   *
   * Customized visitor passed to Dijkstra's algorithm by Brandes'
   * betweenness centrality algorithm. This visitor is responsible for
   * keeping track of the order in which vertices are discovered, the
   * predecessors on the shortest path(s) to a vertex, and the number
   * of shortest paths.
   *
   * In the extension of nctx, this visitor is also responsible for keeping track of the decisions resulting from the decision function. If the function prohibits traversal to a descending node, the visitor rolls back distance updates by Dijkstra's algorithm.
   */
  template<typename Graph, typename WeightMap, typename IncomingMap,
           typename DistanceMap, typename PathCountMap>
  struct brandes_dijkstra_visitor : public bfs_visitor<>
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename std::function<bool(typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_function;

    brandes_dijkstra_visitor(std::stack<vertex_descriptor>& ordered_vertices,
                             WeightMap weight,
                             IncomingMap incoming,
                             DistanceMap distance,
                             PathCountMap path_count_constraint,
                             PathCountMap path_count_unconstraint,
                             decision_function &betw_decision_fct,
                             vertex_descriptor s)
      : ordered_vertices(ordered_vertices), weight(weight),
        incoming(incoming), distance(distance),
        path_count_constraint(path_count_constraint),
        path_count_unconstraint(path_count_unconstraint), betw_decision_fct(betw_decision_fct),
        s(s)
    { }

    /**
     * Whenever an edge e = (v, w) is relaxed, the incoming edge list
     * for w is set to {(v, w)} and the shortest path count of w is set to
     * the number of paths that reach {v}.
     */
    void edge_relaxed(edge_descriptor e, const Graph& g)
    {
      vertex_descriptor v = source(e, g), w = target(e, g);
      incoming[w].clear();
      incoming[w].push_back(e);
      put(path_count_unconstraint, w, get(path_count_unconstraint, v));
      if(betw_decision_fct(s,v,w)){
        put(path_count_constraint, w, get(path_count_constraint, v));
      }
    }

    /**
     * If an edge e = (v, w) was not relaxed, it may still be the case
     * that we've found more equally-short paths, so include {(v, w)} in the
     * incoming edges of w and add all of the shortest paths to v to the
     * shortest path count of w.
     */
    void edge_not_relaxed(edge_descriptor e, const Graph& g)
    {
      typedef typename property_traits<WeightMap>::value_type weight_type;
      typedef typename property_traits<DistanceMap>::value_type distance_type;
      vertex_descriptor v = source(e, g), w = target(e, g);

      distance_type d_v = get(distance, v), d_w = get(distance, w);
      weight_type w_e = get(weight, e);
      closed_plus<distance_type> combine;
      if (d_w == combine(d_v, w_e)) {
        put(path_count_unconstraint, w, get(path_count_unconstraint, w) + get(path_count_unconstraint, v));
        incoming[w].push_back(e);
        if(betw_decision_fct(s,v,w)){
          put(path_count_constraint, w, get(path_count_constraint, w) + get(path_count_constraint, v));
        }
      }

    }

    /// Keep track of vertices as they are reached
    void examine_vertex(vertex_descriptor w, const Graph&)
    {
      ordered_vertices.push(w);
    }

  private:
    std::stack<vertex_descriptor>& ordered_vertices;
    WeightMap weight;
    IncomingMap incoming;
    DistanceMap distance;
    PathCountMap path_count_constraint;
    PathCountMap path_count_unconstraint;
    decision_function &betw_decision_fct;
    vertex_descriptor s;
  };

  /**
   * Function object that calls Dijkstra's shortest paths algorithm
   * using the Dijkstra visitor for the Brandes betweenness centrality
   * algorithm.
   */
  template<typename WeightMap, typename Graph, typename BetweennessDecisionFunction>
  struct brandes_dijkstra_shortest_paths
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    //~ typedef typename BetweennessDecisionFunction = std::function<bool(vertex_descriptor, vertex_descriptor, vertex_descriptor)>;
    brandes_dijkstra_shortest_paths(WeightMap weight_map,
               BetweennessDecisionFunction &betw_decision_fct)
      : weight_map(weight_map), betw_decision_fct(betw_decision_fct) { }

    template<typename IncomingMap, typename DistanceMap,
             typename PathCountMap, typename VertexIndexMap>
    void
    operator()(Graph& g,
               vertex_descriptor s,
               std::stack<vertex_descriptor>& ov,
               IncomingMap incoming,
               DistanceMap distance,
               PathCountMap path_count_constraint,
               PathCountMap path_count_unconstraint,
               VertexIndexMap vertex_index)
    {
      typedef brandes_dijkstra_visitor<Graph, WeightMap, IncomingMap,
                                       DistanceMap, PathCountMap> visitor_type;
      visitor_type visitor(ov, weight_map, incoming, distance,
        path_count_constraint,
        path_count_unconstraint, betw_decision_fct, s);

      boost::dijkstra_shortest_paths(g, s,
                              boost::weight_map(weight_map)
                              .vertex_index_map(vertex_index)
                              .distance_map(distance)
                              .visitor(visitor));
    }

  private:
    BetweennessDecisionFunction betw_decision_fct;
    WeightMap weight_map;
  };

  // When the edge centrality map is a dummy property map, no
  // initialization is needed.
  template<typename Iter>
  inline void
  init_centrality_map(std::pair<Iter, Iter>, dummy_property_map) { }

  // When we have a real edge centrality map, initialize all of the
  // centralities to zero.
  template<typename Iter, typename Centrality>
  void
  init_centrality_map(std::pair<Iter, Iter> keys, Centrality centrality_map)
  {
    typedef typename property_traits<Centrality>::value_type
      centrality_type;
    while (keys.first != keys.second) {
      put(centrality_map, *keys.first, centrality_type(0));
      ++keys.first;
    }
  }

  // When the edge centrality map is a dummy property map, no update
  // is performed.
  template<typename Key, typename T>
  inline void
  update_centrality(dummy_property_map, const Key&, const T&) { }

  // When we have a real edge centrality map, add the value to the map
  template<typename CentralityMap, typename Key, typename T>
  inline void
  update_centrality(CentralityMap centrality_map, Key k, const T& x)
  { put(centrality_map, k, get(centrality_map, k) + x); }

  template<typename Iter>
  inline void
  divide_centrality_by_two(std::pair<Iter, Iter>, dummy_property_map) {}

  template<typename Iter, typename CentralityMap>
  inline void
  divide_centrality_by_two(std::pair<Iter, Iter> keys,
                           CentralityMap centrality_map)
  {
    typename property_traits<CentralityMap>::value_type two(2);
    while (keys.first != keys.second) {
      put(centrality_map, *keys.first, get(centrality_map, *keys.first) / two);
      ++keys.first;
    }
  }

  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename IncomingMap, typename DistanceMap,
           typename DependencyMap, typename PathCountMap,
           typename VertexIndexMap, typename ShortestPaths>
  void
  brandes_betweenness_centrality_impl(const Graph& g,
                                      CentralityMap centrality,     // C_B
                                      EdgeCentralityMap edge_centrality_map,
                                      IncomingMap incoming, // P
                                      DistanceMap distance,         // d
                                      DependencyMap dependency,     // delta
                                      PathCountMap path_count_constraint,      // sigma
                                      PathCountMap path_count_unconstraint,      // sigma
                                      VertexIndexMap vertex_index,
                                      ShortestPaths shortest_paths)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    //~ const BglGraph<T>& g = *(gc.get_graph());
    // Initialize centrality
    init_centrality_map(vertices(g), centrality);
    init_centrality_map(edges(g), edge_centrality_map);

    std::stack<vertex_descriptor> ordered_vertices;
    vertex_iterator s, s_end;
    for (boost::tie(s, s_end) = vertices(g); s != s_end; ++s) {
      // Initialize for this iteration
      vertex_iterator w, w_end;
      for (boost::tie(w, w_end) = vertices(g); w != w_end; ++w) {
        incoming[*w].clear();
        put(path_count_constraint, *w, 0);
        put(path_count_unconstraint, *w, 0);
        put(dependency, *w, 0);
      }
      put(path_count_constraint, *s, 1);
      put(path_count_unconstraint, *s, 1);

      // Execute the shortest paths algorithm. This will be either
      // Dijkstra's algorithm or a customized breadth-first search,
      // depending on whether the graph is weighted or unweighted.
      shortest_paths(g, *s, ordered_vertices, incoming, distance,
                     path_count_constraint,
                     path_count_unconstraint, vertex_index);

      while (!ordered_vertices.empty()) {
        vertex_descriptor w = ordered_vertices.top();
        ordered_vertices.pop();

        typedef typename property_traits<IncomingMap>::value_type
          incoming_type;
        typedef typename incoming_type::iterator incoming_iterator;
        typedef typename property_traits<DependencyMap>::value_type
          dependency_type;

        for (incoming_iterator vw = incoming[w].begin();
             vw != incoming[w].end(); ++vw) {
          vertex_descriptor v = source(*vw, g);
          //~ dependency_type factor_raw = dependency_type(get(path_count_unconstraint, v))
            //~ / dependency_type(get(path_count_unconstraint, w));

          if(dependency_type(get(path_count_constraint, w)) > dependency_type(0)){
            bool ctx_fac = (dependency_type(get(path_count_constraint, v)) != dependency_type(get(path_count_unconstraint, v)))
              && (dependency_type(get(path_count_constraint, w)) != dependency_type(get(path_count_unconstraint, w)));

            //~ dependency_type factor =
              //~ ((dependency_type(get(path_count_unconstraint, v)) + dependency_type(get(path_count_constraint, v))) / dependency_type(2))
              //~ / ((dependency_type(get(path_count_unconstraint, w)) + dependency_type(get(path_count_constraint, w))) / dependency_type(2));
            dependency_type factor = (ctx_fac) ?
              (dependency_type(get(path_count_constraint, v)) / dependency_type(get(path_count_constraint, w)))
              * (dependency_type(get(path_count_unconstraint, v)) / dependency_type(get(path_count_unconstraint, w)))
              :
              dependency_type(get(path_count_unconstraint, v)) / dependency_type(get(path_count_unconstraint, w));
            //~ if(factor_raw == dependency_type(0)){
              //~ std::cout << *s << " | " << v << " -> " << w <<  ": true: " << factor_raw << " \t fac: " << factor << std::endl;
              //~ std::cout << "\t  constraint v: \t" << dependency_type(get(path_count_constraint, v)) << std::endl
                        //~ << "\tunconstraint v: \t" << dependency_type(get(path_count_unconstraint, v)) << std::endl
                        //~ << "\t  constraint w: \t" << dependency_type(get(path_count_constraint, w)) << std::endl
                        //~ << "\tunconstraint w: \t" << dependency_type(get(path_count_unconstraint, w)) << std::endl;
            //~ }
            //~ if(dependency_type(get(path_count_constraint, v)) != dependency_type(get(path_count_unconstraint, v)))
              //~ std::cout << *s << " | " << v << " -> " << w <<  ": true: " << factor_raw << " \t fac: " << factor << std::endl
                        //~ << "\t  constraint v: \t" << dependency_type(get(path_count_constraint, v)) << std::endl
                        //~ << "\tunconstraint v: \t" << dependency_type(get(path_count_unconstraint, v)) << std::endl;
            factor *= (dependency_type(1) + get(dependency, w));
            put(dependency, v, get(dependency, v) + factor);
            update_centrality(edge_centrality_map, *vw, factor);
          }
        }

        if (w != *s) {
          update_centrality(centrality, w, get(dependency, w));
        }
      }
    }

    typedef typename graph_traits<Graph>::directed_category directed_category;
    const bool is_undirected =
      is_convertible<directed_category*, undirected_tag*>::value;
    if (is_undirected) {
      divide_centrality_by_two(vertices(g), centrality);
      divide_centrality_by_two(edges(g), edge_centrality_map);
    }
  }

} } // end namespace detail::graph

template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
         typename IncomingMap, typename DistanceMap,
         typename DependencyMap, typename PathCountMap,
         typename VertexIndexMap, typename WeightMap,
         typename BetweennessDecisionFunction>
void
brandes_betweenness_centrality(const Graph& g,
                               CentralityMap centrality,     // C_B
                               EdgeCentralityMap edge_centrality_map,
                               IncomingMap incoming, // P
                               DistanceMap distance,         // d
                               DependencyMap dependency,     // delta
                               PathCountMap path_count_constraint,      // sigma
                               PathCountMap path_count_unconstraint,      // sigma
                               VertexIndexMap vertex_index,
                               WeightMap weight_map,
                               BetweennessDecisionFunction &betw_decision_fct
                               BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
{

  detail::graph::brandes_dijkstra_shortest_paths<WeightMap, const Graph, BetweennessDecisionFunction>
    shortest_paths(weight_map,betw_decision_fct);

  detail::graph::brandes_betweenness_centrality_impl(g, centrality,
                                                     edge_centrality_map,
                                                     incoming, distance,
                                                     dependency, path_count_constraint,
                                                     path_count_unconstraint,
                                                     vertex_index,
                                                     shortest_paths);
}

namespace detail { namespace graph {
  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename WeightMap, typename VertexIndexMap,
           typename BetweennessDecisionFunction>
  void
  brandes_betweenness_centrality_dispatch2(const Graph& g,
                                           CentralityMap centrality,
                                           EdgeCentralityMap edge_centrality_map,
                                           WeightMap weight_map,
                                           VertexIndexMap vertex_index,
                                           BetweennessDecisionFunction &betw_decision_fct)
  {
    typedef typename graph_traits<Graph>::degree_size_type degree_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename mpl::if_c<(is_same<CentralityMap,
                                        dummy_property_map>::value),
                                         EdgeCentralityMap,
                               CentralityMap>::type a_centrality_map;
    typedef typename property_traits<a_centrality_map>::value_type
      centrality_type;

    typename graph_traits<Graph>::vertices_size_type V = num_vertices(g);

    std::vector<std::vector<edge_descriptor> > incoming(V);
    std::vector<centrality_type> distance(V);
    std::vector<centrality_type> dependency(V);
    std::vector<degree_size_type> path_count_constraint(V);
    std::vector<degree_size_type> path_count_unconstraint(V);

    brandes_betweenness_centrality(
      g, centrality, edge_centrality_map,
      make_iterator_property_map(incoming.begin(), vertex_index),
      make_iterator_property_map(distance.begin(), vertex_index),
      make_iterator_property_map(dependency.begin(), vertex_index),
      make_iterator_property_map(path_count_constraint.begin(), vertex_index),
      make_iterator_property_map(path_count_unconstraint.begin(), vertex_index),
      vertex_index,
      weight_map,
      betw_decision_fct);
  }

  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename VertexIndexMap>
  void
  brandes_betweenness_centrality_dispatch2(const Graph& g,
                                           CentralityMap centrality,
                                           EdgeCentralityMap edge_centrality_map,
                                           VertexIndexMap vertex_index)
  {
    typedef typename graph_traits<Graph>::degree_size_type degree_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename mpl::if_c<(is_same<CentralityMap,
                                        dummy_property_map>::value),
                                         EdgeCentralityMap,
                               CentralityMap>::type a_centrality_map;
    typedef typename property_traits<a_centrality_map>::value_type
      centrality_type;

    typename graph_traits<Graph>::vertices_size_type V = num_vertices(g);

    std::vector<std::vector<edge_descriptor> > incoming(V);
    std::vector<centrality_type> distance(V);
    std::vector<centrality_type> dependency(V);
    std::vector<degree_size_type> path_count(V);

    brandes_betweenness_centrality(
      g, centrality, edge_centrality_map,
      make_iterator_property_map(incoming.begin(), vertex_index),
      make_iterator_property_map(distance.begin(), vertex_index),
      make_iterator_property_map(dependency.begin(), vertex_index),
      make_iterator_property_map(path_count.begin(), vertex_index),
      vertex_index);
  }

  template<typename Graph, typename WeightMap, typename BetweennessDecisionFunction>
  struct brandes_betweenness_centrality_dispatch1
  {
    template<typename CentralityMap,
             typename EdgeCentralityMap, typename VertexIndexMap>
    static void
    run(const Graph& g, CentralityMap centrality,
        EdgeCentralityMap edge_centrality_map, VertexIndexMap vertex_index,
        WeightMap weight_map,
        BetweennessDecisionFunction &betw_decision_fct)
    {
      brandes_betweenness_centrality_dispatch2(g, centrality, edge_centrality_map,
                                               weight_map, vertex_index,
                                               betw_decision_fct);
    }
  };

  template <typename T>
  struct is_bgl_named_params {
    BOOST_STATIC_CONSTANT(bool, value = false);
  };

  template <typename Param, typename Tag, typename Rest>
  struct is_bgl_named_params<bgl_named_params<Param, Tag, Rest> > {
    BOOST_STATIC_CONSTANT(bool, value = true);
  };

} } // end namespace detail::graph

// DocString: betweenness_ctx
/**
 * @brief Betweenness centrality with dynamic contextual constraints.
 *
 * Using this function allows obtaining betweenness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.
 *
 * The function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the betweenness calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.
 *
 * The three nodes are passed as indices allowing for access of (external) attribute and other associated information.
 *
 * Note that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.
 *
 * If the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.
 *
 * Obtaining betweenness centrality is based on Brandes' efficient algorithm. At the current stage, the implementation allows for single-core execution only.
 *
 * @param g The graph object
 * @param betw_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.
 *
 */
template<typename Graph, typename Param, typename Tag, typename Rest, typename DecisionFunction>
void
brandes_betweenness_centrality_ctx(const Graph& g,
                               const bgl_named_params<Param,Tag,Rest>& params,
                               DecisionFunction &betw_decision_fct
                               BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
{
  typedef bgl_named_params<Param,Tag,Rest> named_params;
  typedef typename get_param_type<edge_weight_t, named_params>::type ew;

  detail::graph::brandes_betweenness_centrality_dispatch1<Graph, ew, DecisionFunction>::run(
    g,
    choose_param(get_param(params, vertex_centrality),
                 dummy_property_map()),
    choose_param(get_param(params, edge_centrality),
                 dummy_property_map()),
    choose_param(get_param(params, vertex_index),
                 dummy_property_map()),
    get_param(params, edge_weight),
    betw_decision_fct);

}

// disable_if is required to work around problem with MSVC 7.1 (it seems to not
// get partial ordering getween this overload and the previous one correct)
//~ template<typename Graph, typename CentralityMap>
//~ typename disable_if<detail::graph::is_bgl_named_params<CentralityMap>,
                    //~ void>::type
//~ brandes_betweenness_centrality(const Graph& g, CentralityMap centrality
                               //~ BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
//~ {
  //~ detail::graph::brandes_betweenness_centrality_dispatch2(
    //~ g, centrality, dummy_property_map(), get(vertex_index, g)); //TODO INDIZES
//~ }

//~ template<typename Graph, typename CentralityMap, typename EdgeCentralityMap>
//~ void
//~ brandes_betweenness_centrality(const Graph& g, CentralityMap centrality,
                               //~ EdgeCentralityMap edge_centrality_map
                               //~ BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
//~ {
  //~ detail::graph::brandes_betweenness_centrality_dispatch2(
    //~ g, centrality, edge_centrality_map, get(vertex_index, g)); //TODO INDIZES
//~ }

/**
 * Converts "absolute" betweenness centrality (as computed by the
 * brandes_betweenness_centrality algorithm) in the centrality map
 * into "relative" centrality. The result is placed back into the
 * given centrality map.
 */
template<typename Graph, typename CentralityMap>
void
relative_betweenness_centrality(const Graph& g, CentralityMap centrality)
{
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename property_traits<CentralityMap>::value_type centrality_type;

  typename graph_traits<Graph>::vertices_size_type n = num_vertices(g);
  centrality_type factor = centrality_type(2)/centrality_type(n*n - 3*n + 2);
  vertex_iterator v, v_end;
  for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
    put(centrality, *v, factor * get(centrality, *v));
  }
}

} // end namespace boost

#endif // BOOST_GRAPH_BRANDES_BETWEENNESS_CENTRALITY_HPP


//|
//|    Copyright (C) 2019 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
//|    Authors:  Konstantinos Chatzilygeroudis (maintainer)
//|              Bernardo Fichera
//|              Walid Amanhoud
//|    email:    costashatz@gmail.com
//|              bernardo.fichera@epfl.ch
//|              walid.amanhoud@epfl.ch
//|    Other contributors:
//|              Yoan Mollard (yoan@aubrune.eu)
//|    website:  lasa.epfl.ch
//|
//|    This file is part of iiwa_ros.
//|
//|    iiwa_ros is free software: you can redistribute it and/or modify
//|    it under the terms of the GNU General Public License as published by
//|    the Free Software Foundation, either version 3 of the License, or
//|    (at your option) any later version.
//|
//|    iiwa_ros is distributed in the hope that it will be useful,
//|    but WITHOUT ANY WARRANTY; without even the implied warranty of
//|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//|    GNU General Public License for more details.
//|
#include <Eigen/Dense>

#include <pluginlib/class_list_macros.hpp>

#include <iiwa_control/custom_effort_controller.hpp>

#include <Corrade/Containers/PointerStl.h>

#include <robot_controllers/CascadeController.hpp>
#include <robot_controllers/SumController.hpp>

namespace iiwa_control {
    template <class MatT>
    Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> pseudo_inverse(const MatT& mat, typename MatT::Scalar tolerance = typename MatT::Scalar{1e-4}) // choose appropriately
    {
        typedef typename MatT::Scalar Scalar;
        auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        const auto& singularValues = svd.singularValues();
        Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> singularValuesInv(mat.cols(), mat.rows());
        singularValuesInv.setZero();
        for (unsigned int i = 0; i < singularValues.size(); ++i) {
            if (singularValues(i) > tolerance) {
                singularValuesInv(i, i) = Scalar{1} / singularValues(i);
            }
            else {
                singularValuesInv(i, i) = Scalar{0};
            }
        }
        return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
    }

    std::vector<std::vector<std::string>> get_types(const std::string& input, const std::string& output)
    {
        std::vector<std::vector<std::string>> result(2);

        // Input
        std::string s = input;
        std::string delimiter = "|";

        size_t pos = 0;
        std::string token;
        while ((pos = s.find(delimiter)) != std::string::npos) {
            token = s.substr(0, pos);
            result[0].push_back(token);
            s.erase(0, pos + delimiter.length());
        }
        // add the last one to the list
        result[0].push_back(s);

        // Output
        s = output;
        delimiter = "|";

        pos = 0;
        token = "";
        while ((pos = s.find(delimiter)) != std::string::npos) {
            token = s.substr(0, pos);
            result[1].push_back(token);
            s.erase(0, pos + delimiter.length());
        }
        // add the last one to the list
        result[1].push_back(s);

        return result;
    }

    void set_types(CustomEffortController::ControllerPtr& ctrl, const std::vector<std::string>& input, const std::vector<std::string>& output)
    {
        robot_controllers::IOTypes input_type, output_type;
        for (auto& s : input) {
            if (s == "Position")
                input_type = input_type | robot_controllers::IOType::Position;
            else if (s == "Orientation")
                input_type = input_type | robot_controllers::IOType::Orientation;
            else if (s == "Velocity")
                input_type = input_type | robot_controllers::IOType::Velocity;
            else if (s == "AngularVelocity")
                input_type = input_type | robot_controllers::IOType::AngularVelocity;
            else if (s == "Acceleration")
                input_type = input_type | robot_controllers::IOType::Acceleration;
            else if (s == "AngularAcceleration")
                input_type = input_type | robot_controllers::IOType::AngularAcceleration;
            else if (s == "Force")
                input_type = input_type | robot_controllers::IOType::Force;
            else if (s == "Torque")
                input_type = input_type | robot_controllers::IOType::Torque;
        }

        for (auto& s : output) {
            if (s == "Position")
                output_type = output_type | robot_controllers::IOType::Position;
            else if (s == "Orientation")
                output_type = output_type | robot_controllers::IOType::Orientation;
            else if (s == "Velocity")
                output_type = output_type | robot_controllers::IOType::Velocity;
            else if (s == "AngularVelocity")
                output_type = output_type | robot_controllers::IOType::AngularVelocity;
            else if (s == "Acceleration")
                output_type = output_type | robot_controllers::IOType::Acceleration;
            else if (s == "AngularAcceleration")
                output_type = output_type | robot_controllers::IOType::AngularAcceleration;
            else if (s == "Force")
                output_type = output_type | robot_controllers::IOType::Force;
            else if (s == "Torque")
                output_type = output_type | robot_controllers::IOType::Torque;
        }

        ctrl->SetIOTypes(input_type, output_type);
    }

    void set_input_space(CustomEffortController::ControllerPtr& ctrl, size_t space_dim)
    {
        size_t input_dim = 0;

        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Position) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            input_dim += 3; // This is fixed to 3D
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Force) {
            input_dim += space_dim;
        }
        if (ctrl->GetInput().GetType() & robot_controllers::IOType::Torque) {
            input_dim += 3; // This is fixed to 3D
        }

        robot_controllers::RobotParams params = ctrl->GetParams();
        params.input_dim_ = input_dim;
        // TO-DO: We assume same input/output
        params.output_dim_ = input_dim;

        ctrl->SetParams(params);
    }

    CustomEffortController::CustomEffortController() {}

    CustomEffortController::~CustomEffortController() { sub_command_.shutdown(); }

    bool CustomEffortController::init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n)
    {
        // List of controlled joints
        std::string param_name = "joints";
        if (!n.getParam(param_name, joint_names_)) {
            ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
            return false;
        }
        n_joints_ = joint_names_.size();

        if (n_joints_ == 0) {
            ROS_ERROR_STREAM("List of joint names is empty.");
            return false;
        }

        // Get URDF
        urdf::Model urdf;
        if (!urdf.initParam("robot_description")) {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }

        // Get basic parameters
        n.param<std::string>("params/space", operation_space_, "joint"); // Default operation space is task-space

        // Check the operational space
        if (operation_space_ == "task") {
            space_dim_ = 3;

            // Get the URDF XML from the parameter server
            std::string urdf_string, full_param;
            std::string robot_description = "robot_description";
            std::string end_effector;

            // gets the location of the robot description on the parameter server
            if (!n.searchParam(robot_description, full_param)) {
                ROS_ERROR("Could not find parameter %s on parameter server", robot_description.c_str());
                return false;
            }

            // search and wait for robot_description on param server
            while (urdf_string.empty()) {
                ROS_INFO_ONCE_NAMED("CustomEffortController", "CustomEffortController is waiting for model"
                                                              " URDF in parameter [%s] on the ROS param server.",
                    robot_description.c_str());

                n.getParam(full_param, urdf_string);

                usleep(100000);
            }
            ROS_INFO_STREAM_NAMED("CustomEffortController", "Received urdf from param server, parsing...");

            // Get the end-effector
            n.param<std::string>("params/end_effector", end_effector, "iiwa_link_ee");

            // Initialize iiwa tools
            tools_.init_rbdyn(urdf_string, end_effector);
        }
        else
            space_dim_ = n_joints_;

        // Read Controllers from Params
        std::map<std::string, ControllerPtr> controllers;
        std::vector<std::string> ctrl_names;

        XmlRpc::XmlRpcValue symbols;

        n.getParam("controllers", symbols);

        assert(symbols.getType() == XmlRpc::XmlRpcValue::TypeStruct);
        for (XmlRpc::XmlRpcValue::iterator i = symbols.begin(); i != symbols.end(); ++i) {
            // ROS_WARN_STREAM(i->first << ": " << i->second.getType());
            std::string name = i->first;
            std::string type, input, output;
            std::vector<double> param_values;
            n.getParam("controllers/" + name + "/type", type);
            n.getParam("controllers/" + name + "/params", param_values);
            n.getParam("controllers/" + name + "/input", input);
            n.getParam("controllers/" + name + "/output", output);

            if (type.size() == 0) {
                ROS_WARN_STREAM("Could not find type of controller '" << name << "'. Skipping this controller!");
                continue;
            }

            auto ctrl = manager_.loadAndInstantiate(type);

            if (ctrl) {
                robot_controllers::RobotParams params;
                params.input_dim_ = space_dim_;
                params.output_dim_ = space_dim_;

                params.time_step_ = 0.01; // TO-DO: Get this from controller manager or yaml

                params.values_ = param_values;

                ctrl->SetParams(params);

                // TO-DO: Maybe separate input and output
                if (input.size() > 0 && output.size() > 0) {
                    std::vector<std::vector<std::string>> tt = get_types(input, output);
                    if (tt.size() == 2)
                        set_types(ctrl, tt[0], tt[1]);
                }

                set_input_space(ctrl, space_dim_);

                controllers[name] = std::move(ctrl);
                ctrl_names.push_back(name);
            }
        }

        XmlRpc::XmlRpcValue symbols_structure;
        n.getParam("structure", symbols_structure);

        if (symbols_structure.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
            for (XmlRpc::XmlRpcValue::iterator i = symbols_structure.begin(); i != symbols_structure.end(); ++i) {
                // ROS_WARN_STREAM(i->first << ": " << i->second.getType());
                std::string name = i->first;
                std::vector<std::string> sub;
                n.getParam("structure/" + name, sub);
                bool is_sum = false;
                if (name.find("Add") == 0) {
                    controllers[name] = ControllerPtr(new robot_controllers::SumController);
                    is_sum = true;
                }
                else if (name.find("Cascade") == 0) {
                    controllers[name] = ControllerPtr(new robot_controllers::CascadeController);
                }
                else {
                    ROS_WARN_STREAM("Cannot identify the type of the controller by the name: '" << name << "'. Ignoring!");
                    continue;
                }
                // std::cout << sub.size() << std::endl;
                for (size_t k = 0; k < sub.size(); k++) {
                    // ROS_WARN_STREAM("    " << sub[k]);
                    if (is_sum)
                        static_cast<robot_controllers::SumController*>(controllers[name].get())->AddController(std::move(controllers[sub[k]]));
                    else
                        static_cast<robot_controllers::CascadeController*>(controllers[name].get())->AddController(std::move(controllers[sub[k]]));
                    ctrl_names.erase(std::remove(ctrl_names.begin(), ctrl_names.end(), sub[k]), ctrl_names.end());
                    controllers.erase(sub[k]);
                }

                // Initialize parameters
                robot_controllers::RobotParams params;
                params.input_dim_ = space_dim_;
                params.output_dim_ = space_dim_;

                params.time_step_ = 0.01; // TO-DO: Get this from controller manager or yaml
                controllers[name]->SetParams(params);

                set_input_space(controllers[name], space_dim_);

                ctrl_names.push_back(name);
            }
        }

        null_space_control_ = false;
        if (operation_space_ == "task") {
            std::vector<double> joints;
            n.getParam("params/null_space/joints", joints);
            null_space_control_ = (joints.size() == n_joints_);

            if (null_space_control_) {
                null_space_joint_config_ = Eigen::VectorXd::Map(joints.data(), joints.size());

                null_space_Kp_ = 20.;
                null_space_Kd_ = 0.1;
                null_space_max_torque_ = 10.;

                n.getParam("params/null_space/Kp", null_space_Kp_);
                n.getParam("params/null_space/Kp", null_space_Kd_);
                n.getParam("params/null_space/max_torque", null_space_max_torque_);
            }
        }

        unsigned int ctrl_size = ctrl_names.size();

        if (ctrl_size == 0) {
            ROS_ERROR_STREAM("Could not load specified controllers! Exiting..!");
            return false;
        }

        if (ctrl_size == 1) {
            controller_ = std::move(controllers[ctrl_names[0]]);

            set_input_space(controller_, space_dim_);
        }
        else {
            controller_.reset(new robot_controllers::SumController);
            for (unsigned int i = 0; i < ctrl_size; i++) {
                static_cast<robot_controllers::SumController*>(controller_.get())->AddController(std::move(controllers[ctrl_names[i]]));
            }

            set_input_space(controller_, space_dim_);
        }

        // Initialize the controller(s)
        if (!controller_->Init()) {
            ROS_ERROR("Controllers could not be initialized! Exiting!");
            return false;
        }

        for (unsigned int i = 0; i < n_joints_; i++) {
            try {
                joints_.push_back(hw->getHandle(joint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException& e) {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

            urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
            if (!joint_urdf) {
                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                return false;
            }
            joint_urdfs_.push_back(joint_urdf);
        }

        // Get controller command size
        cmd_dim_ = 0;

        if (controller_->GetInput().GetType() & robot_controllers::IOType::Position) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            cmd_dim_ += 3; // This is fixed to 3D
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Force) {
            cmd_dim_ += space_dim_;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Torque) {
            cmd_dim_ += 3; // This is fixed to 3D
        }

        std::vector<double> init_cmd(cmd_dim_, 0.0);
        has_orientation_ = false;
        if (operation_space_ == "task") {
            has_orientation_ = ((controller_->GetInput().GetType() & robot_controllers::IOType::Orientation)) ? true : false;
            bool has_position = ((controller_->GetInput().GetType() & robot_controllers::IOType::Position)) ? true : false;
            if (has_position || has_orientation_) {
                // if task space, we need to alter the initial command
                iiwa_tools::RobotState robot_state;
                robot_state.position.resize(n_joints_);
                robot_state.velocity.resize(n_joints_);

                for (size_t i = 0; i < n_joints_; i++) {
                    robot_state.position[i] = joints_[i].getPosition();
                    robot_state.velocity[i] = joints_[i].getVelocity();
                }

                auto ee_state = tools_.perform_fk(robot_state);
                Eigen::AngleAxisd aa(ee_state.orientation);
                Eigen::VectorXd o = aa.axis() * aa.angle();
                Eigen::VectorXd p = ee_state.translation;

                size_t offset = 0;
                if (has_orientation_)
                    offset = 3;

                for (size_t i = 0; i < 3; i++) {
                    if (has_orientation_)
                        init_cmd[i] = o(i);
                    if (has_position)
                        init_cmd[i + offset] = p(i);
                }
            }
        }

        ROS_INFO_STREAM("Initial command: " << Eigen::VectorXd::Map(init_cmd.data(), init_cmd.size()).transpose());

        commands_buffer_.writeFromNonRT(init_cmd);

        sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &CustomEffortController::commandCB, this);

        return true;
    }

    void CustomEffortController::update(const ros::Time& time, const ros::Duration& period)
    {
        std::vector<double>& commands = *commands_buffer_.readFromRT();

        Eigen::MatrixXd jac(6, n_joints_);
        Eigen::MatrixXd jac_deriv(6, n_joints_);
        Eigen::MatrixXd jac_t_pinv(n_joints_, 6);
        Eigen::VectorXd eef(6);

        if (operation_space_ == "task") {
            iiwa_tools::RobotState robot_state;
            robot_state.position.resize(n_joints_);
            robot_state.velocity.resize(n_joints_);

            for (size_t i = 0; i < n_joints_; i++) {
                robot_state.position[i] = joints_[i].getPosition();
                robot_state.velocity[i] = joints_[i].getVelocity();
            }

            std::tie(jac, jac_deriv) = tools_.jacobians(robot_state);
            jac_t_pinv = pseudo_inverse(Eigen::MatrixXd(jac.transpose()));
            auto ee_state = tools_.perform_fk(robot_state);
            Eigen::AngleAxisd aa(ee_state.orientation);
            eef.head(3) = aa.axis() * aa.angle();
            eef.tail(3) = ee_state.translation;
        }

        Eigen::VectorXd cmd(n_joints_);

        cmd = Eigen::VectorXd::Map(commands.data(), commands.size());

        robot_controllers::RobotState curr_state, robot_state;
        curr_state.position_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.velocity_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.acceleration_ = Eigen::VectorXd::Zero(n_joints_);
        curr_state.force_ = Eigen::VectorXd::Zero(n_joints_);

        for (unsigned int i = 0; i < n_joints_; i++) {
            curr_state.position_(i) = joints_[i].getPosition();
            curr_state.velocity_(i) = joints_[i].getVelocity();
            // curr_state.acceleration_(i) = joints_[i].getAcceleration();
            // TO-DO: Fill acceleration
            curr_state.force_(i) = joints_[i].getEffort();
        }

        if (operation_space_ == "task") {
            if (null_space_control_)
                robot_state = curr_state;

            Eigen::VectorXd pos = eef;
            Eigen::VectorXd vel = jac * curr_state.velocity_;
            Eigen::VectorXd acc = jac * curr_state.acceleration_ + jac_deriv * curr_state.velocity_;
            Eigen::VectorXd f = jac_t_pinv * curr_state.force_; // TO-DO: This is not perfect, but should be enough

            curr_state.position_ = pos.tail(3);
            curr_state.velocity_ = vel.tail(3);
            curr_state.acceleration_ = acc.tail(3);
            curr_state.force_ = f.tail(3);

            if (has_orientation_) {
                curr_state.orientation_ = pos.head(3);
                curr_state.angular_velocity_ = vel.head(3);
                curr_state.angular_acceleration_ = acc.head(3);
                curr_state.torque_ = f.head(3);
            }
        }

        // Update desired state in controller
        robot_controllers::RobotState desired_state;
        unsigned int size = curr_state.position_.size();
        unsigned int index = 0;
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Orientation) {
            desired_state.orientation_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Position) {
            desired_state.position_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularVelocity) {
            desired_state.angular_velocity_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Velocity) {
            desired_state.velocity_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::AngularAcceleration) {
            desired_state.angular_acceleration_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Acceleration) {
            desired_state.acceleration_ = cmd.segment(index, size);
            index += size;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Torque) {
            desired_state.torque_ = cmd.segment(index, 3);
            index += 3;
        }
        if (controller_->GetInput().GetType() & robot_controllers::IOType::Force) {
            desired_state.force_ = cmd.segment(index, size);
            // index += size;
        }

        controller_->SetInput(desired_state);

        // Update control torques given current velocity
        controller_->Update(curr_state);

        Eigen::VectorXd output; // = Eigen::VectorXd::Zero(space_dim_);
        if (operation_space_ == "task") {
            output = Eigen::VectorXd::Zero(2 * space_dim_);
            if (controller_->GetOutput().GetType() & robot_controllers::IOType::Force)
                output.tail(3) = controller_->GetOutput().desired_.force_;
            if (controller_->GetOutput().GetType() & robot_controllers::IOType::Torque)
                output.head(3) = controller_->GetOutput().desired_.torque_;
        }
        else // regular controller
            output = controller_->GetOutput().desired_.force_;

        if (operation_space_ == "task") {
            // output.head(3) = Eigen::VectorXd::Zero(3);
            output = jac.transpose() * output;

            // Add null-space signal if wanted
            if (null_space_control_) {
                Eigen::VectorXd null_space_signal = null_space_Kp_ * (null_space_joint_config_ - robot_state.position_) - null_space_Kd_ * robot_state.velocity_;
                Eigen::VectorXd null_space_force = (Eigen::MatrixXd::Identity(n_joints_, n_joints_) - jac.transpose() * jac_t_pinv) * null_space_signal;
                for (int i = 0; i < null_space_force.size(); i++) {
                    if (null_space_force(i) > null_space_max_torque_)
                        null_space_force(i) = null_space_max_torque_;
                    else if (null_space_force(i) < -null_space_max_torque_)
                        null_space_force(i) = -null_space_max_torque_;
                }
                output = output + null_space_force;
            }
        }

        // ROS_INFO_STREAM("Effort: " << output.transpose());

        std::vector<double> commanded_effort(n_joints_, 0.);

        Eigen::VectorXd::Map(commanded_effort.data(), commanded_effort.size()) = output;

        for (unsigned int i = 0; i < n_joints_; i++) {
            enforceJointLimits(commanded_effort[i], i);
            joints_[i].setCommand(commanded_effort[i]);
        }
    }

    void CustomEffortController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
    {
        if (msg->data.size() != cmd_dim_) {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") is not correct! Not executing!");
            return;
        }

        commands_buffer_.writeFromNonRT(msg->data);
    }

    void CustomEffortController::enforceJointLimits(double& command, unsigned int index)
    {
        // Check that this joint has applicable limits
        if (command > joint_urdfs_[index]->limits->effort) // above upper limit
        {
            command = joint_urdfs_[index]->limits->effort;
        }
        else if (command < -joint_urdfs_[index]->limits->effort) // below lower limit
        {
            command = -joint_urdfs_[index]->limits->effort;
        }
    }
} // namespace iiwa_control

PLUGINLIB_EXPORT_CLASS(iiwa_control::CustomEffortController, controller_interface::ControllerBase)


#pragma once
#include <model.hpp>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <utils.hpp>
#include <exception>
#include <vector>
#include <iostream>
#include <fstream>

namespace dummyml
{

class gaussian_process : public Model
{
private:
    EigenMatrix _x;
    EigenMatrix _y;
    EigenMatrix _C_inv;
    double _alpha;
    std::unique_ptr<kernel> _kernel;
public:
    struct MetaData{
        Eigen::Index _n;
        Eigen::Index _f;
        kernel::type _t;
        double _a;
        Eigen::Index check_sum;
        MetaData() = default;
        MetaData(const gaussian_process& model):
            _n(model._x.rows()),
            _f(model._x.cols()),
            _t(model._kernel->_T),
            _a(model._alpha),
            check_sum(
                model._x.rows() ^
                model._x.cols() ^
                model._kernel->_T
            ){}
        bool is_check_sum_correct(){
            return (_n ^ _f ^ _t) == check_sum;
        }
    };
    gaussian_process(const char* filename){
        load(filename);
    }
    gaussian_process(
        double alpha = 0.2,
        kernel::type k_type = kernel::type::LinearKernel
    ): _alpha(alpha), _kernel(get_kernel(k_type)){}
    gaussian_process(
        nparray_d x,
        nparray_d y,
        double alpha = 0.2,
        kernel::type k_type = kernel::type::LinearKernel
    ): _alpha(alpha), _kernel(get_kernel(k_type)){
        fit(x, y);
    }
    double run_kernel(double x, double y){
        return (*_kernel)(x,y);
    }
    void load(const char* file_name){
        std::fstream fin_bin(file_name,std::ios_base::in | std::ios_base::binary);
        if(fin_bin.fail()){
            throw std::runtime_error(
                "[ERROR] gaussian_process load: failed to load model."
            );
        }
        MetaData meta;
        fin_bin.read(
            dummy_cast<char*,MetaData*>(&meta),
            sizeof(MetaData)
        );
        if(!meta.is_check_sum_correct()){
            throw std::runtime_error(
                "[ERROR] gaussian_process load: MetaData mismatch."
            );
        }
        _alpha = meta._a;
        _kernel = get_kernel(meta._t);
        _x = EigenMatrix(meta._n, meta._f);
        _y = EigenMatrix(meta._n, 1);
        _C_inv = EigenMatrix(meta._n, meta._n);
        fin_bin.read(
            dummy_cast<char*,double*>(_x.data()),
            sizeof(double) * _x.size()
        ).read(
            dummy_cast<char*,double*>(_y.data()),
            sizeof(double) * _y.size()
        ).read(
            dummy_cast<char*,double*>(_C_inv.data()),
            sizeof(double) * _C_inv.size()
        );
        return;
    }
    void save(const char* file_name){
        std::fstream fout_bin(file_name,std::ios_base::out | std::ios_base::binary);
        if(fout_bin.fail()){
            throw std::runtime_error(
                "[ERROR] gaussian_process save: failed to save model."
            );
        }
        MetaData meta(*this);
        fout_bin.write(
            dummy_cast<char*,MetaData*>(&meta),
            sizeof(MetaData)
        ).write(
            dummy_cast<char*,double*>(_x.data()),
            sizeof(double) * _x.size()
        ).write(
            dummy_cast<char*,double*>(_y.data()),
            sizeof(double) * _y.size()
        ).write(
            dummy_cast<char*,double*>(_C_inv.data()),
            sizeof(double) * _C_inv.size()
        );
        return;
    }
    void fit(nparray_d x, nparray_d y){
        auto x_buf_info = x.request();
        auto y_buf_info = y.request();
        if(x_buf_info.shape[0] != y_buf_info.shape[0]){
            throw std::length_error(
                "[ERROR] gaussian_process fit: data & label counts mismatch."
            );
        }
        size_t dataset_size = x_buf_info.shape[0];
        size_t feature_size = x_buf_info.shape[1];
        double* x_ptr = (double*)x_buf_info.ptr;
        double* y_ptr = (double*)y_buf_info.ptr;

        // copy x,y to _x,_y
        _x.resize(dataset_size, feature_size);
        _y.resize(dataset_size, 1);
        _C_inv.resize(dataset_size, dataset_size);
        memcpy(_x.data(), x_ptr, dataset_size * feature_size * sizeof(double));
        memcpy(_y.data(), y_ptr, dataset_size                * sizeof(double));

        // calculate _C_inv
        for(size_t row = 0;row < dataset_size; ++row)
            for(size_t col = row;col < dataset_size; ++col)
                _C_inv(col, row) = _C_inv(row, col) = (*_kernel)(_x.row(row), _x.row(col));
        for(size_t diag = 0;diag < dataset_size; ++diag)
            _C_inv(diag ,diag) += _alpha;
        _C_inv = _C_inv.inverse();
        return;
    }
    nparray_d operator()(nparray_d x){
        auto x_buf_info = x.request();
        size_t dataset_size = _x.rows();
        size_t feature_size = _x.cols();
        if(x_buf_info.shape[0] != feature_size){
            throw std::length_error(
                "[ERROR] gaussian_process operator(): x size & feature size mismatch."
            );
        }
        EigenVector x_vec(feature_size);
        EigenMatrix kt_vec(1, dataset_size);
        memcpy(x_vec.data(), x_buf_info.ptr, _x.cols()*sizeof(double));

        for(size_t i = 0;i < dataset_size;++i)
            kt_vec(0, i) = (*_kernel)(_x.row(i), x_vec);
        EigenMatrix ktC_inv = kt_vec * _C_inv;
        nparray_d result(2);
        double* result_ptr = (double*)result.request().ptr;
        result_ptr[0] = (ktC_inv * _y)(0);
        result_ptr[1] =
            ((*_kernel)(x_vec, x_vec) + _alpha) -  // c
            (ktC_inv * kt_vec.transpose())(0);// kt * C^-1 * k
        return result;
    }
    void set_alpha(double alpha){
        _alpha = alpha;
    }
};

} // namespace dummyml

void export_gaussian_process(py::module_ &);


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename RadiusType,
    typename CalculationType = void
>
class vincenty
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef RadiusType radius_type;

    inline vincenty()
    {}

    explicit inline vincenty(geometry::detail::ellipsoid<RadiusType> const& e)
        : m_ellipsoid(e)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return calculate<typename calculation_type<Point1, Point2>::type>
            (
                get_as_radian<0>(point1), get_as_radian<1>(point1),
                get_as_radian<0>(point2), get_as_radian<1>(point2)
            );
    }

    inline geometry::detail::ellipsoid<RadiusType> ellipsoid() const
    {
        return m_ellipsoid;
    }

    inline RadiusType radius() const
    {
        // For now return the major axis. It is used in distance_cross_track, from point-to-line
        return m_ellipsoid.a();
    }

private :
    geometry::detail::ellipsoid<RadiusType> m_ellipsoid;

    template <typename CT, typename T>
    inline CT calculate(T const& lon1,
                T const& lat1,
                T const& lon2,
                T const& lat2) const
    {
        CT const c2 = 2;
        CT const pi = geometry::math::pi<CT>();
        CT const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        CT L = lon2 - lon1;
        CT lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return CT(0);
        }

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        CT const c1 = 1;
        CT const one_min_f = c1 - m_ellipsoid.f();

        CT const U1 = atan(one_min_f * tan(lat1)); // above (1)
        CT const U2 = atan(one_min_f * tan(lat2)); // above (1)

        CT const cos_U1 = cos(U1);
        CT const cos_U2 = cos(U2);
        CT const sin_U1 = sin(U1);
        CT const sin_U2 = sin(U2);

        // alpha: azimuth of the geodesic at the equator
        CT cos2_alpha;
        CT sin_alpha;

        // sigma: angular distance p1,p2 on the sphere
        // sigma1: angular distance on the sphere from the equator to p1
        // sigma_m: angular distance on the sphere from the equator to the midpoint of the line
        CT sigma;
        CT sin_sigma;
        CT cos2_sigma_m;

        CT previous_lambda;

        CT const c3 = 3;
        CT const c4 = 4;
        CT const c6 = 6;
        CT const c16 = 16;

        CT const c_e_12 = 1e-12;

        do
        {
            previous_lambda = lambda; // (13)
            CT sin_lambda = sin(lambda);
            CT cos_lambda = cos(lambda);
            sin_sigma = sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            CT cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos2_sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)

            CT C = m_ellipsoid.f()/c16 * cos2_alpha * (c4 + m_ellipsoid.f() * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * m_ellipsoid.f() * sin_alpha *
                (sigma + C * sin_sigma * ( cos2_sigma_m + C * cos_sigma * (-c1 + c2 * math::sqr(cos2_sigma_m)))); // (11)

        } while (geometry::math::abs(previous_lambda - lambda) > c_e_12
                && geometry::math::abs(lambda) < pi);

        CT sqr_u = cos2_alpha * (math::sqr(m_ellipsoid.a()) - math::sqr(m_ellipsoid.b())) / math::sqr(m_ellipsoid.b()); // above (1)

        // Oops getting hard here
        // (again, problem is that ttmath cannot divide by doubles, which is OK)
        CT const c47 = 47;
        CT const c74 = 74;
        CT const c128 = 128;
        CT const c256 = 256;
        CT const c175 = 175;
        CT const c320 = 320;
        CT const c768 = 768;
        CT const c1024 = 1024;
        CT const c4096 = 4096;
        CT const c16384 = 16384;

        CT A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
        CT B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
        CT delta_sigma = B * sin_sigma * ( cos2_sigma_m + (B/c4) * (cos(sigma)* (-c1 + c2 * cos2_sigma_m)
                - (B/c6) * cos2_sigma_m * (-c3 + c4 * math::sqr(sin_sigma)) * (-c3 + c4 * cos2_sigma_m))); // (6)

        return m_ellipsoid.b() * A * (sigma - delta_sigma); // (19)
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename RadiusType, typename CalculationType>
struct tag<vincenty<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<RadiusType, CalculationType>, P1, P2>
    : vincenty<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<vincenty<RadiusType, CalculationType> >
{
    typedef vincenty<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<vincenty<RadiusType, CalculationType> >
{
    static inline vincenty<RadiusType, CalculationType> apply(vincenty<RadiusType, CalculationType> const& input)
    {
        return input;
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<RadiusType, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<RadiusType, CalculationType>, P1, P2>::type
        apply(vincenty<RadiusType, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_similarity_transformation_2d_3d.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <limits>
#include <memory>
#include <vector>

#include "theia/sfm/camera/camera.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/feature.h"
#include "theia/sfm/similarity_transformation.h"
#include "theia/sfm/transformation/gdls_similarity_transform.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

inline void TransformCamera(const SimilarityTransformation& sim_transform,
                            Camera* camera) {
  const Eigen::Vector3d old_position = camera->GetPosition();
  const Eigen::Vector3d new_position =
      sim_transform.scale * sim_transform.rotation *
      old_position + sim_transform.translation;
  camera->SetPosition(new_position);

  const Eigen::Matrix3d old_orientation =
      camera->GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d new_orientation =
      old_orientation * sim_transform.rotation.transpose();
  camera->SetOrientationFromRotationMatrix(new_orientation);
}

// An estimator for computing the similarity transformation from 4 2D-3D
// correspondences using the gDLS algorithm to minimize reprojection error.
class GdlsSimilarityTransformationEstimator
    : public Estimator<CameraAndFeatureCorrespondence2D3D,
                       SimilarityTransformation> {
 public:
  GdlsSimilarityTransformationEstimator() {}

  // 3 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<CameraAndFeatureCorrespondence2D3D>& correspondences,
      std::vector<SimilarityTransformation>* similarity_transformations) const {
    std::vector<Eigen::Vector3d> ray_origins(4), ray_directions(4),
        world_points(4);
    for (int i = 0; i < 4; i++) {
      ray_origins[i] = correspondences[i].camera.GetPosition();
      ray_directions[i] = correspondences[i].camera.PixelToUnitDepthRay(
          correspondences[i].observation).normalized();
      world_points[i] = correspondences[i].point3d.hnormalized();
    }

    // Compute the similarity transformation. Note that this function computes
    // R, t, and s such that:
    //
    //   s * c_i + alpha_i * x_i = R * X_i + t
    //
    // where c_i is the camera position, alpha_i is the depth of the feature,
    // x_i is the unit-norm feature observation, and X_i is the 3D point.
    std::vector<Eigen::Quaterniond> rotations;
    std::vector<Eigen::Vector3d> translations;
    std::vector<double> scales;
    GdlsSimilarityTransform(ray_origins,
                            ray_directions,
                            world_points,
                            &rotations,
                            &translations,
                            &scales);

    // Aggregate the solutions, modifying the output so that R, t, s are of the
    // more useful form of:
    //
    //   s * R * (c_i + alpha_i * x_i) + t = X_i
    //
    // which transforms only the camera coordinate system so that it is aligned
    // with the 3D points.
    for (int i = 0; i < rotations.size(); i++) {
      SimilarityTransformation similarity_transformation;
      similarity_transformation.rotation =
          rotations[i].toRotationMatrix().transpose();
      similarity_transformation.translation =
          similarity_transformation.rotation * -translations[i];
      similarity_transformation.scale = scales[i];
      similarity_transformations->emplace_back(similarity_transformation);
    }
    return similarity_transformations->size() > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(
      const CameraAndFeatureCorrespondence2D3D& correspondence,
      const SimilarityTransformation& similarity_transformation) const {
    // Apply the similarity transformation to the camera.
    Camera transformed_camera = correspondence.camera;
    TransformCamera(similarity_transformation, &transformed_camera);

    Eigen::Vector2d reprojection;
    // If the point is reprojected behind the camera, return the maximum
    // possible error.
    if (transformed_camera.ProjectPoint(correspondence.point3d, &reprojection) <
        0) {
      return std::numeric_limits<double>::max();
    }

    // Return the squared reprojection error.
    return (correspondence.observation - reprojection).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(GdlsSimilarityTransformationEstimator);
};

}  // namespace

bool EstimateSimilarityTransformation2D3D(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<CameraAndFeatureCorrespondence2D3D>& correspondences,
    SimilarityTransformation* similarity_transformation,
    RansacSummary* ransac_summary) {
  GdlsSimilarityTransformationEstimator similarity_transformation_estimator;
  std::unique_ptr <
      SampleConsensusEstimator<GdlsSimilarityTransformationEstimator> > ransac =
      CreateAndInitializeRansacVariant(ransac_type,
                                       ransac_params,
                                       similarity_transformation_estimator);
  // Estimate the absolute pose.
  return ransac->Estimate(correspondences,
                          similarity_transformation,
                          ransac_summary);
}

}  // namespace theia


#include <cmath>
#include <vector>
#include <string>
#include "multi_lidar_calib/common.h"
#include "multi_lidar_calib/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/passthrough.h>    ///直通滤波相关
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

 #include <ceres/ceres.h>
 #include "lidarFactor.hpp"

#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include<vector>

#include<mutex>

#define PI 3.1415926

using std::atan2;
using std::cos;
using std::sin;

double lidar_z_height = -1;

ros::Publisher  pubFittedPlane;

double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; // 激光雷达间相对位姿关系

Eigen::Map<Eigen::Quaterniond> q_to_be_optimized(parameters);
Eigen::Map<Eigen::Vector3d> t_to_be_optimized(parameters + 4);

void planeFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

}

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

    ros::init(argc, argv, "multi_lidar_calibration");
	ros::NodeHandle nh;
    pubFittedPlane = nh.advertise<sensor_msgs::PointCloud2>("/fitted_plane", 100);

    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::LocalParameterization *q_parameterization =
                new ceres::EigenQuaternionParameterization();
            ceres::Problem::Options problem_options;

            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters, 4, q_parameterization);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<36; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = "/media/mjy/Samsung_T5/linux/DX/data/0812forcalib/pcd_select/";
                std::string filename_pt1 = "/media/mjy/Samsung_T5/linux/DX/data/0812forcalib/pcd_select/";
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                planeFitting(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                planeFitting(cloud1,coefficients1);

                // 两平面法线的夹角作为一个损失值
                Eigen::Vector3d norm0(coefficients0->values[0], coefficients0->values[1], coefficients0->values[2]);
                Eigen::Vector3d norm1(coefficients1->values[0], coefficients1->values[1], coefficients1->values[2]);
                ceres::CostFunction *cost_function;
                cost_function = LidarNormFactor::Create(norm0, norm1);
                problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }

    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
    std::cout<<q_to_be_optimized.matrix()<<std::endl;

    return 0;
}


#include "Tillotson.hpp"
#include <assert.h>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/minima.hpp>
#include <iostream>
#include "../../misc/universal_error.hpp"
#include "../../misc/utils.hpp"

Tillotson::Tillotson(double a, double b, double A, double B, double rho0, double E0, double EIV, double ECV, double
	alpha, double beta, bool negative_pressure,size_t e_index) :
	a_(a), b_(b), A_(A), B_(B), rho0_(rho0), E0_(E0), EIV_(EIV), ECV_(ECV), alpha_(alpha), beta_(beta),
	negative_pressure_(negative_pressure), temp_d_(0), temp_p_(0), e_index_(e_index){}

double Tillotson::dp2EI(double d, double p) const
{
	double eta = d / rho0_;
	double c = E0_ * eta*eta;
	double AB = (A_ - 2 * B_)*eta + (B_ - A_) + B_ * eta*eta;
	double A_B = A_ - B_;
	double sqr = std::sqrt(d*c * 2 * (a_ - b_)*(p - (A_B + eta * B_)*(eta - 1)) + d * d*c*c*(a_ + b_)*(a_ + b_) + (p - (eta - 1)*(A_B + eta * B_))
		*(p - (eta - 1)*(A_B + eta * B_)));
	double first_part = p - AB - a_ * c*d - b_ * c*d;
	double E = (first_part + sqr) / (2 * a_*d);
	if (E < 0)
	{
		E = p * 0.001 / d;
	}
	assert(E > 0);
	return E;
}

double Tillotson::dp2EIV(double d, double p) const
{
	double eta = d / rho0_;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
	double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
	double eta3 = beta_ * (rho0_ / d - 1);
	double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
	double b = b_ * exp_alpha;
	double AB = exp_alpha * exp_beta;
	double E = (p - AB - a_ * c*d - b * c*d + sqrt(4 * a_*c*d*(p - AB) + std::pow(AB + (a_ + b)*c*d - p, 2))) /
		(2 * a_*d);
	if (E < 0)
	{
		//std::cout << "d " << d << " p " << p << std::endl;
		E = p * 0.001 / d;
	}
	assert(E > 0);
	return E;
}

double Tillotson::de2pI(double d, double e)const
{
	double eta = d / rho0_;
	double c = E0_ * eta*eta;
	double AB = (A_ - 2 * B_)*eta + (B_ - A_) + B_ * eta*eta;
	double res = 0;
	//if (negative_pressure_)
		//res = (a_ + b_ / (e / c + 1))*d*e + AB;
	//else
	res = std::max((a_ + b_ / (e / c + 1))*d*e + AB, a_*d*e*1e-7);
	return res;
}

double Tillotson::de2pII(double d, double e)const
{
	double P2 = de2pI(d, EIV_);
	double P3 = de2pIV(d, ECV_);
	return std::max(((e - EIV_)*P3 + (ECV_ - e)*P2) / (ECV_ - EIV_), a_*d*e*1e-7);
}

double Tillotson::de2pIV(double d, double e)const
{
	double eta = d / rho0_;
	if (alpha_ > 100 * eta*eta)
		return a_ * d*e;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
	double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
	double eta3 = beta_ * (rho0_ / d - 1);
	double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
	//if (negative_pressure_)
		return a_ * d*e + exp_alpha * (b_*d*e / (e / c + 1) + exp_beta);
	//else
	//	return std::max(a_*d*e + exp_alpha * (b_*d*e / (e / c + 1) + exp_beta), a_ *d*e*1e-7);
}

double Tillotson::dep2cI(double d, double e, double p) const
{
	double eta = d / rho0_;
	double w0 = e / (E0_*eta*eta) + 1;
	double gamma = a_ + b_ / w0;
	double res = (gamma + 1)*p / d + (A_ + B_ * (eta*eta - 1)) / d + b_ * (w0 - 1)*(2 * e - p / d) / (w0*w0);
	res = std::max(res, 1e-10*E0_);
	return res;
}

double Tillotson::dep2cIV(double d, double e, double p) const
{
	double eta = d / rho0_;
	double w0 = e / (E0_*eta*eta) + 1;
	double z = 1.0 / eta - 1.0;
	double afactor = (alpha_*z*z > 100) ? 0 : std::exp(-alpha_ * z*z);
	double res0 = p * (a_ + b_ * afactor / w0 + 1) / d;
	double bfactor = (beta_*z > 100) ? 0 : std::exp(-beta_ * z);
	double res1 = A_ * bfactor*afactor*(1 + (eta - 1)*(beta_ + 2 * alpha_*z - eta) / (eta*eta)) / rho0_;
	res1 += b_ * d*e*afactor*(2 * alpha_*z*w0 / rho0_ + (p / d - 2 * e) / (E0_*d)) / (w0*w0*eta*eta);
	double res = std::max(res0 + res1, 1e-10*E0_);
	return res;
}

struct dp2eII
{
	dp2eII(Tillotson const& eos) : eos_(eos)
	{}

	double operator()(double e)
	{
		double res = std::abs(1.0 - eos_.de2pII(eos_.temp_d_, e) / eos_.temp_p_);
		return res;
	}
private:
	Tillotson const& eos_;
};

double Tillotson::dp2e(double d, double p, tvector const & tracers, vector<string> const & tracernames) const
{
	if(tracernames.size() > 0)
		return tracers[e_index_];

	double eta = d / rho0_;
	double mu = eta - 1;
	double c = E0_ * eta*eta;
	double A = A_ * mu;
	if (d >= rho0_)
	{
		return dp2EI(d, p);
	}
	else
	{
		double e4 = dp2EIV(d, p);
		double p4 = de2pI(d, e4);
		double e1 = dp2EI(d, p);
		double p1 = de2pI(d, e1);
		//double p1 = de2pI(d, e1);
		if (e1 < EIV_ && p1 > 0)
			return dp2EI(d, p);
		if (p4<0 || e4>=ECV_)
			return e4;

		temp_d_ = d;
		temp_p_ = p;
		boost::uintmax_t it = 50;
		std::pair<double, double> res,res2;
		try
		{
			//res = boost::math::tools::toms748_solve(dp2eII(*this), EIV_, ECV_, boost::math::tools::eps_tolerance<double>(30), it);
			res=boost::math::tools::brent_find_minima(dp2eII(*this), EIV_, ECV_, 30, it);
			//res2 = boost::math::tools::brent_find_minima(dp2eII(*this), EIV_, 0.5*(EIV_+ECV_), 30, it);
		}
		catch (boost::exception const& eo)
		{
			double eta2 = alpha_ * (rho0_ / d - 1) * (rho0_ / d - 1);
			double exp_alpha = (eta2 > 100) ? 0 : std::exp(-eta2);
			double eta3 = beta_ * (rho0_ / d - 1);
			double exp_beta = (eta3 > 100) ? 0 : A * std::exp(-eta3);
			double PIV = de2pI(d, EIV_);
			double PCV = 0;
			if (negative_pressure_)
				PCV = a_ * d*ECV_ + exp_alpha * (b_*d*ECV_ / (ECV_ / c + 1) + exp_beta);
			else
				PCV = std::max(a_*d*ECV_ + exp_alpha * (b_*d*ECV_ / (ECV_ / c + 1) + exp_beta), (a_ + b_)*d*ECV_*1e-7);
			std::cout << " EIV_ " << EIV_ << " ECV_ " << ECV_ << " density " << d << " pressure " << p << " PIV " << PIV << " PCV " << PCV << std::endl;
		}
		double result = res.first;
		//double result = res.second > res2.second ? res2.first : res.first;
		if (result > EIV_ && result < ECV_)
		{
			double newp = de2p(d, result);
			if (newp > p*0.001)
			{
				if (std::abs(p - newp) > 0.001*std::abs(p))
				{
					UniversalError eo("No dp2e convergence");
					eo.AddEntry("Density", d);
					eo.AddEntry("Pressure", p);
					eo.AddEntry("New Pressure", newp);
					eo.AddEntry("EIV", EIV_);
					eo.AddEntry("ECV", ECV_);
					eo.AddEntry("First energy", res.first);
					eo.AddEntry("Second energy", res.second);
					eo.AddEntry("First pressure", de2p(d, res.first));
					eo.AddEntry("Second pressure", de2p(d, res.second));
					throw eo;
				}
			}
			assert(result > 0);
			return result;
		}
		return e4;
	}
}

double Tillotson::de2p(double d, double e, tvector const& /*tracers*/, vector<string> const& /*tracernames*/) const
{
	if (d >= rho0_)
	{
		return de2pI(d, e);
	}
	else
	{
		if (e <= EIV_)
			return de2pI(d, e);
		if (e >= ECV_)
			return de2pIV(d, e);
		return de2pII(d, e);
	}
}

double Tillotson::de2c(double d, double e, tvector const & tracers, vector<string> const & tracernames) const
{
	double p = de2p(d, e, tracers, tracernames);
	return dp2c(d, p, tracers, tracernames);
}

double Tillotson::dp2c(double d, double p, tvector const & tracers, vector<string> const & tracernames) const
{
	double e = dp2e(d, p, tracers, tracernames);
	if (d >= rho0_)
	{
		double res = dep2cI(d, e, p);
		assert(res > 0);
		return std::sqrt(res);
	}
	else
	{
		if (e <= EIV_)
		{
			double res = dep2cI(d, e, p);
			assert(res > 0);
			return std::sqrt(res);
		}
		//double e4 = dp2EIV(d, p);
		if (e >= ECV_)
		{
			double res = dep2cIV(d, e, p);
			assert(res > 0);
			return std::sqrt(res);
		}
		else
		{
			double c1 = dep2cI(d, e, p);
			double c4 = dep2cIV(d, e, p);
			double res = std::sqrt((c1*(ECV_ - e) + c4 * (e - EIV_)) / (ECV_ - EIV_));
			assert(res > 0);
			return res;
		}
	}
}

double Tillotson::dp2s(double /*d*/, double /*p*/, tvector const & /*tracers*/, vector<string> const & /*tracernames*/) const
{
	std::cout << "dp2s not implemented" << std::endl;
	assert(false);
	return 0;
}

double Tillotson::sd2p(double /*s*/, double /*d*/, tvector const & /*tracers*/, vector<string> const & /*tracernames*/) const
{
	std::cout << "sd2p not implemented" << std::endl;
	assert(false);
	return 0;
}

Tillotson::~Tillotson(void)
{}


#pragma once

#include <vector>
#include <cppsim/circuit_builder.hpp>
#include <cppsim/type.hpp>
#include <cppsim/simulator.hpp>
#include <cppsim/utility.hpp>
#include "problem.hpp"
#include "optimizer.hpp"
#include "differential.hpp"
#include <functional>
#include <Eigen/Dense>

class QuantumCircuitEnergyMinimizationSolver {
private:
    ParametricQuantumCircuit* _circuit;
    const std::function<ParametricQuantumCircuit* (UINT, UINT)>* _circuit_construction;
    UINT _param_count;
    std::vector<double> _parameter;
    double loss;
public:
    bool verbose;
    QuantumCircuitEnergyMinimizationSolver(const std::function<ParametricQuantumCircuit*(UINT,UINT)>* circuit_generator, UINT param_count = 0) {
        _circuit_construction = circuit_generator;
        _param_count = param_count;
        _circuit = NULL;
        verbose = false;
    };
    virtual ~QuantumCircuitEnergyMinimizationSolver() {
    }

    virtual void solve(
        EnergyMinimizationProblem* instance,
        UINT max_iteration = 100,
        std::string optimizer_name = "GD",
        std::string differentiation_method = "HalfPi"
    ) {
        if (_circuit != NULL) {
            delete _circuit;
            _circuit = NULL;
        }
        _circuit = (*_circuit_construction)(instance->get_qubit_count(), _param_count);
        auto _simulator = new ParametricQuantumCircuitSimulator(_circuit);
        _param_count = _simulator->get_parametric_gate_count();

        _parameter = std::vector<double>(_param_count, 0.);
        std::vector<double> gradient(_param_count);
        Random random;
        random.set_seed(0);
        for (auto& val : _parameter) val = random.uniform()*acos(0.0)*4;

        GradientBasedOptimizer* optimizer;
        QuantumCircuitGradientDifferentiation* differentiation;

        if (optimizer_name == "Adam") {
            optimizer = new AdamOptimizer(_param_count);
        }
        else if (optimizer_name == "GD") {
            optimizer = new GradientDecentOptimizer(_param_count);
        }
        else return;
        if (differentiation_method == "HalfPi") {
            differentiation = new GradientByHalfPi();
        }
        std::vector<double> old_param;
        for (UINT iteration = 0; iteration < max_iteration; ++iteration) {
            loss = differentiation->compute_gradient(_simulator, instance, _parameter, &gradient);

            if(verbose){
                std::cout << " *** epoch " << iteration << " *** " << std::endl;
                std::cout << " * loss = " << loss << std::endl;
                old_param = _parameter;
            }

            optimizer->apply_gradient(&_parameter, gradient);

            if (verbose) {
                for (UINT i = 0; i < _param_count; ++i) {
                    std::cout << " ** id " << i << " para = " << old_param[i] << " -> " << _parameter[i] << " , grad = " << gradient[i] << std::endl;
                }
            }
        }
        delete optimizer;
        delete differentiation;
        delete _simulator;
    }
    virtual double get_loss() { return loss; }
    virtual std::vector<double> get_parameter() { return _parameter; }
    ParametricQuantumCircuitSimulator* get_quantum_circuit_simulator() {
        return new ParametricQuantumCircuitSimulator(_circuit);
    }
};

class DiagonalizationEnergyMinimizationSolver {
private:
    ParametricQuantumCircuit* _circuit;
    const std::function<ParametricQuantumCircuit* (UINT, UINT)>* _circuit_construction;
    UINT _param_count;
    std::vector<double> _parameter;
    double loss;
public:
    bool verbose;
    DiagonalizationEnergyMinimizationSolver() {
        verbose = false;
    };
    virtual ~DiagonalizationEnergyMinimizationSolver() {
    }

    virtual void solve(
        EnergyMinimizationProblem* instance
    ) {
        const UINT qubit_count = instance->get_qubit_count();
        const UINT term_count = instance->get_term_count();
        const ITYPE matrix_dim = 1ULL << qubit_count;

        ComplexMatrix observable_matrix = ComplexMatrix::Zero(matrix_dim, matrix_dim);
        for (UINT term_index = 0; term_index < term_count; ++term_index) {
            auto Pauli_operator = instance->get_Pauli_operator(term_index);
            double coef = Pauli_operator->get_coef();
            auto target_index_list = Pauli_operator->get_index_list();
            auto pauli_id_list = Pauli_operator->get_pauli_id_list();

            std::vector<UINT> whole_pauli_id_list(qubit_count, 0);
            for (UINT i = 0; i < target_index_list.size(); ++i) {
                whole_pauli_id_list[target_index_list[i]] = pauli_id_list[i];
            }

            ComplexMatrix pauli_matrix;
            get_Pauli_matrix(pauli_matrix,whole_pauli_id_list);
            observable_matrix += coef*pauli_matrix;
        }

        observable_matrix.eigenvalues();
        Eigen::SelfAdjointEigenSolver<ComplexMatrix> eigen_solver(observable_matrix);
        loss = eigen_solver.eigenvalues()[0];

        if (verbose)    std::cout << "Eigenvalues : " << std::endl << eigen_solver.eigenvalues() << std::endl;
    }
    virtual double get_loss() { return loss; }
};