/*!
* Copyright 2007  Technical University of Catalonia
*
* 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)
*
*  Authors: Dmitry Bufistov
*           Andrey Parfenov
*/

#ifndef BOOST_GRAPH_HOWARD_CYCLE_RATIO_HOWARD_HPP
#define BOOST_GRAPH_HOWARD_CYCLE_RATIO_HOWARD_HPP

/*!
* \file Maximum cycle ratio algorithm (Jean Cochet-Terrasson, Guy
* Cochen and others)
*/
#include <exception>
#include <set>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost {
  namespace detail {
    /// To avoid round error.
    static const double mcr_howard_ltolerance = 0.00001;

    /*!
     * Calculate maximum cycle ratio of "good" directed multigraph
     * g. Use Howard's iteration policy algorithm ("Numerical
     * Computation of Spectral Elements in MAX-PLUS algebra" by Jean
     * Cochet-Terrasson, Guy Cochen and others).
     *
     * \param g = (V, E) - a "good" directed multigraph (out_degree of
     * each vertex is greater then 0). If graph is strongly connected
     * then it is "good".
     *
     * \param vim - Vertex Index, read property Map: V -> [0,
     * num_vertices(g)).
     *
     * \param ewm - edge weight read property map: E -> R
     *
     * \param ewm2 - edge weight2 read property map: E -> R+
     *
     * \return maximum_{for all cycles C}CR(C), or
     * -(std::numeric_limits<double>)::max() if g is not "good".
     */
    template <typename TGraph, typename TVertexIndexMap,
              typename TWeight1EdgeMap, typename TWeight2EdgeMap >
    class Cmcr_Howard
    {
    public:
      Cmcr_Howard(const TGraph& g, TVertexIndexMap vim, TWeight1EdgeMap ewm,
                  TWeight2EdgeMap ew2m)
        : m_g(g), m_vim(vim), m_ew1m(ewm), m_ew2m(ew2m),
          m_g2pi_g_vm(std::vector<pi_vertex_t>().end(), m_vim), /// Stupid dummy initialization
          m_minus_infinity(-(std::numeric_limits<double>::max)())
      {
        typedef typename boost::graph_traits<TGraph>::directed_category DirCat;
        BOOST_STATIC_ASSERT((boost::is_convertible<DirCat*, boost::directed_tag*>::value == true));
        m_cr = m_minus_infinity;
      }

      double operator()()
      {
        return maximum_cycle_ratio_Howard();
      }

      virtual ~Cmcr_Howard() { }

    protected:
      typedef typename boost::graph_traits<TGraph>::vertex_descriptor
        mcr_vertex_t;
      typedef typename boost::graph_traits<TGraph>::edge_descriptor
        mcr_edge_t;

      const TGraph&     m_g;
      typedef   std::vector<double>     eigenmode_t;
      eigenmode_t       m_eigen_value;
      eigenmode_t       m_eigen_vector;
      TVertexIndexMap   m_vim;
      TWeight1EdgeMap   m_ew1m;
      TWeight2EdgeMap   m_ew2m;

      typedef typename boost::remove_const<typename boost::property_traits<TWeight1EdgeMap>::value_type>::type mcr_edge_weight1_t;
      typedef typename boost::remove_const<typename boost::property_traits<TWeight2EdgeMap>::value_type>::type  mcr_edge_weight2_t;
      typedef typename boost::adjacency_list<
                         boost::listS, boost::vecS, boost::bidirectionalS,
                         boost::no_property,
                         boost::property<boost::edge_weight_t,
                                         mcr_edge_weight1_t,
                                         boost::property<boost::edge_weight2_t,
                                                         mcr_edge_weight2_t> > >
        pi_graph_t;
      typedef typename boost::property_map<pi_graph_t, boost::vertex_index_t>::type   TPiGraphVertexIndexMap;
      typedef typename boost::property_map<pi_graph_t, boost::edge_weight_t>::type    TPiGraphEdgeWeight1Map;
      typedef typename boost::property_map<pi_graph_t, boost::edge_weight2_t>::type   TPiGraphEdgeWeight2Map;

      typedef typename boost::property_traits<TPiGraphVertexIndexMap>::value_type     pigraph_vertex_index_t;

      pi_graph_t        m_pi_g;
      typedef   typename boost::graph_traits<pi_graph_t>::vertex_descriptor pi_vertex_t;
      typedef   typename boost::graph_traits<pi_graph_t>::edge_descriptor pi_edge_t;
      typedef   typename boost::iterator_property_map<typename std::vector<pi_vertex_t>::iterator, TVertexIndexMap> g2pi_g_vm_t;
      g2pi_g_vm_t m_g2pi_g_vm; ///Graph to Pi graph vertex map
      std::vector<pi_vertex_t> m_g2pig;
      int       m_step_number;
      const double m_minus_infinity;
      typedef typename std::vector<mcr_edge_t>        critical_cycle_t;
      double m_cr; ///Cycle ratio that already has been found

      class bad_graph
      {
      public:
        typedef typename boost::property_traits<TVertexIndexMap>::value_type
          v_index_t;

        bad_graph(v_index_t bvi) : bad_vertex_index(bvi) {}
        v_index_t what() const throw()
        {
          return bad_vertex_index;
        }

      private:
        v_index_t       bad_vertex_index;
      };

      double maximum_cycle_ratio_Howard()
      {
        try
          {
            construct_pi_graph();
          }
        catch (const bad_graph& a)
          {
            return m_minus_infinity;
          }
        std::vector<double>  max_eigen_val(boost::num_vertices(m_g));
        m_eigen_value.resize(boost::num_vertices(m_g));
        m_eigen_vector.resize(boost::num_vertices(m_g));
        m_step_number = 0;
        do
          {
            pi_eingen_value(get(vertex_index, m_pi_g), get(boost::edge_weight, m_pi_g), get(boost::edge_weight2, m_pi_g));
            ++m_step_number;
          }
        while (improve_policy_try1(max_eigen_val) || improve_policy_try2(max_eigen_val));
        return *(std::max_element(m_eigen_value.begin(), m_eigen_value.end()));
      }

      /*!
       *  Construct an arbitrary policy m_pi_g.
       */
      void      construct_pi_graph()
      {
        m_g2pig.resize(boost::num_vertices(m_g));
        m_g2pi_g_vm = boost::make_iterator_property_map(m_g2pig.begin(), m_vim);
        BGL_FORALL_VERTICES_T(vd, m_g, TGraph)
          {
            m_g2pi_g_vm[vd] = boost::add_vertex(m_pi_g);
            store_pivertex(m_g2pi_g_vm[vd], vd);
          }
        BGL_FORALL_VERTICES_T(vd1, m_g, TGraph)
          {
            if (boost::out_edges(vd1, m_g).first == boost::out_edges(vd1, m_g).second) throw bad_graph(m_vim[vd1]);
            mcr_edge_t ed = *boost::out_edges(vd1, m_g).first;
            pi_edge_t pied = boost::add_edge(m_g2pi_g_vm[source(ed, m_g)], m_g2pi_g_vm[target(ed, m_g)], m_pi_g).first;
            boost::put(boost::edge_weight, m_pi_g, pied, m_ew1m[ed]);
            boost::put(boost::edge_weight2, m_pi_g, pied, m_ew2m[ed]);
          }
      }

      class bfs_eingmode_visitor : public boost::default_bfs_visitor
      {
      public:
        bfs_eingmode_visitor(TPiGraphVertexIndexMap vi_m, TPiGraphEdgeWeight1Map w_m, TPiGraphEdgeWeight2Map& d_m,
                             eigenmode_t& e_val, eigenmode_t& e_vec, double ev) : m_index_map(vi_m), m_weight_map(w_m), m_delay_map(d_m),
                                                                                m_eig_value(&e_val), m_eig_vec(&e_vec), m_eigen_value(ev) { }

        template < typename Edge, typename  g_t>
        void examine_edge(Edge e, const g_t & g) const
        {
          typedef       typename boost::graph_traits<g_t>::vertex_descriptor Vertex;
          Vertex u = boost::target(e, g), v = boost::source(e, g);
          pigraph_vertex_index_t ind = m_index_map[u];
          (*m_eig_value)[ind] =  m_eigen_value;
          (*m_eig_vec)[ind] = m_weight_map[e] - m_eigen_value * m_delay_map[e] + (*m_eig_vec)[m_index_map[v]];
        }
      private:
        TPiGraphVertexIndexMap  m_index_map;
        TPiGraphEdgeWeight1Map  m_weight_map;
        TPiGraphEdgeWeight2Map  m_delay_map;
        eigenmode_t*    m_eig_value;
        eigenmode_t*    m_eig_vec;
        double                  m_eigen_value;
      };

      /*!
       *  Find a vertex in the Pi Graph which belongs to cycle, just a DFV until back edge found
       */
      pi_vertex_t       find_good_source(const pi_vertex_t start_vertex)
      {
        pi_vertex_t     good_vertex = start_vertex;
        typename std::set<pi_vertex_t>  s;
        s.insert(start_vertex);
        do
          {
            good_vertex = boost::target(*boost::out_edges(good_vertex, m_pi_g).first, m_pi_g);
          }
        while (s.insert(good_vertex).second);
        return good_vertex;
      }
      virtual   void    store_pivertex(pi_vertex_t pivd, mcr_vertex_t vd) {}
      virtual void      store_critical_edge(pi_edge_t   ed, critical_cycle_t& cc) {}
      virtual void      store_critical_cycle(critical_cycle_t& cc) {}

      /*!
       * \param startV - vertex that belongs to a cycle in policy graph m_pi_g
       */
      double    calculate_eigen_value(pi_vertex_t       startV)
      {
        std::pair<double, double>       accum_sums(0., 0.);
        pi_vertex_t vd = startV;
        critical_cycle_t        cc;
        do
          {
            pi_edge_t tmp_ed = *(boost::out_edges(vd, m_pi_g).first);
            store_critical_edge(tmp_ed, cc);
            accum_sums.first += boost::get(boost::edge_weight, m_pi_g, tmp_ed);
            accum_sums.second += boost::get(boost::edge_weight2, m_pi_g, tmp_ed);
            vd = boost::target(tmp_ed, m_pi_g);
          }
        while (vd != startV);
        //assert((std::abs<double>(accum_sums.first) <= 0.00000001) && "Division by zerro!");
        double cr = accum_sums.first / accum_sums.second;
        if (cr > m_cr)
          {
            m_cr = cr;
            store_critical_cycle(cc);
          }
        else
          {

          }
        return  cr;
      }

      /*!
       * Value determination. Find a generalized eigenmode (n^{k+1}, x^{k+1}) of A^{I_{k+1}} of the pi graph (Algorithm IV.1).
       */
      void pi_eingen_value(
                           TPiGraphVertexIndexMap index_map,
                           TPiGraphEdgeWeight1Map weight_map,
                           TPiGraphEdgeWeight2Map weigh2_map)
      {
        using namespace boost;
        typedef std::vector<default_color_type> color_map_t;
        color_map_t     vcm(num_vertices(m_pi_g), white_color);//Vertex color map
        color_map_t::iterator   uv_itr = vcm.begin(); //Undiscovered vertex
        reverse_graph<pi_graph_t>       rev_g(m_pi_g); //For backward breadth visit

        while ((uv_itr = std::find_if(uv_itr, vcm.end(),
                                      boost::bind(std::equal_to<default_color_type>(), boost::white_color, _1))) != vcm.end())
          ///While there are undiscovered vertices
          {
            pi_vertex_t gv = find_good_source(pi_vertex_t(uv_itr - vcm.begin()));
            pigraph_vertex_index_t      gv_ind = index_map[gv];
            m_eigen_value[gv_ind] = calculate_eigen_value(gv) ;
            bfs_eingmode_visitor        bfs_vis(index_map, weight_map, weigh2_map, m_eigen_value, m_eigen_vector, m_eigen_value[gv_ind]);
            typename boost::queue<pi_vertex_t> Q;
            breadth_first_visit(rev_g, gv, Q, bfs_vis, make_iterator_property_map(vcm.begin(), index_map));
          }
      }

      void improve_policy(mcr_vertex_t vd, mcr_edge_t new_edge)
      {
        remove_edge(*(out_edges(m_g2pi_g_vm[vd], m_pi_g).first), m_pi_g);
        pi_edge_t ned = add_edge(m_g2pi_g_vm[vd], m_g2pi_g_vm[target(new_edge, m_g)], m_pi_g).first;
        put(edge_weight, m_pi_g, ned, m_ew1m[new_edge]);
        put(edge_weight2, m_pi_g, ned, m_ew2m[new_edge]);
      }
      /*!
       * Policy Improvement. Improve the policy graph. The new policy graph has greater cycle ratio.
       * \return false if nothing can be improved.
       */
      bool  improve_policy_try1(std::vector<double>&  max_eing_vals)
      {
        bool  improved = false;
        BGL_FORALL_VERTICES_T(vd, m_g, TGraph)
          {
            double      max_ev = m_minus_infinity;/// Maximum eigen value for vertex
            mcr_edge_t  cr_ed;///Critical edge

            BGL_FORALL_OUTEDGES_T(vd, outed, m_g, TGraph)
              {
                if (m_eigen_value[m_vim[target(outed, m_g)]] > max_ev)
                  {
                    max_ev = m_eigen_value[m_vim[boost::target(outed, m_g)]];
                    cr_ed = outed;
                  }
              }
            if (max_ev > m_eigen_value[get(m_vim,vd)])
              {
                improve_policy(vd, cr_ed);
                improved = true;
              }
            max_eing_vals[get(m_vim,vd)] = max_ev;
          }
        return  improved;
      }

      /*!
       * \param max_eigen_values[u] = max_(for all adjacent vertices (u,v)) m_eigen_value[v]
       */
      bool improve_policy_try2(const std::vector<double>& max_eigen_values)
      {
        bool  improved = false;
        BGL_FORALL_VERTICES_T(vd, m_g, TGraph)
          {
            mcr_edge_t  impr_edge;
            double      max_val = m_minus_infinity;
            BGL_FORALL_OUTEDGES_T(vd, outed, m_g, TGraph)
              {
                ///If vertex vd is in the K(vd) set
                if (max_eigen_values[get(m_vim, vd)] <= m_eigen_value[get(m_vim, target(outed, m_g))])
                  {
                    double c_val = m_ew1m[outed] - m_ew2m[outed] * m_eigen_value[m_vim[boost::target(outed, m_g)]] +
                      m_eigen_vector[m_vim[boost::target(outed, m_g)]];
                    if (c_val > max_val)
                      {
                        max_val = c_val;
                        impr_edge = outed;
                      }
                  }
              }
            if ((max_val - m_eigen_vector[get(m_vim, vd)]) > mcr_howard_ltolerance)
              ///If m_eigen_vector[vd] == max_val
              {
                improve_policy(vd, impr_edge);
                improved = true;
              }
          }
        return improved;
      }
    };///Cmcr_Howard

    /*!
     * \return maximum cycle ratio and one critical cycle.
     */
    template <typename TGraph, typename TVertexIndexMap, typename TWeight1EdgeMap, typename TWeight2EdgeMap>
    class Cmcr_Howard1  : public        Cmcr_Howard<TGraph, TVertexIndexMap, TWeight1EdgeMap, TWeight2EdgeMap>
    {
    public:
      typedef Cmcr_Howard<TGraph, TVertexIndexMap, TWeight1EdgeMap, TWeight2EdgeMap> inhr_t;
      Cmcr_Howard1(const TGraph& g, TVertexIndexMap vim, TWeight1EdgeMap ewm, TWeight2EdgeMap ew2m) : inhr_t(g, vim, ewm, ew2m)
      {
        m_pi_g2g.resize(boost::num_vertices(g));
        m_pi_g2g_vm = boost::make_iterator_property_map(m_pi_g2g.begin(), boost::get(boost::vertex_index, this->m_pi_g));
      }

      void      get_critical_cycle(typename inhr_t::critical_cycle_t& cc) { return cc.swap(m_critical_cycle); }
    protected:
      void      store_pivertex(typename inhr_t::pi_vertex_t pivd, typename inhr_t::mcr_vertex_t vd)
      {
        m_pi_g2g_vm[pivd] = vd;
      }
      void      store_critical_edge(typename inhr_t::pi_edge_t  ed, typename inhr_t::critical_cycle_t& cc)
      {
        typename inhr_t::pi_vertex_t s = boost::source(ed, this->m_pi_g);
        typename inhr_t::pi_vertex_t t = boost::target(ed, this->m_pi_g);
        assert(boost::edge(m_pi_g2g_vm[s], m_pi_g2g_vm[t], this->m_g).second);
        cc.push_back(boost::edge(m_pi_g2g_vm[s], m_pi_g2g_vm[t], this->m_g).first); ///Store corresponding edge of the m_g
      }
      void      store_critical_cycle(typename inhr_t::critical_cycle_t& cc)
      {
        m_critical_cycle.swap(cc);
      }
    private:
      typename inhr_t::critical_cycle_t m_critical_cycle;
      typedef   typename boost::iterator_property_map<typename std::vector<typename inhr_t::mcr_vertex_t>::iterator, typename inhr_t::TPiGraphVertexIndexMap> pi_g2g_vm_t;
      pi_g2g_vm_t       m_pi_g2g_vm; ///Maps policy graph vertices to input graph vertices
      typename std::vector<typename inhr_t::mcr_vertex_t> m_pi_g2g;
    };

    /*!
     * Add sink vertex - this will make any graph good, the selfloop will have ratio equal to infinity
     * Properties must be "self increasing"
     */
    template <typename TGraph, typename TWeight1EdgeMap, typename TWeight2EdgeMap>
    typename boost::graph_traits<TGraph>::vertex_descriptor
    make_graph_good(TGraph& g, TWeight1EdgeMap ewm, TWeight2EdgeMap ew2m,
                    typename boost::property_traits<TWeight1EdgeMap>::value_type infinity)
    {
      typedef typename boost::graph_traits<TGraph>::edge_descriptor Edge;
      typename boost::graph_traits<TGraph>::vertex_descriptor sink = boost::add_vertex(g);

      BGL_FORALL_VERTICES_T(vd, g, TGraph)
        {
          Edge newed = boost::add_edge(vd, sink, g).first;
          boost::put(ewm, newed, 0);
          boost::put(ew2m, newed, 1);
        }
      Edge selfed = boost::edge(sink, sink, g).first;
      boost::put(ewm, selfed, infinity);
      return sink;
    }

    /*!
     * Construct from input graph g "safe" (suitable for maximum_cycle_ratio1() call) version - safeg
     */
    template <typename TG, typename TIndVertexMap, typename TW1EdgeMap, typename TW2EdgeMap, typename TSafeG, typename SafeG2GEdgeMap>
    void construct_safe_graph(const TG& g, TIndVertexMap vim, TW1EdgeMap ew1m, TW2EdgeMap ew2m, TSafeG& safeg, SafeG2GEdgeMap& sg2gm)
    {
      assert(num_vertices(g) == num_vertices(safeg));
      typedef typename graph_traits<TSafeG>::edge_descriptor tmp_edge_t;
      typedef typename graph_traits<TG>::edge_descriptor edge_t;
      typename graph_traits<TG>::edge_iterator  ei, ei_end;

      for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        {
          tmp_edge_t tmped = add_edge(vim[source(*ei, g)], vim[target(*ei, g)], safeg).first;
          sg2gm[tmped] = *ei;
          put(edge_weight, safeg, tmped, get(ew1m, *ei));
          put(edge_weight2, safeg, tmped, get(ew2m, *ei));
        }
    }

    template <typename TGraph, typename TVertexIndexMap, typename TWeight1EdgeMap, typename TWeight2EdgeMap>
    double      maximum_cycle_ratio_good_graph(const TGraph& g, TVertexIndexMap vim, TWeight1EdgeMap ewm, TWeight2EdgeMap ew2m,
                                               typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0)
    {
      if (pcc == 0)
        {
          return        detail::Cmcr_Howard<TGraph, TVertexIndexMap, TWeight1EdgeMap, TWeight2EdgeMap>(g, vim, ewm, ew2m)();
        }
      else
        {
          detail::Cmcr_Howard1<TGraph, TVertexIndexMap, TWeight1EdgeMap, TWeight2EdgeMap> obj(g, vim, ewm, ew2m);
          double maxcr = obj();
          obj.get_critical_cycle(*pcc);
          return        maxcr;
        }
    }

    template <typename TGraph, typename TVertexIndexMap, typename TWeight1EdgeMap, typename TWeight2EdgeMap, typename TEdgeIndexMap>
    double  minimum_cycle_ratio_good_graph(const TGraph& g, TVertexIndexMap vim, TWeight1EdgeMap ewm,
                                           TWeight2EdgeMap ew2m, TEdgeIndexMap eim,
                                           typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0)
    {
      typedef   typename boost::remove_const<typename boost::property_traits<TWeight1EdgeMap>::value_type>::type weight_value_t;
      BOOST_STATIC_ASSERT(!is_integral<weight_value_t>::value || is_signed<weight_value_t>::value);
      typename  std::vector<weight_value_t> ne_w(boost::num_edges(g));
      BGL_FORALL_EDGES_T(ed, g, TGraph)  ne_w[boost::get(eim, ed)] = -ewm[ed];
      return -maximum_cycle_ratio_good_graph(g, vim, boost::make_iterator_property_map(ne_w.begin(), eim), ew2m, pcc);
    }

    /*!
     * \param g directed multigraph.
     * \param pcc - pointer to the critical edges list.
     * \param minus_infinity must be small enough to garanty that g has at least one cycle with greater ratio.
     * \return minus_infinity if there're no cycles in the graph
     */
    template <typename TGraph, typename TWeight1EdgeMap, typename TWeight2EdgeMap>
    double      maximum_cycle_ratio1(const TGraph& g, TWeight1EdgeMap ewm, TWeight2EdgeMap ew2m,
                                     typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                                     typename boost::property_traits<TWeight1EdgeMap>::value_type minus_infinity = -(std::numeric_limits<int>::max)())
    {
      typedef typename boost::graph_traits<TGraph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<TGraph>::edge_descriptor Edge;
      boost::function_requires< boost::ReadWritePropertyMapConcept<TWeight1EdgeMap, Edge> >();
      boost::function_requires< boost::ReadWritePropertyMapConcept<TWeight2EdgeMap, Edge> >();

      TGraph& ncg = const_cast<TGraph&>(g);
      Vertex sink = detail::make_graph_good(ncg, ewm, ew2m, minus_infinity );

      double res = maximum_cycle_ratio_good_graph(ncg, boost::get(boost::vertex_index, g), ewm, ew2m, pcc);
      boost::clear_vertex(sink, ncg); boost::remove_vertex(sink, ncg);
      return    res;
    }

    /*!
     * Edge index MUST be in diapazon [0,..., num_edges(g)-1]
     * \return plus_infinity if g has no cycles.
     */
    template <typename TGraph, typename TWeight1EdgeMap, typename TWeight2EdgeMap, typename TEdgeIndexMap>
    double      minimum_cycle_ratio1(const TGraph& g, TWeight1EdgeMap ewm, TWeight2EdgeMap ew2m, TEdgeIndexMap eim,
                                     typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                                     typename boost::property_traits<TWeight1EdgeMap>::value_type plus_infinity = (std::numeric_limits<int>::max)()
                                     )
    {
      typedef typename boost::property_traits<TEdgeIndexMap>::value_type ei_t;
      typedef typename boost::graph_traits<TGraph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<TGraph>::edge_descriptor Edge;

      boost::function_requires< boost::ReadWritePropertyMapConcept<TWeight1EdgeMap, Edge> >();
      boost::function_requires< boost::ReadWritePropertyMapConcept<TWeight2EdgeMap, Edge> >();
      boost::function_requires< boost::ReadWritePropertyMapConcept<TEdgeIndexMap, Edge> >();

      TGraph& ncg = const_cast<TGraph&>(g);

      ei_t      nei = ei_t(boost::num_edges(g));
      Vertex sink = detail::make_graph_good(ncg, ewm, ew2m, plus_infinity );
      ///Maintain edge index invariant
      BGL_FORALL_VERTICES_T(vd, ncg, TGraph)
        {
          typename boost::graph_traits<TGraph>::edge_descriptor ed = boost::edge(vd, sink, ncg).first;
          boost::put(eim, ed, nei++);
        }
      double res = minimum_cycle_ratio_good_graph(ncg, boost::get(boost::vertex_index, ncg), ewm, ew2m, eim, pcc);
      boost::clear_vertex(sink, ncg); boost::remove_vertex(sink, ncg);
      return    res;
    }
    struct edge_less_than
    {
      template <typename TEdgeDescriptor> bool operator()(const TEdgeDescriptor& x, const TEdgeDescriptor& y) const
      {
        return x.get_property() < y.get_property();
      }
    };
  }///namespace detail
  namespace
  {
    template <typename TW1, typename TW2> struct safe_graph
    {
      typedef typename boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                                             typename boost::property<boost::edge_weight_t, TW1, typename boost::property<boost::edge_weight2_t, TW2> > > type;
    };
  }

  /*!
   * Calculate the maximum cycle ratio (mcr) of the directed multigraph g.
   * \param g directed multigraph
   * \param pcc - If provided then a critical cycle will be written to corresponding vector.
   * \param minus_infinity small enough value to garanty that g has at least one cycle with greater ratio.
   * \return mcr or minus_infinity if g has no cycles.
   */
  template <typename TGraph, typename TVertexIndexMap, typename TW1EdgeMap, typename TW2EdgeMap>
  double        maximum_cycle_ratio(const TGraph& g, TVertexIndexMap vim, TW1EdgeMap ew1m, TW2EdgeMap ew2m,
                                    typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                                    typename boost::property_traits<TW1EdgeMap>::value_type minus_infinity =
                                    -(std::numeric_limits<int>::max)())
  {
    typedef     typename remove_const<typename property_traits<TW1EdgeMap>::value_type>::type w1_t;
    typedef     typename remove_const<typename property_traits<TW2EdgeMap>::value_type>::type w2_t;
    typedef     typename safe_graph<w1_t, w2_t>::type safe_graph_t;
    typedef typename graph_traits<safe_graph_t>::edge_descriptor tmp_edge_t;
    typedef typename graph_traits<TGraph>::edge_descriptor edge_t;
    typename std::map<tmp_edge_t, edge_t, detail::edge_less_than>       tmpg2g;
    std::vector<tmp_edge_t> cc;
    safe_graph_t sg(num_vertices(g));
    detail::construct_safe_graph(g, vim, ew1m, ew2m, sg, tmpg2g);
    double  mcr = maximum_cycle_ratio1(sg, get(edge_weight, sg), get(edge_weight2, sg), pcc ? &cc : 0, minus_infinity);
    if (pcc && (mcr > minus_infinity))
      {
        pcc->clear();
        for (typename std::vector<tmp_edge_t>::iterator it = cc.begin(); it != cc.end(); ++it) pcc->push_back(tmpg2g[*it]);
      }
    return mcr;
  }

  template <typename TGraph, typename TVertexIndexMap, typename TW1EdgeMap, typename TW2EdgeMap, typename TIndEdgeMap>
  double        minimum_cycle_ratio(const TGraph& g, TVertexIndexMap vim, TW1EdgeMap ew1m, TW2EdgeMap ew2m, TIndEdgeMap eim,
                                    typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                                    typename boost::property_traits<TW1EdgeMap>::value_type plus_infinity =
                                    (std::numeric_limits<int>::max)())
  {
    typedef     typename boost::remove_const<typename boost::property_traits<TW1EdgeMap>::value_type>::type weight_value_t;
    BOOST_STATIC_ASSERT(!is_integral<weight_value_t>::value || is_signed<weight_value_t>::value);
    typename    std::vector<weight_value_t> ne_w(boost::num_edges(g));
    BGL_FORALL_EDGES_T(ed, g, TGraph)  ne_w[boost::get(eim, ed)] = -ew1m[ed];
    return -maximum_cycle_ratio(g, vim, boost::make_iterator_property_map(ne_w.begin(), eim), ew2m, pcc, -plus_infinity);
  }
  /*!
   * Calculate maximum mean cycle of directed weighted multigraph.
   * \param g directed multigraph
   * \return maximum mean cycle of g or minus_infinity if g has no cycles.
   */
  template <typename TGraph, typename TVertexIndexMap, typename TWeightEdgeMap, typename TIndEdgeMap>
  double  maximum_mean_cycle(const TGraph& g, TVertexIndexMap vim, TWeightEdgeMap ewm, TIndEdgeMap eim,
                             typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                             typename boost::property_traits<TWeightEdgeMap>::value_type minus_infinity =
                             -(std::numeric_limits<int>::max)())
  {
    typedef     typename boost::remove_const<typename boost::property_traits<TWeightEdgeMap>::value_type>::type weight_value_t;
    typedef     typename boost::graph_traits<TGraph>::edge_descriptor Edge;
    typename    std::vector<weight_value_t> ed_w2(boost::num_edges(g), 1);
    return maximum_cycle_ratio(g, vim, ewm, boost::make_iterator_property_map(ed_w2.begin(), eim), pcc, minus_infinity);
  }

  template <typename TGraph, typename TVertexIndexMap, typename TWeightEdgeMap, typename TIndEdgeMap>
  double  minimum_mean_cycle(const TGraph& g, TVertexIndexMap vim, TWeightEdgeMap ewm, TIndEdgeMap eim,
                             typename std::vector<typename boost::graph_traits<TGraph>::edge_descriptor>* pcc = 0,
                             typename boost::property_traits<TWeightEdgeMap>::value_type plus_infinity =
                             (std::numeric_limits<int>::max)())
  {
    typedef     typename boost::remove_const<typename boost::property_traits<TWeightEdgeMap>::value_type>::type weight_value_t;
    typedef     typename boost::graph_traits<TGraph>::edge_descriptor Edge;
    typename    std::vector<weight_value_t> ed_w2(boost::num_edges(g), 1);
    return      minimum_cycle_ratio(g, vim, ewm, boost::make_iterator_property_map(ed_w2.begin(), eim), eim, pcc, plus_infinity);
  }
} //namespace boost
#endif


//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/boostorg/beast
//

#ifndef BOOST_BEAST_CORE_DETAIL_PCG_HPP
#define BOOST_BEAST_CORE_DETAIL_PCG_HPP

#include <boost/core/ignore_unused.hpp>
#include <cstdint>
#include <random>

namespace boost {
namespace beast {
namespace detail {

class pcg
{
    std::uint64_t state_ = 0;
    std::uint64_t increment_;

public:
    using result_type = std::uint32_t;

    // Initialize the generator.
    // There are no restrictions on the input values.
    pcg(
        std::uint64_t seed,
        std::uint64_t stream)
    {
        // increment must be odd
        increment_ = 2 * stream + 1;
        boost::ignore_unused((*this)());
        state_ += seed;
        boost::ignore_unused((*this)());
    }

    std::uint32_t
    operator()()
    {
        std::uint64_t const p = state_;
        state_ = p *
            6364136223846793005ULL +
            increment_;
        std::uint32_t const x =
            static_cast<std::uint32_t>(
            ((p >> 18) ^ p) >> 27);
        std::uint32_t const r = p >> 59;
    #ifdef BOOST_MSVC
        return _rotr(x, r);
    #else
        return (x >> r) | (x << ((1 + ~r) & 31));
    #endif
    }
};

} // detail
} // beast
} // boost

#endif


//
//  AudioProcessor.cpp
//  Ribbons
//
//  Created by Bryce Matsumori on 10/4/14.
//
//

#include "cinder/audio/Context.h"
#include "cinder/audio/Source.h"
#include <boost/filesystem/path.hpp>

#include "AudioProcessor.h"

#define MIN_FREQ 100
#define MAX_FREQ 4300

#define CAPTURE_INTERVAL 0.02f

#define BEAT_MAX_FREQ 250
#define BEAT_THRESHOLD 0.09f

#define FREQ_TO_BIN(freq) (size_t)((float)freq * mMonitorSpectralNode->getFftSize() / mMonitorSpectralNode->getSampleRate())
#define BIN_TO_FREQ(bin) bin * mMonitorSpectralNode->getSampleRate() / mMonitorSpectralNode->getFftSize()

AudioProcessor::AudioProcessor() {
}

AudioProcessor::~AudioProcessor() {
    if (mLastMagSpectrum) delete mLastMagSpectrum;
}

bool AudioProcessor::init(const ci::DataSourceRef dataSource) {
    auto ctx = ci::audio::Context::master();

    // Create the spectrum node
    // By providing an FFT size double that of the window size, we 'zero-pad' the analysis data, which gives an increase in resolution of the resulting spectrum data
    auto monitorFormat = ci::audio::MonitorSpectralNode::Format().fftSize(2048).windowSize(1024);
    mMonitorSpectralNode = ctx->makeNode( new ci::audio::MonitorSpectralNode(monitorFormat) );

    ctx->enable();

    try {
        ci::audio::SourceFileRef sourceFile(ci::audio::SourceFile::create(dataSource));

        mFilePlayerNode = ctx->makeNode( new ci::audio::FilePlayerNode );
        mFilePlayerNode->setSourceFile(sourceFile);

        mFilePlayerNode >> mMonitorSpectralNode;
        mFilePlayerNode >> ctx->getOutput();

        mFilePlayerNode->enable();

        return true;
    } catch (ci::audio::AudioFileExc e) {
        printf("Error loading audio: %s\n", e.what());
    }

    return false;
}

void AudioProcessor::start() {
    mFilePlayerNode->start();
}

void AudioProcessor::update(float time) {
//    if (time - mLastCaptureTime > CAPTURE_INTERVAL) {
        if (mLastMagSpectrum) delete mLastMagSpectrum;
        if (mMagSpectrum) mLastMagSpectrum = new std::vector<float>(*mMagSpectrum);

//        mLastCaptureTime = time;
//    }

    mMagSpectrum = &mMonitorSpectralNode->getMagSpectrum();
    mVolume      =  mMonitorSpectralNode->getVolume();

    mMinBin = FREQ_TO_BIN(MIN_FREQ);
    mMaxBin = FREQ_TO_BIN(MAX_FREQ);
}

void AudioProcessor::setPaused(bool paused) {
    mFilePlayerNode->setEnabled(!paused);
}

size_t AudioProcessor::getNumBins() const {
    return mMaxBin - mMinBin + 1;
}

size_t AudioProcessor::getSampleRate() const {
    return mMonitorSpectralNode->getSampleRate();
}

float AudioProcessor::getAmpForBin(size_t bin) const {
    return mMagSpectrum->at(bin);
}

float AudioProcessor::getAmpForBins(size_t start, size_t len) const {
    size_t low = mMinBin + start, high = mMinBin + start + len;
    if (high > mMagSpectrum->size() - 1) high = mMagSpectrum->size() - 1;
    if (low > high) low = high;

    float total = 0;
    for (size_t i = low; i < high; ++i) {
        total += mMagSpectrum->at(i);
    }

    return total / len;
}

float AudioProcessor::getVolume() const {
    return mVolume;
}

float AudioProcessor::getTotalDiff() const {
    if (!mLastMagSpectrum) return 0.0f;

    float total = 0;
    for (size_t i = 0, len = mMagSpectrum->size(); i < len; ++i) {
        total += mMagSpectrum->at(i) - mLastMagSpectrum->at(i);
    }
    return total;
}

float AudioProcessor::getDiffBetween(float minFreq, float maxFreq) const {
    if (!mLastMagSpectrum) return 0.0f;

    float total = 0;
    size_t minBeatBin = FREQ_TO_BIN(minFreq); // can't be < 0
    size_t maxBeatBin = FREQ_TO_BIN(maxFreq);
    if (maxBeatBin > mMagSpectrum->size() - 1) maxBeatBin = mMagSpectrum->size() - 1;
    if (minBeatBin > maxBeatBin) minBeatBin = maxBeatBin;

    for (size_t i = minBeatBin; i <= maxBeatBin; ++i) {
        total += mMagSpectrum->at(i) - mLastMagSpectrum->at(i);
    }

    return total;
}

bool AudioProcessor::isBeat() {
    bool beat = getDiffBetween(0, BEAT_MAX_FREQ) > BEAT_THRESHOLD;

    bool lastState = mLastBeatState;
    mLastBeatState = beat;

    return lastState ? false : beat;
}

size_t AudioProcessor::getMaxFreq(float min, float max) const {
    size_t minBin = FREQ_TO_BIN(min); // can't be < 0
    size_t maxBin = FREQ_TO_BIN(max);
    if (maxBin > mMagSpectrum->size()) maxBin = mMagSpectrum->size();
    if (minBin > maxBin) minBin = maxBin;

    size_t highestBin;
    float highestAmp = 0;
    for (size_t i = minBin; i <= maxBin; ++i) {
        float amp = mMagSpectrum->at(i);
        if (amp > highestAmp) {
            highestBin = i;
            highestAmp = amp;
        }
        ++i;
    }

    return BIN_TO_FREQ(highestBin);
}


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

using namespace Eigen;

namespace DQ_robotics
{

Controller_invariant_Pid_SRIvar_Tracking::Controller_invariant_Pid_SRIvar_Tracking(DQ_kinematics robot, VectorXd upper_joint_limits, VectorXd lower_joint_limits, MatrixXd feedback_gain, double beta, double lambda_max, double epsilon) : DQ_controller()
{

    //Constants
    dq_one_ = DQ(1);

    //Initialization of argument parameters
    robot_dofs_     = (robot.links() - robot.n_dummy());
    robot_          = robot;
    kp_             = feedback_gain;
    ki_             = MatrixXd::Zero(feedback_gain.rows(),feedback_gain.cols());
    kd_             = MatrixXd::Zero(feedback_gain.rows(),feedback_gain.cols());
    beta_           = beta;
    lambda_max_     = lambda_max;

    //Initilization of remaining parameters
    thetas_         = MatrixXd(robot_dofs_,1);
    delta_thetas_   = MatrixXd::Zero(robot_dofs_,1);

    task_jacobian_                 = MatrixXd(8,robot_dofs_);
    task_jacobian_pseudoinverse_   = MatrixXd(robot_dofs_,8);
    svd_                           = JacobiSVD<MatrixXd>(robot_dofs_,8);
    svd_sigma_inverted_            = MatrixXd::Zero(robot_dofs_,8);
    identity_                      = Matrix<double,8,8>::Identity();

    error_             = MatrixXd(8,1);
    integral_error_     = MatrixXd::Zero(8,1);
    last_error_         = MatrixXd::Zero(8,1);
    at_least_one_error_ = false;

    // New terms included in proper error [NEW ERROR]
    enable_trackintTerm = true;
    enable_trackintTerm = false;
    trackingTerm_           = MatrixXd::Zero(8,1);
    last_reference_         = DQ(0,0,0,0,0,0,0,0);
    at_least_one_reference_ = false;

    end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);

    upper_joint_limits_    = upper_joint_limits;
    lower_joint_limits_    = lower_joint_limits;
    original_dummy_joints_ = robot_.dummy();
}

Controller_invariant_Pid_SRIvar_Tracking::Controller_invariant_Pid_SRIvar_Tracking( const DQ_kinematics& robot, VectorXd upper_joint_limits, VectorXd lower_joint_limits, const MatrixXd& feedback_gain, const MatrixXd& ki, const double ki_memory,  const MatrixXd& kd, const double& beta, const double& lambda_max, const double& epsilon) : DQ_controller()
{

   //Constants
    dq_one_ = DQ(1);

    //Initialization of argument parameters
    robot_dofs_     = (robot.links() - robot.n_dummy());
    robot_          = robot;
    kp_             = feedback_gain;
    ki_             = ki;
    kd_             = kd;
    ki_memory_      = ki_memory;
    beta_           = beta;
    lambda_max_     = lambda_max;

    //Initilization of remaining parameters
    thetas_         = MatrixXd(robot_dofs_,1);
    delta_thetas_   = MatrixXd::Zero(robot_dofs_,1);

    task_jacobian_                 = MatrixXd(8,robot_dofs_);
    task_jacobian_pseudoinverse_   = MatrixXd(robot_dofs_,8);
    svd_                           = JacobiSVD<MatrixXd>(robot_dofs_,8);
    svd_sigma_inverted_            = MatrixXd::Zero(robot_dofs_,8);
    identity_                      = Matrix<double,8,8>::Identity();

    error_             = MatrixXd(8,1);
    integral_error_     = MatrixXd::Zero(8,1);
    last_error_         = MatrixXd::Zero(8,1);
    at_least_one_error_ = false;

    // New terms included in proper error and tracking  [NEW ERROR]
    enable_trackintTerm = true;
    enable_trackintTerm = false;

    trackingTerm_           = MatrixXd::Zero(8,1);
    last_reference_         = DQ(0,0,0,0,0,0,0,0);
    at_least_one_reference_ = false;

    end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);

    upper_joint_limits_    = upper_joint_limits;
    lower_joint_limits_    = lower_joint_limits;
    original_dummy_joints_ = robot_.dummy();
}

VectorXd Controller_invariant_Pid_SRIvar_Tracking::getNewJointPositions( const DQ reference, const VectorXd thetas)
{

    delta_thetas_ = getNewJointVelocities(reference, thetas);

    // Send updated thetas to simulation
    return (thetas_ + delta_thetas_);

}

VectorXd Controller_invariant_Pid_SRIvar_Tracking::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{

    thetas_ = thetas; //This is necessary for the getNewJointPositions to work

    DQ robot_base = robot_.base();
    DQ robot_effector = robot_.effector();

    VectorXd pseudo_dummy_joint_marker = VectorXd::Zero(robot_dofs_);
    VectorXd pseudo_thetas = thetas_;
    VectorXd pseudo_delta_thetas = VectorXd::Zero(robot_dofs_);
    VectorXd possible_new_thetas = VectorXd::Zero(robot_dofs_);

    MatrixXd original_dh_matrix = robot_.getDHMatrix();
    MatrixXd step_dh_matrix = original_dh_matrix;
    DQ_kinematics pseudo_robot(original_dh_matrix);
    pseudo_robot.set_base(robot_base);
    pseudo_robot.set_effector(robot_effector);

    bool should_break_loop = false;
    while(not should_break_loop){

        //Calculate jacobian (N = Hminus8(x_d) * C8 * J  ) [NEW ERROR]
        task_jacobian_  = pseudo_robot.analyticalJacobian(pseudo_thetas);
        task_jacobian_  = Hminus8(reference)*C8()*task_jacobian_;

        // Recalculation of measured data.
        end_effector_pose_ = pseudo_robot.fkm(pseudo_thetas);

        //Error: [NEW ERROR] invariant to transformations
        last_error_      = error_;
        error_           = vec8( dq_one_ - (end_effector_pose_.conj())*reference );
        // error_           = vec8(reference - end_effector_pose_);
        integral_error_ = error_ + ki_memory_*integral_error_;
        //error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference);

        ///Inverse calculation
        svd_.compute(task_jacobian_, ComputeFullU);
        singular_values_ = svd_.singularValues();

        //Damping Calculation
        int current_step_relevant_dof = ( pseudo_robot.links() - pseudo_robot.n_dummy() );
        double sigma_min = singular_values_( current_step_relevant_dof - 1);
        VectorXd u_min = svd_.matrixU().col( current_step_relevant_dof - 1);
        double lambda = lambda_max_;
        if (sigma_min < epsilon_)
        {
            lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_;
        }

        task_jacobian_pseudoinverse_ =    (task_jacobian_.transpose())*((task_jacobian_*task_jacobian_.transpose()
                                            + (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).inverse());

        // Trackink term [NEW ERROR]
        if( at_least_one_reference_ )
          trackingTerm_ = Hminus8( reference - last_reference_ )*C8()*vec8( end_effector_pose_ );
        else
          at_least_one_reference_ = true;
        last_reference_ = reference;
        // If trackint term is disabled
        if (enable_trackintTerm == false)
            trackingTerm_  = MatrixXd::Zero(8,1);

        // pseudo_delta_thetas = task_jacobian_pseudoinverse_*kp_*error_;  [NEW ERROR]
        if( at_least_one_error_ )
          pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_)  - trackingTerm_ );
        else
        {
          at_least_one_error_ = true;
          pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_  - trackingTerm_ );
        }

        //Update delta_thetas for possiblenewthetas calculation
        for(int i=0,j=0; i < robot_dofs_; i++)
        {
            if(pseudo_dummy_joint_marker(i) == 0)
            {
                delta_thetas_(i) = pseudo_delta_thetas(j);
                ++j;
            }
            //Do NOTHING if it should be ignored.
        }

        //Possible new thetas
        possible_new_thetas = thetas_ + delta_thetas_;

        //Verify if loop should end
        should_break_loop = true;
        int j=0;
        //For all joints
        for(int i = 0; i < robot_dofs_; i++){

            //If joint is not yet marked for not being considered in the minimization
            if(pseudo_dummy_joint_marker(i) == 0){

                //If the controller is trying to put a joint further than any of its limits
                if(    possible_new_thetas(i) > upper_joint_limits_(i)
                    || possible_new_thetas(i) < lower_joint_limits_(i) )
                {

                    //If the joint was already saturated sometime ago
                    double ep = 1.e-05;
                    if (    thetas_(i) > upper_joint_limits_(i) - ep
                         || thetas_(i) < lower_joint_limits_(i) + ep){

                        pseudo_dummy_joint_marker(i) = 1; //Mark it to be ignored in the minization
                        //std::cout << std::endl << "Joint " << i << " will be ignored in the next controller step.";
                        step_dh_matrix(4,i) = 1;          //Set matrix as dummy.
                        step_dh_matrix(0,i) = thetas_(i); //Set matrix theta as a fixed value.
                        should_break_loop = false;
                    }
                    //If the joint was not yet saturated and the controller wants to saturate it
                    else{

                        // Saturate the joint in this step.
                        if   ( possible_new_thetas(i) > upper_joint_limits_(i) ){
                            delta_thetas_(i) = upper_joint_limits_(i) - thetas_(i);
                            //std::cout << std::endl << "Joint = " << i << " was saturated in its upper_limit";
                        }
                        else if ( possible_new_thetas(i) < lower_joint_limits_(i) ){
                            delta_thetas_(i) = lower_joint_limits_(i) - thetas_(i);
                            //std::cout << std::endl << "Joint = " << i << " was saturated in its lower_limit";
                        }
                        else{
                            std::cout << std::endl << "Something is really wrong";
                        }
                        //The joint should still be considered in the minimizations.
                        pseudo_thetas(j) = thetas_(i);
                        ++j;
                    }

                }
                //If the controller is not trying to put this joint further than any of its limits, we consider the velocity given normally
                else{
                    delta_thetas_(i) = pseudo_delta_thetas(j);
                    pseudo_thetas(j) = thetas_(i);
                    ++j;
                }
            }
            //If joint was marked to be ignored, it shall be ignored.
            else{
                delta_thetas_(i) = 0;
            }

        }
        if( j == 0 ){
            std::cout << std::endl << "Robot will be unable to get out of this configuration using this controller.";
            delta_thetas_ = VectorXd::Zero(robot_dofs_);
            break;
        }

        if(not should_break_loop){
            pseudo_robot = DQ_kinematics(step_dh_matrix);  //Change DH
            pseudo_robot.set_base(robot_base);
            pseudo_robot.set_effector(robot_effector);
            pseudo_thetas.conservativeResize(j);           //Resize pseudothetas
        }

    }//While not should_break_loop

    return delta_thetas_;

}

void Controller_invariant_Pid_SRIvar_Tracking::enableTrackingTerm(const bool bool_falseortrue)
{
    enable_trackintTerm = bool_falseortrue;
}

} // END OF NAMESPACE


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_ULPDIST_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief ulpdist generic tag

     Represents the ulpdist function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct ulpdist_ : ext::elementwise_<ulpdist_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<ulpdist_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_ulpdist_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::ulpdist_, Site> dispatching_ulpdist_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::ulpdist_, Site>();
   }
   template<class... Args>
   struct impl_ulpdist_;
  }
  /*!
    Returns ulp distance of the two values.

    @par Semantic:

    @code
    T r = ulpdist(a0,a1);
    @endcode

    is similar to:

    @code
    T r = abs(x-y)/Eps<T>();
    @endcode

    @par Note:

    If the common type is integer it is the same as @c dist

    If the common type is floating point the ulpdist is is computed,
    by the above described method.

    It is often difficult to answer to the following question: "are
    these two floating computations results similar enough?". The
    ulpdist is a way to answer which is tuned for relative errors
    estimations and peculiarly adapted to cope with the limited bits
    accuracy of floating point representations.

    The method is the following:

     - Properly normalize the two numbers by the same factor in a way
     that the largest of the two numbers exponents will be brought to
     zero

      -Return the absolute difference of these normalized numbers
      divided by the rounding error Eps

    The roundind error is the ulp (unit in the last place) value, i.e. the
    floating number, the exponent of which is 0 and the mantissa is all zeros
    but a 1 in the last digit (it is not hard coded that way however).
    This means \f$2^-23\f$ for float and \f$2^-52\f$ for double.

    \arg For instance if two floating numbers (of same type) have an ulpdist of
    zero that means that their floating representation are identical.

    \arg Generally equality up to 0.5 ulp is the best that one can wish beyond
    strict equality.

    @par Examples:

    \arg Typically if a double is compared to the float representation of
    its floating conversion (they are exceptions as for fully representable
    reals) the ulpdist will be around 2^26.5 (~10^8)

    The ulpdist is also roughly equivalent to the number of representable
    floating points values between two given floating points values.

    \arg  @c ulpdist(1.0,1+Eps\<double\>())==0.5
    \arg  @c ulpdist(1.0,1+Eps\<double\>()/2)==0.0
    \arg  @c ulpdist(1.0,1-Eps\<double\>()/2)==0.25
    \arg  @c ulpdist(1.0,1-Eps\<double\>())==0.5
    \arg  @c ulpdist(double(Pi\<float\>()),Pi\<double\>())==9.84293e+07

    @see @funcref{ulp}, @funcref{Eps}, @funcref{eps}
    @param a0
    @param a1

    @return a value of same type as the inputs
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::ulpdist_, ulpdist, 2)
} }

#endif


/*********************                                                        */
/*! \file absEngine.h
 ** Top contributors (to current version):
 **   Jiangchao Liu
 ** This file is part of the AI & SMTReluplex project.
 ** Copyright (c) 2018-2100 by the authors listed in the file AUTHORS
 ** (in the top-level source directory) and their institutional affiliations.
 ** All rights reserved. See the file COPYING in the top-level source
 ** directory for licensing information.\endverbatim
 **/
#include "Log.h"
#include <string.h>
#include "dnn.h"
#include "absEle.h"
#include "absEngine.h"
#include <cassert>
#include <iomanip>
#include <iostream>
#include <sstream>
#include "matrix.h"
#include "absExe.h"
#include <sys/resource.h>
#include <signal.h>
#include <fstream>
#include <boost/date_time.hpp>

std::string inFileName = "";
std::string summaryFileName = "";
std::string ans = "";

void got_signal( int )
{
    printf( "Got signal\n" );

    if(summaryFileName != "") {

//        auto current = boost::posix_time::microsec_clock::local_time();
//        auto totalTime = current - problem.startTime;
//
//        long long initialTime = problem.initialWorkTime.total_milliseconds();
//        long long solveTime   = problem.solveTime.total_milliseconds();
//        long long total = totalTime.total_milliseconds() ;
//
//
//        std::ofstream summary(summaryFileName,std::ios::app);
//        auto p = inFileName.find_last_of('/');
//        summary << inFileName.substr(p+1,inFileName.length()) << " ,"
//                << "TIMEOUT , "
//                << totalTime << " ,"
//                << total << " ,"
//                << initialTime << " ,"
//                << solveTime << " ,"
//                << problem.cacheSize()<< std::endl;
//        summary.close();
    }

    exit(0);
}

int acas(){
  std::string path("/home/jiangchao/AIProjects/reluplex/ReluplexCav2017/nnet/ACASXU_run2a_1_2_batch_2000.nnet");
  //std::string path("/home/jiangchao/AIProjects/AIsmt/ReluplexCav2017/nnet/test.nnet");
  Log_info("Build an Acas network");
  Dnn *adnn = new Dnn(Acas, path);
  Log_info("Get the dims in the input layer");
  int inputDim = adnn->get_inputdim();
  std::cout<<"dim is"<<inputDim<<std::endl;
  Log_info("Build an engine based on abstract interpretation with an Acas network");
  AbsEngine *aeng = new AbsEngine(adnn);
  std::vector<std::string> name(inputDim);
  int i;
  for(i = 0; i < inputDim; i++){
    name[i] =  "dim" + std::to_string(i);
  }
  Log_info("Introduce contriants on inputs");
  double *mins = adnn->get_mins();
  double *maxes = adnn->get_maxes();
  Matrix amatrix(inputDim);
  // for(int i = 0; i < inputDim; i++){
  //   amatrix.set_uperbound(i,maxes[i]);
  //    amatrix.set_lowerbound(i,mins[i]);
  // }
  // amatrix.set_lowerbound(0,55947);
  // amatrix.set_lowerbound(3,1145);
  // amatrix.set_uperbound(4,60);

  // amatrix.set_lowerbound(0,-0.2000);
  // amatrix.set_uperbound(0,0.00000);
  // amatrix.set_lowerbound(1,0.027465);
  // amatrix.set_uperbound(1,0.227465);
  // amatrix.set_lowerbound(2,-0.200703);
  // amatrix.set_uperbound(2,-0.000703);
  // amatrix.set_lowerbound(3,0.053843);
  // amatrix.set_uperbound(3,0.253843);
  // amatrix.set_lowerbound(4,-0.20000);
  // amatrix.set_uperbound(4,-0.00000);

  // amatrix.set_lowerbound(0,-0.02000);
  // amatrix.set_uperbound(0,0.00000);
  // amatrix.set_lowerbound(1,0.0);
  // amatrix.set_uperbound(1,0.02);
  // amatrix.set_lowerbound(2,-0.0200);
  // amatrix.set_uperbound(2,-0.000);
  // amatrix.set_lowerbound(3,0.0);
  // amatrix.set_uperbound(3,0.02);
  // amatrix.set_lowerbound(4,-0.220000);
  // amatrix.set_uperbound(4,-0.20000);

  amatrix.set_lowerbound(0,-0.1000);
  amatrix.set_uperbound(0,-0.10000);
  amatrix.set_lowerbound(1,0.127465);
  amatrix.set_uperbound(1,0.127465);
  amatrix.set_lowerbound(2,-0.100703);
  amatrix.set_uperbound(2,-0.100703);
  amatrix.set_lowerbound(3,0.153843);
  amatrix.set_uperbound(3,0.153843);
  amatrix.set_lowerbound(4,-0.10000);
  amatrix.set_uperbound(4,-0.10000);

  double **coffs = amatrix.get_coff();
  double **bias = amatrix.get_bias();
  LogicOP *lop =  amatrix.get_lop();
  int num = amatrix.get_num();
  std::cout<<"num of cons is "<<num<<std::endl;
  std::cout<<"before building an absele"<<std::endl;
  AbsEle *aele = new AbsEle(inputDim,name,BOX,num,coffs,lop,bias);
  //  std::cout<<"after building an absele, the ales is "<<*aele<<std::endl<<std::flush;

  aeng->iterateWholeNet(aele);
  std::cout<<"computation finished"<<std::endl;
  //  std::cout<<*aele<<std::endl;
  std::cout<<"unsplit is "<<unsplit<<std::endl;
  std::cout<<"split is "<<split<<std::endl;
  return 0;
}

static void show_usage(std::string name)
{
    std::cerr << "Usage: " << name << std::endl
              << "Options:\n"
              << "\t-h,--help  \t\tShow this help message\n"
              << "\t-d,--delta  \t\tDelta value, a integer (delta %%) \n"
              << "\t-t,--type   \t\tRobustness type : AI2 / PLANET\n"
//              << "\t-m,--domain \t\tAbstract domain : BOX / TOPO\n"
              << "\t-n,--net    \t\tInputFile, RLV format file\n"
              << "\t-p,--point  \t\tFix input point: a 28*28 MNIST img in integer format\n"
              << "\t-b,--bound  \t\tOutput file, stores the bounds\n"
              << "\t-s,--summary\t\tSummary file: store the statistics\n"
              << std::endl;
}

int main(int argc, char* argv[])
{
    const rlim_t kStackSize = 128L * 1024L * 1024L;
    struct rlimit rl;
    int result = getrlimit(RLIMIT_STACK, &rl);
    if(result == 0){
        if (rl.rlim_cur < kStackSize){
            rl.rlim_cur = kStackSize;
            result = setrlimit(RLIMIT_STACK, &rl);
            if(result!=0){
                std::cout<<"stack limit reset faild"<<std::endl;
            }
        }
    }

    if (argc == 1) {
        std::string img("pic1.txt");
        //  std::string net("test.rlv");
        // std::string net("testNetworkB.rlv");
        std::string net("../caffeprototxt/AI2_MNIST_FNN_1/testNetworkB.rlv");

        //  std::string net("MNISTAI2_new.rlv");
        robust_rlv(img,AI, 0.04, net,TOPO,"bounds.txt","summary.txt");
        //  acas();
        return 0;
    }

    double delta = 0.0;
    ROBUST rob_type = PLANET;
    Domain domain_type = BOX;
    std::string net_file_name;
    std::string img_file_name;
    std::string bound_file_name;
    std::string summary_file_name;

    for (int i = 1; i < argc; ++i) {
        std::string arg = argv[i];
        if ((arg == "-h") || (arg == "--help")) {
            show_usage(argv[0]);
            return 0;
        } else if ((arg == "-d") || (arg == "--delta")) {
            if (i + 1 < argc) {
                int d = std::atol(argv[++i]);
//                if (delta > 1) {
                delta = d / 1000.0;
//                }
//                cout << delta << endl;
            } else {
                std::cerr << "--delta option requires one argument." << std::endl;
                return 1;
            }
        } else if ((arg == "-t") || (arg == "--type")) {
            if (i + 1 < argc) {
                string type_str = string(argv[++i]);
                //cout << type_str << endl;
                if ( type_str == "PLANET") {
                    rob_type = PLANET;
                }
                else if (type_str == "AI2") {
                    rob_type = AI;
                }
                else {
                    std::cerr << "--type option wrong argument." << std::endl;
                    return 1;
                }
            } else {
                std::cerr << "--type option requires one argument." << std::endl;
                return 1;
            }
//        } else if ((arg == "-m") || (arg == "--domain")) {
//            if (i + 1 < argc) {
//                string type_str = string(argv[++i]);
//                //cout << type_str << endl;
//                if (type_str == "BOX") {
//                    domain_type = BOX;
//                }
//                else if (type_str == "TOPO") {
//                    domain_type == TOPO;
//                }
//                else {
//                    std::cerr << "--domain option wrong argument." << std::endl;
//                    return 1;
//                }
//            } else {
//                std::cerr << "--type option requires one argument." << std::endl;
//                return 1;
//            }
        } else if ((arg == "-n") || (arg == "--net")) {
            if (i + 1 < argc) {
                net_file_name = string(argv[++i]);
            } else {
                std::cerr << "--net option requires one argument." << std::endl;
                return 1;
            }
        } else if ((arg == "-p") || (arg == "--point")) {
            if (i + 1 < argc) {
                img_file_name = string(argv[++i]);
            } else {
                std::cerr << "--point option requires one argument." << std::endl;
                return 1;
            }
        } else if ((arg == "-b") || (arg == "--bound")) {
            if (i + 1 < argc) {
                bound_file_name = string(argv[++i]);
            } else {
                std::cerr << "--bound option requires one argument." << std::endl;
                return 1;
            }
        } else if ((arg == "-s") || (arg == "--summary")) {
            if (i + 1 < argc) {
                summary_file_name = string(argv[++i]);
            } else {
                std::cerr << "--summary option requires one argument." << std::endl;
                return 1;
            }
        } else {
            return 1;
        }
    }

#ifdef BDOMAIN
    domain_type = BOX;
#endif

#ifdef TDOMAIN
    domain_type = TOPO;
#endif

    robust_rlv(img_file_name, rob_type, delta, net_file_name, domain_type,bound_file_name,summary_file_name);

}


/**
 * @file basic_monomial.hh
 * @author  李昊坤 (ker@pm.me)
 * @brief 定义类：basic_monomial
 *
 */

#ifndef CLPOLY_BASIC_MONOMIAL_HH
#define CLPOLY_BASIC_MONOMIAL_HH
#include <clpoly/variable.hh>
#include <clpoly/basic.hh>
#include <vector>
#include <functional>
#include <list>
#include <iostream>
#include <sstream>
#include <string>
#include <cassert>
#include <boost/container_hash/hash.hpp>

namespace clpoly{
    template <class compare>
    class basic_monomial
    {
        private:
            std::vector<std::pair<variable,int64_t>> __data;
            int64_t __deg=0;
            const compare* __comp= &compare::init;
        public:
            typedef compare compare_type;
            typedef variable first_type;
            typedef int64_t second_type;
            typedef typename std::vector<std::pair<variable,int64_t>>::iterator iterator;
            typedef typename std::vector<std::pair<variable,int64_t>>::const_iterator  const_iterator ;
            // static compare init_comp;

            constexpr int64_t deg() const {return this->__deg;}
            constexpr int64_t & deg() {return this->__deg;}
            inline int64_t re_deg()
            {
                this->__deg=0;
                for(auto &&i:this->__data)
                    this->__deg+=i.second;
                return this->__deg;
            }
            inline bool is_normal()  const
            {
                int64_t tmp_deg=0;
                for(auto &&i:this->__data)
                    tmp_deg+=i.second;
                return tmp_deg==this->__deg && pair_vec_normal_check(this->__data,this->comp());
            }
            inline void normalization()
            {
                auto tmp_size=pair_vec_normalization(this->__data,pair_compare<variable,int64_t,compare>(this->__comp));
                this->__data.resize(tmp_size);
                if (this->__data.size()*1.0/this->__data.capacity()<CLPOLY_shrink_bound && this->__data.capacity()>CLPOLY_shrink_bound_abs)
                {
                    //std::cout<<"shrink_to_fit";
                    this->__data.shrink_to_fit();
                }
                this->re_deg();
            }
            basic_monomial():__data(),__deg(0){}
            basic_monomial(const compare * c):__data(),__deg(0),__comp(c){}
            basic_monomial(const basic_monomial<compare> &m)
            :__data(m.__data),__deg(m.__deg),__comp(m.__comp)
            {}
            // explicit
            basic_monomial(const variable & v)
            :__data({{v,1}}),__deg(1)
            {}
            basic_monomial(basic_monomial<compare> &&m)
            :__data(std::move(m.__data)),__deg(std::move(m.__deg)),__comp(m.__comp)
            {
                m.clear();
            }
            basic_monomial( std::initializer_list<std::pair<variable,int64_t>> init)
            :__data(init)
            {
                this->normalization();
            }
            basic_monomial(const std::vector<std::pair<variable,int64_t>> & v)
            :__data(v)
            {
                this->normalization();
            }
            basic_monomial(std::vector<std::pair<variable,int64_t>> && v)
            :__data(std::move(v))
            {
                this->normalization();
            }

            basic_monomial<compare> & operator=(const variable &v)
            {
                this->__data={{v,1}};
                this->__deg=1;
                return *this;
            }
            basic_monomial<compare> & operator=(const basic_monomial<compare> & m)
            {
                if (&m==this)
                    return *this;
                this->__data=m.__data;
                this->__deg=m.__deg;
                this->__comp=m.__comp;
                return *this;
            }
            basic_monomial<compare> & operator=(basic_monomial<compare> && m)
            {
                if (&m==this)
                    return *this;
                this->__data=std::move(m.__data);
                this->__deg=std::move(m.__deg);
                this->__comp=m.__comp;
                m.clear();
                return *this;
            }
            basic_monomial<compare> & operator=( std::initializer_list<std::pair<variable,int64_t>> init)
            {
                this->__data=init;
                this->normalization();
                return *this;
            }
            basic_monomial<compare> & operator=(const std::vector<std::pair<variable,int64_t>> & init)
            {
                this->__data=init;
                this->normalization();
                return *this;
            }
            basic_monomial<compare> & operator=(std::vector<std::pair<variable,int64_t>> && init)
            {
                this->__data=std::move(init);
                this->normalization();
                return *this;
            }

            constexpr auto size() const {return this->__data.size();}
            constexpr iterator begin() {return this->__data.begin();}
            constexpr const_iterator begin() const {return this->__data.begin();}

            constexpr iterator end() {return this->__data.end();}
            constexpr const_iterator end() const {return this->__data.end();}

            constexpr std::pair<variable,int64_t>& front() {return this->__data.front();}
            constexpr const std::pair<variable,int64_t>& front() const {return this->__data.front();}
            constexpr std::pair<variable,int64_t>& back() {return this->__data.back();}
            constexpr const std::pair<variable,int64_t>& back() const {return this->__data.back();}
            constexpr void resize(std::size_t size){this->__data.resize(size);}
            constexpr void reserve(std::size_t size){this->__data.reserve(size);}
            inline void pop_back(){this->__deg-=this->back().second;this->__data.pop_back();}
            inline void push_back(const std::pair<variable,int64_t>&  value ){
                this->__deg+=value.second;
                this->__data.push_back(value);
            }
            inline void push_back(std::pair<variable,int64_t>&&  value){
                this->__deg+=value.second;
                this->__data.push_back(std::move(value));
            }
            constexpr std::pair<variable,int64_t>& operator[](std::size_t pos){return this->__data[pos];}
            constexpr const std::pair<variable,int64_t>& operator[](std::size_t pos) const {return this->__data[pos];}
            constexpr std::pair<variable,int64_t>& at(std::size_t pos){return this->__data.at(pos);}
            constexpr const std::pair<variable,int64_t>& at(std::size_t pos) const {return this->__data.at(pos);}
            constexpr bool empty()const{return this->__data.empty();}
            constexpr std::vector<std::pair<variable,int64_t>> & data() {return this->__data;}
            constexpr const std::vector<std::pair<variable,int64_t>> & data() const {return this->__data;}
            constexpr const compare & comp() const {return *(this->__comp);}
            //constexpr compare & comp() {return *(this->__comp);}
            inline bool comp(const variable & a,const variable &b)const {return (*this->__comp)(a,b);}
            constexpr void comp( const compare * c){this->__comp=c;}
            constexpr const compare * comp_ptr() const {return this->__comp;}
            inline void clear()
            {
                this->__data.clear();
                this->__deg=0;
            }
            inline void swap(basic_monomial<compare> &m)
            {
                this->__data.swap(m.__data);
                std::swap(this->__deg,m.__deg);
                std::swap(this->__comp,m.__comp);
            }
            friend inline basic_monomial operator*  (const basic_monomial &p1,const basic_monomial &p)
            {
                assert(p1.__comp==p.__comp || p1.comp()==p.comp());
                // #ifdef DEBUG
                //     if (!pair_vec_normal_check(p1.begin(),p1.end(),p1.comp()))
                //         throw std::invalid_argument("Left basic_monomial is not normal.");
                //     if (!pair_vec_normal_check(p.begin(),p.end(),p1.comp()))
                //         throw std::invalid_argument("Right basic_monomial is not normal.(In left comparation.)");
                //     std::asser
                // #endif
                basic_monomial new_p(p1.__comp);
                pair_vec_add(new_p.__data,p1.__data,p.__data,p1.comp());
                __auto_shrink(new_p.__data);
                new_p.__deg=p1.__deg+p.__deg;
                return new_p;
            }
            friend inline basic_monomial operator/  (const basic_monomial &p1,const basic_monomial &p)
            {
                assert(p1.__comp==p.__comp || p1.comp()==p.comp());
                // #ifdef DEBUG
                //     if (!pair_vec_normal_check(p1.begin(),p1.end(),p1.comp))
                //         throw std::invalid_argument("Left basic_monomial is not normal.");
                //     if (!pair_vec_normal_check(p.begin(),p.end(),p1.comp))
                //         throw std::invalid_argument("Right basic_monomial is not normal.(In left comparation.)");
                // #endif
                basic_monomial new_p(p1.__comp);
                pair_vec_sub(new_p.__data,p1.__data,p.__data,p1.comp());
                __auto_shrink(new_p.__data);
                new_p.__deg=p1.__deg-p.__deg;
                return new_p;
            }

            // inline basic_monomial  power(unsigned i) const
            // {
            //     basic_monomial newp;
            //     newp.comp(this->__comp);
            //     if (this->empty() || i==0)
            //         return newp;
            //     basic_monomial p(*this);
            //     while (i!=0)
            //     {
            //         if (i%2!=0)
            //             newp=newp*p;
            //         i>>=1;
            //         if (i!=0)
            //             p=p*p;
            //     }
            //     return newp;
            // }

            inline bool operator> (const basic_monomial<compare> &p) const
            {
                assert(this->__comp==p.__comp || this->comp()==p.comp());
                // #ifdef DEBUG
                //     if (!pair_vec_normal_check(this->begin(),this->end(),this->comp))
                //         throw std::invalid_argument("Left basic_monomial is not normal.");
                //     if (!pair_vec_normal_check(p.begin(),p.end(),this->comp))
                //         throw std::invalid_argument("Right basic_monomial is not normal.(In left comparation.)");
                // #endif
                // if (this->deg()==p.deg())
                //     return pair_vec_comp(this->__data,p.__data,this->comp);
                return (this->deg()>p.deg()) || ( this->deg()==p.deg() && pair_vec_comp(this->__data,p.__data,this->comp()));
            }
            inline bool operator== (const basic_monomial<compare> &p) const
            {
                assert(this->__comp==p.__comp || this->comp()==p.comp());
                // #ifdef DEBUG
                //     if (!pair_vec_normal_check(this->begin(),this->end(),this->comp))
                //         throw std::invalid_argument("Left basic_monomial is not normal.");
                //     if (!pair_vec_normal_check(p.begin(),p.end(),this->comp))
                //         throw std::invalid_argument("Right basic_monomial is not normal.(In left comparation.)");
                // #endif
                // if (this->deg()==p.deg())
                //     return pair_vec_equal_to(p.__data,this->__data);
                return (this->deg()==p.deg() && pair_vec_equal_to(p.__data,this->__data));
            }
            constexpr bool operator< (const basic_monomial<compare> &p) const
            {
                return (p>*this);
            }
            constexpr bool operator<=(const basic_monomial<compare> &p) const
            {
                return !(*this>p);
            }
            constexpr bool operator>=(const basic_monomial<compare> &p) const
            {
                return !(p>*this);
            }
            constexpr bool operator!=(const basic_monomial<compare> &p) const
            {
                return !(*this==p);
            }

            // inline basic_monomial operator-   () const
            // {
            //     return *this;
            // }
            // inline basic_monomial operator+ () const
            // {
            //     basic_monomial new_m;
            //     new_m.reserve(this->size());
            //     for (auto &&i:*this)
            //     {
            //         new_m.push_back({i.first,negate(i.second)});
            //     }
            //     new_m.comp=this->comp;
            //     return new_m;
            // }
            inline const_iterator find(const variable & m) const
            {
                // std::function<bool(const std::pair<variable,int16_t> &,const std::pair<variable,int16_t> &)> tmp_comp=
                //     [&](std::pair<variable,int16_t> a,std::pair<variable,int16_t> b){return (this->comp(a.first,b.first));};
                // return pair_vec_find_first(this->begin(),this->end(),
                //     std::pair<variable,int16_t>(m,0),pair_compare<variable,int64_t,compare>(this->__comp));
                const_iterator i=this->begin();
                while (i!=this->end() && i->first!=m)
                    ++i;
                return i;
            }
            inline iterator find(const variable & m)
            {
                // std::function<bool(const std::pair<variable,int16_t> &,const std::pair<variable,int16_t> &)> tmp_comp=
                //     [&](std::pair<variable,int16_t> a,std::pair<variable,int16_t> b){return (this->comp(a.first,b.first));};
                // return pair_vec_find_first(this->begin(),this->end(),
                //     std::pair<variable,int16_t>(m,0),pair_compare<variable,int64_t,compare>(this->__comp));
                iterator i=this->begin();
                while (i!=this->end() && i->first!=m)
                    ++i;
                return i;
            }
            inline int64_t deg(const variable & m) const
            {
                auto l=this->find(m);
                if (l!=this->end())
                    return l->second;
                else
                    return 0;
            }
            // inline int64_t & deg(const variable & m)
            // {
            //     auto l=this->find(m);
            //     if (l!=this->end())
            //         return l->second;
            //     else
            //         return 0;
            // }
            friend std::ostream& operator<<  (std::ostream& stream, const basic_monomial<compare>& v) {
                if (v.empty())
                    return stream<<'1';
                bool is_print=false;
                for (const auto &i:v.data())
                {
                    // if (i.second!=0)
                    // {
                        if (is_print)
                            stream<<"*";
                        stream<<i.first;
                        is_print=true;
                        if (i.second!=1)
                            stream<<"^"<<i.second;
                    // }
                }
                return stream;
            }
            std::string  str() const{
                if (this->empty())
                    return "1";
                std::ostringstream ss;
                // bool is_print=false;
                // for (auto&&i:this->__data)
                // {
                //     if (i.second!=0)
                //     {
                //         if (is_print)
                //             ss<<"*";
                //         ss<<i.first;
                //         is_print=true;
                //         if (i.second!=1)
                //             ss<<"^"<<i.second;
                //     }
                // }
                ss<<(*this);
                return ss.str();
            }

    };
    // template<class compare>  compare basic_monomial<compare>::init_comp=compare();
    // template<class compare>
    // void swap(basic_monomial<compare> & v1,basic_monomial<compare> & v2)
    // {
    //     v1.swap(v2);
    // }
    template<class compare>
    std::size_t hash_value(const basic_monomial<compare>& b)
    {
        boost::hash<std::vector<std::pair<variable,int64_t>>> hasher;
        return hasher(b.data());
    }

}
namespace std{
    template<class compare>
    struct hash<clpoly::basic_monomial<compare>>: public std::unary_function<clpoly::basic_monomial<compare>, std::size_t>
    {
        std::size_t operator()(const clpoly::basic_monomial<compare> & m) const
        {
            return clpoly::hash_value(m);
        }
    };
}
#endif


#ifndef _SEEGNIFY_TYPES_H_
#define _SEEGNIFY_TYPES_H_

#include <Eigen/Dense>

namespace seegnify {

// computation precision type
#define DTYPE float

// differential delta (lower is more accurate)
#define FINITE_DELTA 1e-3

// epsilon neighborhood
#define EPSILON 1e-6

// learning rate (lower is slower but more stable)
#define LEARNING_RATE 1e-2

// reinforcement learning reward discount
#define GAMMA_DISCOUNT 0.99

// Eigen types
typedef Eigen::Matrix<DTYPE, Eigen::Dynamic, Eigen::Dynamic> Tensor;
typedef Eigen::Map<Tensor> TensorMap;
typedef Eigen::Map<const Tensor> ConstTensorMap;

typedef Eigen::Matrix<DTYPE, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Vector> VectorMap;
typedef Eigen::Map<const Vector> ConstVectorMap;

typedef Eigen::Matrix<DTYPE, 1, Eigen::Dynamic> RowVector;
typedef Eigen::Map<RowVector> RowVectorMap;
typedef Eigen::Map<const RowVector> ConstRowVectorMap;

// Export symbols
#ifndef DLL_EXPORT
#define DLL_EXPORT __attribute__ ((visibility ("default")))
#endif

#ifndef DLL_EXPORT
#define DLL_LOCAL  __attribute__ ((visibility ("hidden")))
#endif

} /* namespace */

#endif /*_SEEGNIFY_TYPES_H_*/


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "GenericConstraintsModel.h"
#include <iostream>
#include "Simulation/TimeStepController.h"
#include "Demos/Visualization/Visualization.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"
#include "GenericConstraints.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setBendingStiffness(const void* value, void* clientData);
void TW_CALL getBendingStiffness(void* value, void* clientData);
void TW_CALL setDistanceStiffness(const void* value, void* clientData);
void TW_CALL getDistanceStiffness(void* value, void* clientData);

DemoBase *base;

const int nRows = 30;
const int nCols = 30;
const Real width = 10.0;
const Real height = 10.0;
Real bendingStiffness = 0.01;
Real distanceStiffness = 1.0;

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

	base = new DemoBase();
	base->init(argc, argv, "Generic cloth demo");

	GenericConstraintsModel *model = new GenericConstraintsModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	base->createParameterGUI();

	// OpenGL
	MiniGL::setClientIdleFunc (timeStep);
	MiniGL::addKeyFunc('r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwAddVarCB(MiniGL::getTweakBar(), "BendingStiffness", TW_TYPE_REAL, setBendingStiffness, getBendingStiffness, model, " label='Bending stiffness'  min=0.0 step=0.1 precision=4 group='Bending' ");
	TwAddVarCB(MiniGL::getTweakBar(), "DistanceStiffness", TW_TYPE_REAL, setDistanceStiffness, getDistanceStiffness, model, " label='Distance constraint stiffness'  min=0.0 step=0.1 precision=4 group='Cloth' ");

	MiniGL::mainLoop();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTriangleModels().size(); i++)
		model->getTriangleModels()[i]->updateMeshNormals(model->getParticles());
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));

	createMesh();
}

void render ()
{
	base->render();
}

/** Create a particle model mesh
*/
void createMesh()
{
	GenericConstraintsModel* model = (GenericConstraintsModel*)Simulation::getCurrent()->getModel();
	model->addRegularTriangleModel(nCols, nRows,
		Vector3r(0, 1, 0), AngleAxisr(M_PI * 0.5, Vector3r(1, 0, 0)).matrix(), Vector2r(width, height));

	// Set mass of points to zero => make it static
	ParticleData& pd = model->getParticles();
	pd.setMass(0, 0.0);
	pd.setMass(nRows-1, 0.0);

	// init constraints
	for (unsigned int cm = 0; cm < model->getTriangleModels().size(); cm++)
	{
		const unsigned int offset = model->getTriangleModels()[cm]->getIndexOffset();
		IndexedFaceMesh &mesh = model->getTriangleModels()[cm]->getParticleMesh();
		const unsigned int nEdges = mesh.numEdges();
		const IndexedFaceMesh::Edge *edges = mesh.getEdges().data();

		// distance constraints
		for (unsigned int i = 0; i < nEdges; i++)
		{
			const unsigned int v1 = edges[i].m_vert[0] + offset;
			const unsigned int v2 = edges[i].m_vert[1] + offset;

			model->addGenericDistanceConstraint(v1, v2, distanceStiffness);
		}

		// bending constraints
		const unsigned int *tris = mesh.getFaces().data();
		for (unsigned int i = 0; i < nEdges; i++)
		{
			const int tri1 = edges[i].m_face[0];
			const int tri2 = edges[i].m_face[1];
			if ((tri1 != 0xffffffff) && (tri2 != 0xffffffff))
			{
				// Find the triangle points which do not lie on the axis
				const int axisPoint1 = edges[i].m_vert[0];
				const int axisPoint2 = edges[i].m_vert[1];
				int point1 = -1;
				int point2 = -1;
				for (int j = 0; j < 3; j++)
				{
					if ((tris[3 * tri1 + j] != axisPoint1) && (tris[3 * tri1 + j] != axisPoint2))
					{
						point1 = tris[3 * tri1 + j];
						break;
					}
				}
				for (int j = 0; j < 3; j++)
				{
					if ((tris[3 * tri2 + j] != axisPoint1) && (tris[3 * tri2 + j] != axisPoint2))
					{
						point2 = tris[3 * tri2 + j];
						break;
					}
				}
				if ((point1 != -1) && (point2 != -1))
				{
					const unsigned int vertex1 = point1 + offset;
					const unsigned int vertex2 = point2 + offset;
					const unsigned int vertex3 = edges[i].m_vert[0] + offset;
					const unsigned int vertex4 = edges[i].m_vert[1] + offset;
					model->addGenericIsometricBendingConstraint(vertex1, vertex2, vertex3, vertex4, bendingStiffness);
				}
			}
		}
	}

	LOG_INFO << "Number of triangles: " << model->getTriangleModels()[0]->getParticleMesh().numFaces();
	LOG_INFO << "Number of vertices: " << nRows*nCols;

}

void TW_CALL setBendingStiffness(const void* value, void* clientData)
{
	bendingStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<GenericIsometricBendingConstraint, Real, &GenericIsometricBendingConstraint::m_stiffness>(bendingStiffness);
}

void TW_CALL getBendingStiffness(void* value, void* clientData)
{
	*(Real*)(value) = bendingStiffness;
}

void TW_CALL setDistanceStiffness(const void* value, void* clientData)
{
	distanceStiffness = *(const Real*)(value);
	((SimulationModel*)clientData)->setConstraintValue<GenericDistanceConstraint, Real, &GenericDistanceConstraint::m_stiffness>(distanceStiffness);
}

void TW_CALL getDistanceStiffness(void* value, void* clientData)
{
	*(Real*)(value) = distanceStiffness;
}


#include <stdio.h>
#include <stdlib.h>
#include <GL/glu.h>
#include <GLFW/glfw3.h>
#include <armadillo>
#include "Object.hpp"
#include "Transform.hpp"
#include "Plotter.hpp"

int main( void )
{
    arma::frowvec eye = {0.0, 0.0, 10.0};
    arma::frowvec camera = {0.0, 0.0, 0.0};

    GLFWwindow* window;

    Object sol = Object();
    sol.init("cube.obj");
    sol.print();

    Object tierra = Object();
    tierra.init("roca.obj");

    if( !glfwInit() )
    {
        fprintf( stderr, "Fallo al inicializar GLFW\n" );
        getchar();
        return -1;
    }

    window = glfwCreateWindow(1024, 768, "Sistema Solar", NULL, NULL);
    if( window == NULL ) {
        fprintf( stderr, "Fallo al abrir la ventana de GLFW.\n" );
        getchar();
        glfwTerminate();
        return -1;
    }
    glfwMakeContextCurrent(window);
    glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);

    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

//  Proyecciones
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    int width, height;
    glfwGetFramebufferSize(window, &width, &height);

    float ar = width / height;

//  Proyección en paralelo
    glViewport(0, 0, width, height);
    glOrtho(-ar, ar, -1.0, 1.0, -20.0, 20.0);

//  Proyección en perspectiva
//    glFrustum(-ar, ar, -ar, ar, 2.0, 4.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    Transform Tr = Transform();
    float t_angle = 0.0f;

    do {
        glClear( GL_COLOR_BUFFER_BIT  | GL_DEPTH_BUFFER_BIT );

        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(eye[0], eye[1], eye[2],
                camera[0], camera[1], camera[2],
                0.0, 1.0, 0.0);

        // Dibujar el Sol
        arma::fmat transf = Tr.S(0.6, 0.6, 0.6);

        std::vector<Vertex> p_vertices = sol.get_faces_verts();
        std::vector<Vertex> sol_vertices;
        for ( unsigned int i=0; i<p_vertices.size(); i++ )
        {
            arma::fcolvec v = p_vertices[i].getHomg();
            arma::fcolvec vp = transf * v;
            Vertex rv = Vertex();
            rv.set_value(arma::trans(vp));
            sol_vertices.push_back(rv);
        }

        //plotWireFrameLines(sol);

        glColor3f(1.0, 1.0, 0.0);
        glBegin(GL_TRIANGLES);
        for ( unsigned int i=0; i<sol_vertices.size(); i++ ) {
            arma::frowvec vert = sol_vertices[i].get_value();
            glVertex3f(vert[0], vert[1], vert[2]);
        }
        glEnd();

        // Fin dibujado del Sol

        // Dibujar la Tierra
        t_angle = (t_angle < 360.0f) ? t_angle+5.0 : 0.0f;

        transf = Tr.R(0.0f, 1.0f, 0.0f, t_angle) * Tr.T(-0.7, 0.0, 0.0) * Tr.S(0.4, 0.4, 0.4) * transf;

        p_vertices = tierra.get_faces_verts();
        std::vector< Vertex > tierra_vertices;
        for ( unsigned int i=0; i<p_vertices.size(); i++ ) {
            arma::fcolvec v = p_vertices[i].getHomg();
            arma::fcolvec vp = transf * v;
            Vertex rv = Vertex();
            rv.set_value(arma::trans(vp));
            tierra_vertices.push_back(rv);
        }

        glColor3f(0.0, 0.0, 1.0);
        glBegin(GL_TRIANGLES);
        for ( unsigned int i=0; i<tierra_vertices.size(); i++ ) {
            arma::frowvec vert = tierra_vertices[i].get_value();
            glVertex3f(vert[0], vert[1], vert[2]);
        }
        glEnd();

        // Fin dibujado de la Tierra

        glfwSwapBuffers(window);
        glfwPollEvents();

    } while( glfwGetKey(window, GLFW_KEY_ESCAPE ) != GLFW_PRESS &&
           glfwWindowShouldClose(window) == 0 );

    glfwTerminate();

    return 0;
}