#include <iostream>
#include <string>
#include <sstream>
#include <boost/regex.hpp>
#include <cstdlib>

std::string encode ( const std::string & ) ;
std::string decode ( const std::string & ) ;

int main( ) {
   std::string to_encode ( "WWWWWWWWWWWWBWWWWWWWWWWWWBBBWWWWWWWWWWWWWWWWWWWWWWWWBWWWWWWWWWWWWWW" ) ;
   std::cout << to_encode << " encoded:" << std::endl ;
   std::string encoded ( encode ( to_encode ) ) ;
   std::cout << encoded << std::endl ;
   std::string decoded ( decode( encoded ) ) ;
   std::cout << "Decoded again:\n" ;
   std::cout << decoded << std::endl ;
   if ( to_encode == decoded )
      std::cout << "It must have worked!\n" ;
   return 0 ;
}

std::string encode( const std::string & to_encode ) {
   std::string::size_type found = 0 , nextfound = 0 ;
   std::ostringstream oss ;
   nextfound = to_encode.find_first_not_of( to_encode[ found ] , found ) ;
   while ( nextfound != std::string::npos ) {
      oss << nextfound - found ;
      oss << to_encode[ found ] ;
      found = nextfound ;
      nextfound = to_encode.find_first_not_of( to_encode[ found ] , found ) ;
   }
   //since we must not discard the last characters we add them at the end of the string
   std::string rest ( to_encode.substr( found ) ) ;//last run of characters starts at position found
   oss << rest.length( ) << to_encode[ found ] ;
   return oss.str( ) ;
}

std::string decode ( const std::string & to_decode ) {
   boost::regex e ( "(\\d+)(\\w)" ) ;
   boost::match_results<std::string::const_iterator> matches ;
   std::ostringstream oss ;
   std::string::const_iterator start = to_decode.begin( ) , end = to_decode.end( ) ;
   while ( boost::regex_search ( start , end , matches , e ) ) {
      std::string numberstring ( matches[ 1 ].first , matches[ 1 ].second ) ;
      int number = atoi( numberstring.c_str( ) ) ;
      std::string character ( matches[ 2 ].first , matches[ 2 ].second ) ;
      for ( int i = 0 ; i < number ; i++ )
	 oss << character ;
      start = matches[ 2 ].second ;
   }
   return oss.str( ) ;
}


#include "f1_datalogger/udp_logging/utils/eigen_utils.h"
#include <thread>
#include <iostream>
#include <google/protobuf/util/json_util.h>
#include <sstream>
#include "f1_datalogger/udp_logging/utils/udp_stream_utils.h"
#ifdef USE_ARMADILLO
#include <armadillo>
#endif
#include <stdexcept>
namespace deepf1
{

EigenUtils::EigenUtils()
{
}

EigenUtils::~EigenUtils()
{
}
Eigen::MatrixXd EigenUtils::loadArmaTxt(const std::string& armafile)
{
  #ifdef USE_ARMADILLO
  arma::Mat<double> arma_mat;
  if (!arma_mat.load(armafile, arma::arma_ascii))
  {
    throw std::runtime_error("Could not load arma txt file: " + armafile);
  }
  Eigen::MatrixXd rtn(Eigen::Map<Eigen::MatrixXd>(arma_mat.memptr(), arma_mat.n_rows, arma_mat.n_cols));
  return rtn;
  #else
  throw std::runtime_error("This feature only works with the armadillo library. Recompile with the WITH_ARMA option turned on");
  #endif
}
deepf1::protobuf::eigen::Pose3d EigenUtils::eigenToProto(const Eigen::Affine3d& poseEigen, const double& session_time, deepf1::protobuf::eigen::FrameId frameid)
{
  deepf1::protobuf::eigen::Pose3d rtn;

  Eigen::Vector3d translation(poseEigen.translation());
  Eigen::Quaterniond rotation(poseEigen.rotation());

  rtn.mutable_translation()->set_x(translation.x());
  rtn.mutable_translation()->set_y(translation.y());
  rtn.mutable_translation()->set_y(translation.z());

  rtn.mutable_rotation()->set_x(rotation.x());
  rtn.mutable_rotation()->set_y(rotation.y());
  rtn.mutable_rotation()->set_y(rotation.z());
  rtn.mutable_rotation()->set_w(rotation.w());

  rtn.set_frame(frameid);
  rtn.set_session_time(session_time);

  return rtn;
}
Eigen::Affine3d EigenUtils::protoToEigen(const deepf1::protobuf::eigen::Pose3d& poseProto)
{
  Eigen::Affine3d poseEigen;
  Eigen::Vector3d translation(poseProto.translation().x(), poseProto.translation().y(), poseProto.translation().z());
  Eigen::Quaterniond rotation(poseProto.rotation().w(), poseProto.rotation().x(), poseProto.rotation().y(), poseProto.rotation().z());
  poseEigen.fromPositionOrientationScale(translation, rotation, Eigen::Vector3d::Ones());
  return poseEigen;
}
Eigen::Affine3d EigenUtils::interpPoses(const Eigen::Affine3d& a, const Eigen::Affine3d& b, const double& s)
{
	Eigen::Affine3d rtn;
	Eigen::Vector3d translationA(a.translation());
	Eigen::Quaterniond rotationA(a.rotation());
	Eigen::Vector3d translationB(b.translation());
	Eigen::Quaterniond rotationB(b.rotation());

	Eigen::Vector3d translationOut = (1 - s) * translationA + s * translationB;
	Eigen::Quaterniond rotationOut = rotationA.slerp(s, rotationB);

	rtn.fromPositionOrientationScale(translationOut, rotationOut, Eigen::Vector3d::Ones());

	return rtn;
}
Eigen::Affine3d EigenUtils::motionPacketToPose(const deepf1::twenty_eighteen::CarMotionData& motion_packet)
{
	const deepf1::twenty_eighteen::protobuf::CarMotionData& motion_packet_pb =
		deepf1::twenty_eighteen::TwentyEighteenUDPStreamUtils::toProto(motion_packet);
	return motionPacketToPose(motion_packet_pb);
}
Eigen::Affine3d EigenUtils::motionPacketToPose(const deepf1::twenty_eighteen::protobuf::CarMotionData& motion_packet)
{
	Eigen::Affine3d rtn;
	Eigen::Vector3d translation(motion_packet.m_worldpositionx(), motion_packet.m_worldpositiony(), motion_packet.m_worldpositionz());

	Eigen::Vector3d forward(motion_packet.m_worldforwarddirx(), motion_packet.m_worldforwarddiry(), motion_packet.m_worldforwarddirz());
	forward.normalize();
	Eigen::Vector3d right(motion_packet.m_worldrightdirx(), motion_packet.m_worldrightdiry(), motion_packet.m_worldrightdirz());
	right.normalize();
	Eigen::Vector3d up = right.cross(forward);
	up.normalize();
	Eigen::Matrix3d rotationMat(Eigen::Matrix3d::Identity());
	rotationMat.col(0) = -right;
	rotationMat.col(1) = up;
	rotationMat.col(2) = forward;
	Eigen::Quaterniond rotation(rotationMat);

	rtn.fromPositionOrientationScale(translation, rotation, Eigen::Vector3d::Ones());

	return rtn;
}
Eigen::MatrixXd EigenUtils::vectorToMatrix(const std::vector < Eigen::Vector4d >& vector)
{
	Eigen::MatrixXd rtnMat(4, vector.size());
	/**/
	rtnMat.resize(4, vector.size());
	unsigned int idx = 0;
	std::for_each(vector.begin(), vector.end(), [&rtnMat, &idx](const Eigen::Vector4d & point)
	{
		rtnMat.col(idx) = point;
		idx++;
	});
	return rtnMat;
}

}


/*******************************************************************************
 * An array content domain.
 *
 * This domain is a simplified implementation of the paper:
 * "A Partial-Order Approach to Array Content Analysis" by
 *  Gange, Navas, Schachte, Sondergaard, and Stuckey
 *  available here http://arxiv.org/pdf/1408.1754v1.pdf.
 *
 * It keeps a graph where vertices are array indexes and edges are
 * labelled with weights.  An edge (i,j) with weight w denotes that
 * the property w holds for the array segment [i,j). A weight is an
 * arbitrary lattice that can relate multiple array variables as well
 * as array with scalar variables.
 ******************************************************************************/

/* IMPORTANT
 * JN: the implementation works for toy programs but two main things
 * must to be done for being able to analyze real programs:
 *
 * 1) landmarks must be kept as local state as part of each abstract
 *    state.
 *
 * 2) reduction between scalar and weight domains must be done
 *    incrementally. For that, we need some assumptions about the
 *    underlying scalar domain. For instance, if we assume zones then
 *    after each operation we know which are the indexes affected by
 *    the operation. We can use that information for doing reduction
 *    only on those indexes. This would remove the need of having
 *    methods such as array_sgraph_domain_traits::is_unsat and
 *    array_sgraph_domain_traits::active_variables which are anyway
 *    domain dependent.
 */

#ifndef ARRAY_SPARSE_GRAPH_HPP
#define ARRAY_SPARSE_GRAPH_HPP

#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/domains/operators_api.hpp>
#include <crab/domains/domain_traits.hpp>
#include <crab/domains/array_sparse_graph/array_segmentation.hpp>
#include <crab/domains/array_sparse_graph/array_graph_ops.hpp>
#include <crab/domains/graphs/adapt_sgraph.hpp>
#include <crab/domains/graphs/sparse_graph.hpp>
#include <crab/domains/linear_constraints.hpp>
#include <crab/domains/intervals.hpp>

// XXX: if expression domain is a template parameter no need to include
#include <crab/domains/term_equiv.hpp>
// XXX: for customized propagations between weight and scalar domains
#include <crab/domains/combined_domains.hpp>
#include <crab/domains/nullity.hpp>

#include <boost/unordered_map.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/join.hpp>
#include <boost/iterator/transform_iterator.hpp>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-compare"

namespace crab {

  namespace domains {

    /*
       A weighted directed graph where the weight is an abstract
       domain. The graph should be always kept in a consistent form,
       i.e., for all i,j,k:: weight(i,j) <= join(weight(i,k), weight(k,j))
    */
    template< typename Vertex, typename Weight, bool IsDistWeight >
    class array_sparse_graph_: public writeable {

     public:

      // XXX: make this a template parameter later
      //typedef AdaptGraph<Weight> graph_t;
      typedef SparseWtGraph<Weight> graph_t;
      typedef typename graph_t::vert_id _vert_id;
      typedef typename graph_t::edge_ref_t edge_ref_t;
      typedef typename graph_t::Wt Wt;
      typedef typename graph_t::mut_val_ref_t mut_val_ref_t;
      // XXX: needs to have this typedef so we can use GraphRev
      typedef Vertex vert_id;

     private:

      typedef GraphPerm<graph_t> GrPerm;
      typedef ArrayGrOps<graph_t, IsDistWeight> GrOps;
      typedef typename GrOps::Wt_join Wt_join;
      typedef typename GrOps::Wt_meet Wt_meet;

      typedef boost::container::flat_map<Vertex, _vert_id> vert_map_t;
      typedef typename vert_map_t::value_type vmap_elt_t;
      typedef std::vector<boost::optional<Vertex> > rev_map_t;
      typedef std::unordered_set<_vert_id> vert_set_t;
      typedef array_sparse_graph_<Vertex, Weight, IsDistWeight> array_sparse_graph_t;

      vert_map_t _vert_map;
      rev_map_t _rev_map;
      graph_t _g;
      vert_set_t _unstable;
      bool _is_bottom;

      struct vert_set_wrap_t {
        vert_set_wrap_t(const vert_set_t& _vs)
            : vs(_vs) { }

        bool operator[](_vert_id v) const {
          return vs.find(v) != vs.end();
        }
        const vert_set_t& vs;
      };

      _vert_id get_vert(Vertex v)
      {
        auto it = _vert_map.find(v);
        if(it != _vert_map.end())
          return (*it).second;

        _vert_id vert(_g.new_vertex());
        assert(vert <= _rev_map.size());

        if(vert < _rev_map.size()) {
          assert(!_rev_map[vert]);
          _rev_map[vert] = v;
        } else {
          _rev_map.push_back(v);
        }
        _vert_map.insert(vmap_elt_t(v, vert));

        return vert;
      }

     public:

      template<class ItS>
      class iterator {
       public:
        typedef iterator<ItS> iter_t;
        iterator(const ItS& _it, const rev_map_t& _rev_map)
            : it(_it), rev_map(_rev_map) { }
        bool operator!=(const iter_t& o)
        { return it != o.it; }
        iter_t& operator++(void) { ++it; return *this; }
        Vertex operator*(void) const {
          if (!rev_map[*it]) CRAB_ERROR("Reverse map failed");
          return *(rev_map[*it]);
        }
       protected:
        ItS it;
        const rev_map_t& rev_map;
      };

      struct edge_t {
        edge_t(Vertex _v, Wt& _w) : vert(_v), val(_w) { }
        Vertex vert;
        Wt& val;
      };

      template<class ItS>
      class edge_iterator {
       public:
        typedef edge_iterator<ItS> iter_t;
        edge_iterator(const ItS& _it, const rev_map_t& _rev_map)
            : it(_it), rev_map(_rev_map) { }
        bool operator!=(const iter_t& o)
        { return it != o.it; }
        iter_t& operator++(void) { ++it; return *this; }
        edge_t operator*(void) const {
          edge_ref_t e = *it;
          if (!rev_map[e.vert]) CRAB_ERROR("Reverse map failed");
          Vertex v = *(rev_map[e.vert]);
          return edge_t(v, e.val);
        }
       protected:
        ItS it;
        const rev_map_t& rev_map;
      };

      template<class Range, class ItS>
      class iterator_range {
       public:
        typedef ItS iterator;
        iterator_range(const Range &r, const rev_map_t &rev_map)
            : _r(r), _rev_map(rev_map) { }
        iterator begin(void) const { return iterator(_r.begin(), _rev_map); }
        iterator end(void) const { return iterator(_r.end(), _rev_map); }
       protected:
        Range _r;
        const rev_map_t &_rev_map;
      };

      typedef iterator<typename graph_t::succ_iterator> succ_transform_iterator;
      typedef iterator<typename graph_t::pred_iterator> pred_transform_iterator;
      typedef iterator<typename graph_t::vert_iterator> vert_transform_iterator;
      typedef edge_iterator<typename graph_t::fwd_edge_iterator> fwd_edge_transform_iterator;
      typedef edge_iterator<typename graph_t::rev_edge_iterator> rev_edge_transform_iterator;

      typedef iterator_range<typename graph_t::succ_range, succ_transform_iterator> succ_range;
      typedef iterator_range<typename graph_t::pred_range, pred_transform_iterator> pred_range;
      typedef iterator_range<typename graph_t::vert_range, vert_transform_iterator> vert_range;
      typedef iterator_range<typename graph_t::e_succ_range, fwd_edge_transform_iterator>
      e_succ_range;
      typedef iterator_range<typename graph_t::e_pred_range, rev_edge_transform_iterator>
      e_pred_range;

      vert_range verts() {
        typename graph_t::vert_range p = _g.verts();
        return vert_range(p, _rev_map);
      }

      succ_range succs(Vertex v) {
        typename graph_t::succ_range p = _g.succs(get_vert(v));
        return succ_range(p, _rev_map);
      }

      pred_range preds(Vertex v) {
        typename graph_t::pred_range p = _g.preds(get_vert(v));
        return pred_range(p, _rev_map);
      }

      e_succ_range e_succs(Vertex v) {
        typename graph_t::e_succ_range p = _g.e_succs(get_vert(v));
        return e_succ_range(p, _rev_map);
      }

      e_pred_range e_preds(Vertex v) {
        typename graph_t::e_pred_range p = _g.e_preds(get_vert(v));
        return e_pred_range(p, _rev_map);
      }

     public:

      array_sparse_graph_(bool is_bottom = false)
          : writeable(), _is_bottom(is_bottom)
      { }

      array_sparse_graph_(const array_sparse_graph_t& o)
          : writeable(),
            _vert_map(o._vert_map), _rev_map(o._rev_map), _g(o._g),
            _unstable(o._unstable), _is_bottom (false)
      {
        if (o._is_bottom)
          set_to_bottom();
      }

      array_sparse_graph_(array_sparse_graph_t&& o)
          : _vert_map(std::move(o._vert_map)), _rev_map(std::move(o._rev_map)),
            _g(std::move(o._g)), _unstable(std::move(o._unstable)), _is_bottom(o._is_bottom)
      { }

      array_sparse_graph_(vert_map_t& vert_map, rev_map_t& rev_map, graph_t& g,
			  vert_set_t unstable)
	: writeable(),
	  _vert_map(vert_map), _rev_map(rev_map), _g(g),
	  _unstable(unstable), _is_bottom(false)
      { }

      array_sparse_graph_(vert_map_t&& vert_map, rev_map_t&& rev_map, graph_t&& g,
			  vert_set_t &&unstable)
          : writeable(),
            _vert_map(std::move(vert_map)), _rev_map(std::move(rev_map)), _g(std::move(g)),
            _unstable(std::move(unstable)), _is_bottom(false)
      { }

      array_sparse_graph_t& operator=(const array_sparse_graph_t& o)
      {
        if(this != &o)
        {
          if(o._is_bottom)
            set_to_bottom();
          else {
            _is_bottom = false;
            _vert_map = o._vert_map;
            _rev_map = o._rev_map;
            _g = o._g;
            _unstable = o._unstable;
          }
        }
        return *this;
      }

      array_sparse_graph_t& operator=(array_sparse_graph_t&& o)
      {
        if(o._is_bottom) {
          set_to_bottom();
        } else {
          _is_bottom = false;
          _vert_map = std::move(o._vert_map);
          _rev_map = std::move(o._rev_map);
          _unstable = std::move(o._unstable);
          _g = std::move(o._g);
        }
        return *this;
      }

     public:

      void set_to_bottom() {
        _vert_map.clear();
        _rev_map.clear();
        _g.clear();
        _unstable.clear();
        _is_bottom = true;
      }

      static array_sparse_graph_t top() { return array_sparse_graph_t(false); }

      static array_sparse_graph_t bottom() { return array_sparse_graph_t(true); }

      bool is_bottom() const { return _is_bottom; }

      bool is_top() {
        if(_is_bottom)
          return false;
        return _g.is_empty();
      }

      bool lookup_edge(Vertex s, Vertex d, mut_val_ref_t* w) {
        if (is_bottom()) return false;
        auto se = get_vert(s);
        auto de = get_vert(d);
        return _g.lookup(se, de, w);
      }

      // // update edge but do not close graph
      // void update_edge_unclosed (Vertex s, Weight w, Vertex d) {
      //   if (w.is_top()) return;
      //   normalize();
      //   if (is_bottom ()) return;
      //   auto se = get_vert(s);
      //   auto de = get_vert(d);
      //   Wt_meet op;
      //   _g.update_edge(se, w, de, op);
      // }

      // close the graph after edge (s,d) has been updated
      void close_edge (Vertex s, Vertex d) {
        normalize();
        if (is_bottom ()) return;
        auto se = get_vert(s);
        auto de = get_vert(d);
        GrOps::close_after_edge(_g, se, de);
      }

      void update_edge (Vertex s, Weight w, Vertex d) {
        if (w.is_top())
          return;

        normalize();

        if (is_bottom ())
          return;

        auto se = get_vert(s);
        auto de = get_vert(d);
        Wt_meet op;
        _g.update_edge(se, w, de, op);
        GrOps::close_after_edge(_g, se, de);
      }

      // void full_close () { // for debugging
      //   if (is_bottom ()) return;
      //   GrOps::floyd_warshall(_g);
      // }

      void expand (Vertex s, Vertex d) {
        if(is_bottom())
          return;

        auto it = _vert_map.find(d);
        if(it != _vert_map.end()) {
          CRAB_ERROR("array_sparse_graph expand failed because vertex ", d, " already exists");
        }

        auto se = get_vert(s);
        auto de = get_vert(d);

        for (auto edge : _g.e_preds(se))
          _g.add_edge (edge.vert, edge.val, de);

        for (auto edge : _g.e_succs(se))
          _g.add_edge (de, edge.val, edge.vert);

      }

      void normalize() {
        #if 0
        GrOps::closure(_g); // only for debugging purposes
        #else
        // Always maintained in closed form except for widening
        if(_unstable.size() == 0)
          return;
        GrOps::close_after_widen(_g, vert_set_wrap_t(_unstable));
        _unstable.clear();
        #endif
      }

      void operator|=(array_sparse_graph_t& o) {
        *this = *this | o;
      }

      bool operator<=(array_sparse_graph_t& o)  {
        if (is_bottom())
          return true;
        else if(o.is_bottom())
          return false;
        else if (o.is_top ())
          return true;
        else if (is_top ())
          return false;
        else {
          normalize();

          if(_vert_map.size() < o._vert_map.size())
            return false;

          // Set up a mapping from o to this.
          std::vector<unsigned int> vert_renaming(o._g.size(),-1);
          for(auto p : o._vert_map)
          {
            auto it = _vert_map.find(p.first);
            // We can't have this <= o if we're missing some
            // vertex.
            if(it == _vert_map.end())
              return false;
            vert_renaming[p.second] = (*it).second;
          }

          assert(_g.size() > 0);
          mut_val_ref_t wx;

          for(_vert_id ox : o._g.verts()) {
            assert(vert_renaming[ox] != -1);
            _vert_id x = vert_renaming[ox];
            for(auto edge : o._g.e_succs(ox)) {
              _vert_id oy = edge.vert;
              assert(vert_renaming[ox] != -1);
              _vert_id y = vert_renaming[oy];
              auto ow = (Weight) edge.val;
              if(!_g.lookup(x, y, &wx) || (! ((Weight) wx <= ow)))
                return false;
            }
          }
          return true;
        }
      }

      array_sparse_graph_t operator|(array_sparse_graph_t& o) {

        if (is_bottom() || o.is_top ())
          return o;
        else if (is_top () || o.is_bottom())
          return *this;
        else {
          CRAB_LOG ("array-sgraph",
                    crab::outs() << "Before join:\n"<<"Graph 1\n"<<*this
                                 <<"\n"<<"Graph 2\n"<<o << "\n");

          normalize();
          o.normalize();

          // Figure out the common renaming.
          std::vector<_vert_id> perm_x;
          std::vector<_vert_id> perm_y;

          vert_map_t out_vmap;
          rev_map_t out_revmap;

          for(auto p : _vert_map)
          {
            auto it = o._vert_map.find(p.first);
            // Vertex exists in both
            if(it != o._vert_map.end())
            {
              out_vmap.insert(vmap_elt_t(p.first, perm_x.size()));
              out_revmap.push_back(p.first);

              perm_x.push_back(p.second);
              perm_y.push_back((*it).second);
            }
          }

          // Build the permuted view of x and y.
          assert(_g.size() > 0);
          GrPerm gx(perm_x, _g);
          assert(o._g.size() > 0);
          GrPerm gy(perm_y, o._g);

          // We now have the relevant set of relations. Because g_rx
          // and g_ry are closed, the result is also closed.
          graph_t join_g(GrOps::join(gx, gy));

          // Now garbage collect any unused vertices
          for(_vert_id v : join_g.verts())
          {
            if(join_g.succs(v).size() == 0 && join_g.preds(v).size() == 0)
            {
              join_g.forget(v);
              if(out_revmap[v])
              {
                out_vmap.erase(*(out_revmap[v]));
                out_revmap[v] = boost::none;
              }
            }
          }

          array_sparse_graph_t res(std::move(out_vmap), std::move(out_revmap),
                                   std::move(join_g), vert_set_t());
          CRAB_LOG ("array-sgraph", crab::outs() << "Result join:\n"<< res <<"\n";);
          return res;
        }
      }

      template<typename Thresholds>
      array_sparse_graph_t widening_thresholds (array_sparse_graph_t &o,
                                                const Thresholds & /*ts*/) {
        return (*this || o);
      }

      array_sparse_graph_t operator||(array_sparse_graph_t &o) {
        if (is_bottom())
          return o;
        else if (o.is_bottom())
          return *this;
        else {
          CRAB_LOG ("array-sgraph",
                    crab::outs() << "Before widening:\n"<<"Graph 1\n"<<*this
                                 <<"\n"<<"Graph 2\n"<<o<<"\n";);
          o.normalize();

          // Figure out the common renaming
          std::vector<_vert_id> perm_x;
          std::vector<_vert_id> perm_y;
          vert_map_t out_vmap;
          rev_map_t out_revmap;
          vert_set_t widen_unstable(_unstable);

          for(auto p : _vert_map)
          {
            auto it = o._vert_map.find(p.first);
            // Vertex exists in both
            if(it != o._vert_map.end())
            {
              out_vmap.insert(vmap_elt_t(p.first, perm_x.size()));
              out_revmap.push_back(p.first);

              perm_x.push_back(p.second);
              perm_y.push_back((*it).second);
            }
          }

          // Build the permuted view of x and y.
          //assert(_g.size() > 0);
          GrPerm gx(perm_x, _g);
          //assert(o._g.size() > 0);
          GrPerm gy(perm_y, o._g);

          // Now perform the widening
          std::vector<_vert_id> destabilized;
          graph_t widen_g(GrOps::widen(gx, gy, destabilized));
          for(_vert_id v : destabilized) {
            widen_unstable.insert(v);
          }

          array_sparse_graph_t res(std::move(out_vmap), std::move(out_revmap),
                                   std::move(widen_g), std::move(widen_unstable));
          CRAB_LOG ("array-sgraph", crab::outs() << "Result widening:\n"<<res<<"\n";);
          return res;
        }
      }

      array_sparse_graph_t meet_or_narrowing(array_sparse_graph_t &o, bool is_meet,
					     const std::string op) {

        if (is_bottom() || o.is_bottom())
          return bottom();
        else if (is_top())
          return o;
        else if (o.is_top())
          return *this;
        else {
          CRAB_LOG ("array-sgraph",
                    crab::outs() << "Before " << op << ":\n"<<"Graph 1\n"<<*this<<"\n"
                                 <<"Graph 2\n"<<o << "\n");

          normalize();
          o.normalize();

          // Figure out the common renaming.
          std::vector<_vert_id> perm_x;
          std::vector<_vert_id> perm_y;

          vert_map_t out_vmap;
          rev_map_t out_revmap;

          for(auto p : _vert_map)
          {
            _vert_id vv = perm_x.size();
            out_vmap.insert(vmap_elt_t(p.first, vv));
            out_revmap.push_back(p.first);

            perm_x.push_back(p.second);
            perm_y.push_back(-1);
          }

          // Add missing mappings from the right operand.
          for(auto p : o._vert_map)
          {
            auto it = out_vmap.find(p.first);
            if(it == out_vmap.end())
            {
              _vert_id vv = perm_y.size();
              out_revmap.push_back(p.first);

              perm_y.push_back(p.second);
              perm_x.push_back(-1);
              out_vmap.insert(vmap_elt_t(p.first, vv));
            } else {
              perm_y[(*it).second] = p.second;
            }
          }

          // Build the permuted view of x and y.
          GrPerm gx(perm_x, _g);
          GrPerm gy(perm_y, o._g);

          // Compute the syntactic meet/narrowing of the permuted graphs.
          std::vector<_vert_id> changes;
          graph_t out_g(GrOps::meet_or_narrowing(gx, gy, is_meet, changes));
          vert_set_t unstable;
          for(_vert_id v : changes)
            unstable.insert(v);

          GrOps::close_after_meet_or_narrowing(_g, vert_set_wrap_t(unstable));

          array_sparse_graph_t res(std::move(out_vmap), std::move(out_revmap),
                                   std::move(out_g), vert_set_t());
          CRAB_LOG ("array-sgraph", crab::outs() << "Result " << op << ":\n"<< res <<"\n";);
          return res;
        }
      }

      array_sparse_graph_t operator&(array_sparse_graph_t& o) {
        return meet_or_narrowing(o, true, "meet");
      }

      array_sparse_graph_t operator&&(array_sparse_graph_t& o) {
        return meet_or_narrowing(o, false, "narrowing");
      }

      // array_sparse_graph_t operator&(array_sparse_graph_t& o) {

      //   if (is_bottom() || o.is_bottom())
      //     return bottom();
      //   else if (is_top())
      //     return o;
      //   else if (o.is_top())
      //     return *this;
      //   else {
      //     CRAB_LOG ("array-sgraph",
      //               crab::outs() << "Before meet:\n"<<"Graph 1\n"<<*this<<"\n"
      //                            <<"Graph 2\n"<<o << "\n");

      //     normalize();
      //     o.normalize();

      //     // Figure out the common renaming.
      //     std::vector<_vert_id> perm_x;
      //     std::vector<_vert_id> perm_y;

      //     vert_map_t out_vmap;
      //     rev_map_t out_revmap;

      //     for(auto p : _vert_map)
      //     {
      //       _vert_id vv = perm_x.size();
      //       out_vmap.insert(vmap_elt_t(p.first, vv));
      //       out_revmap.push_back(p.first);

      //       perm_x.push_back(p.second);
      //       perm_y.push_back(-1);
      //     }

      //     // Add missing mappings from the right operand.
      //     for(auto p : o._vert_map)
      //     {
      //       auto it = out_vmap.find(p.first);
      //       if(it == out_vmap.end())
      //       {
      //         _vert_id vv = perm_y.size();
      //         out_revmap.push_back(p.first);

      //         perm_y.push_back(p.second);
      //         perm_x.push_back(-1);
      //         out_vmap.insert(vmap_elt_t(p.first, vv));
      //       } else {
      //         perm_y[(*it).second] = p.second;
      //       }
      //     }

      //     // Build the permuted view of x and y.
      //     //assert(_g.size() > 0);
      //     GrPerm gx(perm_x, _g);
      //     //assert(o._g.size() > 0);
      //     GrPerm gy(perm_y, o._g);

      //     // Compute the syntactic meet of the permuted graphs.
      //     std::vector<_vert_id> changes;
      //     graph_t meet_g(GrOps::meet_or_narrowing(gx, gy, true /*meet*/, changes));
      //     vert_set_t unstable;
      //     for(_vert_id v : changes)
      //       unstable.insert(v);

      //     GrOps::close_after_meet_or_narrowing(_g, vert_set_wrap_t(unstable));

      //     array_sparse_graph_t res(std::move(out_vmap), std::move(out_revmap),
      //                              std::move(meet_g), vert_set_t());
      //     CRAB_LOG ("array-sgraph", crab::outs() << "Result meet:\n"<< res <<"\n";);
      //     return res;
      //   }
      // }

      // array_sparse_graph_t operator&&(array_sparse_graph_t& o) {
      //   if (is_bottom() || o.is_bottom())
      //     return bottom();
      //   else if (is_top ())
      //     return o;
      //   else{
      //     CRAB_LOG ("array-sgraph",
      //               crab::outs() << "Before narrowing:\n"<<"Graph 1\n"<<*this<<"\n"
      //                            <<"Graph 2\n"<<o<<"\n";);

      //     // Narrowing as a no-op should be sound.
      //     normalize();
      //     array_sparse_graph_t res(*this);

      //     CRAB_LOG ("array-sgraph",
      //               crab::outs() << "Result narrowing:\n" << res<<"\n";);
      //     return res;
      //   }
      // }

      void operator-=(Vertex v) {
        if (is_bottom ())
          return;
        auto it = _vert_map.find (v);
        if (it != _vert_map.end ()) {
          normalize();
          _g.forget(it->second);
          _rev_map[it->second] = boost::none;
          _vert_map.erase(v);
        }
      }

      void remove_from_weights(typename Weight::varname_t v) {
        mut_val_ref_t w_pq;
        for(auto p : _g.verts())
          for(auto e : _g.e_succs(p)) {
            auto q = e.vert;
            if (_g.lookup(p, q, &w_pq)) {
              Weight w = (Weight) w_pq;
              w -= v;
              Wt_meet op;
              _g.update_edge(p, w, q, op);
              GrOps::close_after_edge(_g, p, q);
            }
          }
      }

      void write(crab_os& o) {
        write(o, true);
      }

      void write(crab_os& o, bool print_bottom_edges) {

        normalize ();

        if(is_bottom()){
          o << "_|_";
          return;
        }
        else if (is_top()){
          o << "{}";
          return;
        }
        else
        {
          bool first = true;
          o << "{";
          for(_vert_id s : _g.verts())
          {
            if(!_rev_map[s]) continue;

            auto vs = *_rev_map[s];
            for(_vert_id d : _g.succs(s))
            {
              if(!_rev_map[d]) continue;

              auto w = _g.edge_val(s, d);
              if (!print_bottom_edges && w.is_bottom())
                continue; // do not print bottom edges

              auto vd = *_rev_map[d];

              if(first)
                first = false;
              else
                o << ", ";
              o << "[" << vs << "," << vd << ")=>" << w;
            }
          }
          o << "}";
        }
      }
    };

    namespace array_sparse_graph_impl {

      // JN: I do not know how to propagate arbitrary invariants
      // between weight and scalar domains in a domain-independent
      // manner. Here, we define propagations between specific
      // domains.

      template <typename Dom>
      interval<typename Dom::number_t>
      eval_interval(Dom dom, typename Dom::linear_expression_t e)
      {
        interval<typename Dom::number_t>  r = e.constant();
        for (auto p : e)
          r += p.first * dom[p.second.name()];
        return r;
      }

      template<typename Dom1, typename Dom2>
      void propagate_between_weight_and_scalar(Dom1 src, typename Dom1::varname_t src_var,
                                               variable_type ty,
                                               Dom2 &dst, typename Dom2::varname_t dst_var) {
        if (ty == ARR_INT_TYPE) {
          // --- XXX: simplification wrt Gange et.al.:
          //     Only non-relational numerical invariants are
          //     propagated from the graph domain to the scalar domain.
          dst.set (dst_var, eval_interval(src, typename Dom1::variable_t(src_var)));
        } else {
          CRAB_WARN ("Missing propagation between weight and scalar domains");
        }
      }

      template<typename BaseDom>
      void propagate_between_weight_and_scalar(numerical_nullity_domain<BaseDom> src,
                                               typename BaseDom::varname_t src_var,
                                               variable_type ty,
                                               numerical_nullity_domain<BaseDom> &dst,
                                               typename BaseDom::varname_t dst_var) {
        if (ty == ARR_INT_TYPE) {
          // --- XXX: simplification wrt Gange et.al.:
          //     Only non-relational numerical invariants are
          //     propagated from the graph domain to the scalar domain.
          dst.set (dst_var, eval_interval(src, typename BaseDom::variable_t(src_var)));
        } else if (ty == ARR_PTR_TYPE) {
          auto &null_dom = dst.second ();
          null_dom.set_nullity (dst_var, src.get_nullity (src_var));
        } else {
          CRAB_WARN ("Missing propagation between weight and scalar domains");
        }
      }

      template<typename BaseDom>
      void propagate_between_weight_and_scalar(nullity_domain<typename BaseDom::number_t,
                                                              typename BaseDom::varname_t> src,
                                               typename BaseDom::varname_t src_var,
                                               variable_type ty,
                                               numerical_nullity_domain<BaseDom> &dst,
                                               typename BaseDom::varname_t dst_var) {
        if (ty == ARR_INT_TYPE) {
          // do nothing
        } else if (ty == ARR_PTR_TYPE) {
          auto &null_dom = dst.second ();
          null_dom.set_nullity (dst_var, src.get_nullity (src_var));
        } else {
          CRAB_WARN ("Missing propagation between weight and scalar domains");
        }
      }

      template<typename BaseDom>
      void propagate_between_weight_and_scalar(numerical_nullity_domain<BaseDom> src,
                                               typename BaseDom::varname_t src_var,
                                               variable_type ty,
                                               nullity_domain<typename BaseDom::number_t,
                                                              typename BaseDom::varname_t> &dst,
                                               typename BaseDom::varname_t dst_var) {
        if (ty == ARR_INT_TYPE) {
          // do nothing
        } else if (ty == ARR_PTR_TYPE) {
          dst.set_nullity (dst_var, src.second ().get_nullity (src_var));
        } else {
          CRAB_WARN ("Missing propagation between weight and scalar domains");
        }
      }

    } /* end array_sparse_graph_impl */

    // Wrapper which uses shared references with copy-on-write.
    template<class Vertex, class Weight, bool IsDistWeight>
    class array_sparse_graph : public writeable {
      public:

      typedef array_sparse_graph_<Vertex, Weight, IsDistWeight> array_sgraph_impl_t;
      typedef std::shared_ptr<array_sgraph_impl_t> array_sgraph_ref_t;
      typedef array_sparse_graph<Vertex, Weight, IsDistWeight> array_sgraph_t;

      typedef typename array_sgraph_impl_t::Wt Wt;
      typedef typename array_sgraph_impl_t::mut_val_ref_t mut_val_ref_t;
      typedef typename array_sgraph_impl_t::graph_t graph_t;
      typedef typename array_sgraph_impl_t::vert_id vert_id;
      typedef typename array_sgraph_impl_t::succ_range succ_range;
      typedef typename array_sgraph_impl_t::pred_range pred_range;
      typedef typename array_sgraph_impl_t::vert_range vert_range;
      typedef typename array_sgraph_impl_t::e_succ_range e_succ_range;
      typedef typename array_sgraph_impl_t::e_pred_range e_pred_range;

      array_sparse_graph(array_sgraph_ref_t _ref) : norm_ref(_ref) { }

      array_sparse_graph(array_sgraph_ref_t _base, array_sgraph_ref_t _norm)
        : base_ref(_base), norm_ref(_norm) { }

      array_sgraph_t create(array_sgraph_impl_t&& t)
      {
        return std::make_shared<array_sgraph_impl_t>(std::move(t));
      }

      array_sgraph_t create_base(array_sgraph_impl_t&& t)
      {
        array_sgraph_ref_t base = std::make_shared<array_sgraph_impl_t>(t);
        array_sgraph_ref_t norm = std::make_shared<array_sgraph_impl_t>(std::move(t));
        return array_sgraph_t(base, norm);
      }

      void lock(void)
      { // Allocate a fresh copy.
        if(!norm_ref.unique())
          norm_ref = std::make_shared<array_sgraph_impl_t>(*norm_ref);
        base_ref.reset();
      }

    public:

      static array_sgraph_t top() { return array_sparse_graph(false); }

      static array_sgraph_t bottom() { return array_sparse_graph(true); }

      array_sparse_graph(bool is_bottom = false)
        : norm_ref(std::make_shared<array_sgraph_impl_t>(is_bottom)) { }

      array_sparse_graph(const array_sgraph_t& o)
        : base_ref(o.base_ref), norm_ref(o.norm_ref)
      { }

      array_sgraph_t& operator=(const array_sgraph_t& o) {
        base_ref = o.base_ref;
        norm_ref = o.norm_ref;
        return *this;
      }

      array_sgraph_impl_t& base(void) {
        if(base_ref)
          return *base_ref;
        else
          return *norm_ref;
      }

      array_sgraph_impl_t& norm(void) { return *norm_ref; }
      const array_sgraph_impl_t& norm(void) const { return *norm_ref; }

      bool is_bottom() { return norm().is_bottom(); }

      bool is_top() { return norm().is_top(); }

      bool operator<=(array_sgraph_t& o) { return norm() <= o.norm(); }

      void operator|=(array_sgraph_t o) { lock(); norm() |= o.norm(); }

      array_sgraph_t operator|(array_sgraph_t o) { return create(norm() | o.norm()); }

      array_sgraph_t operator||(array_sgraph_t o) { return create_base(base() || o.norm()); }

      array_sgraph_t operator&(array_sgraph_t o) { return create(norm() & o.norm()); }

      array_sgraph_t operator&&(array_sgraph_t o) { return create(norm() && o.norm()); }

      template<typename Thresholds>
      array_sgraph_t widening_thresholds (array_sgraph_t o, const Thresholds &ts) {
        return create_base(base().template widening_thresholds<Thresholds>(o.norm(), ts));
      }

      void normalize() { lock(); norm().normalize(); }

      vert_range verts () { return norm().verts(); }

      succ_range succs (Vertex v) { return norm().succs(v); }

      pred_range preds (Vertex v) { return norm().preds(v); }

      e_succ_range e_succs (Vertex v) { return norm().e_succs(v); }

      e_pred_range e_preds (Vertex v) { return norm().e_preds(v); }

      void set_to_bottom() { lock(); norm().set_to_bottom(); }

      bool lookup_edge(Vertex s, Vertex d, mut_val_ref_t* w)
      { lock(); return norm().lookup_edge(s,d,w); }

      void expand(Vertex s, Vertex d)
      { lock(); norm().expand(s,d); }

      void update_edge (Vertex s, Weight w, Vertex d) { lock(); norm().update_edge(s,w,d); }

      // void full_close () { lock(); norm().full_close(); }
      // void update_edge_unclosed (Vertex s, Weight w, Vertex d) { lock(); norm().update_edge_unclosed(s,w,d); }
      void close_edge (Vertex s, Vertex d) { lock(); norm().close_edge(s,d); }

      void remove_from_weights(typename Weight::varname_t v)
      { lock(); norm().remove_from_weights(v);}

      void operator-=(Vertex v) { lock(); norm() -= v; }

      void write(crab_os& o) { norm().write(o); }
      void write(crab_os& o, bool print_bottom_edges) { norm().write(o, print_bottom_edges); }

    protected:
      array_sgraph_ref_t base_ref;
      array_sgraph_ref_t norm_ref;
    };

    // Another C++ datatype to wrap variables and numbers as graph
    // vertices.

    enum landmark_kind_t { LMC, LMV, LMVP};
    template<class V, class N>
    class landmark {
     protected:
      landmark_kind_t _kind;
      landmark(landmark_kind_t kind): _kind(kind) { }
     public:
      virtual ~landmark() { }
      landmark_kind_t kind() const { return _kind;}
      virtual bool operator==(const landmark<V,N>& o) const = 0;
      virtual bool operator<(const landmark<V,N>& o) const = 0;
      virtual void write(crab_os&o) const = 0;
      virtual std::size_t hash () const = 0;
    };

    template<class V, class N>
    class landmark_cst: public landmark<V,N> {
      N _n;
      typedef landmark<V,N> landmark_t;
      typedef landmark_cst<V,N> landmark_cst_t;

     public:
      landmark_cst (N n): landmark_t(landmark_kind_t::LMC), _n(n) {}

      bool operator==(const landmark_t& o) const {
        if (this->_kind != o.kind()) return false;

        assert (o.kind () == landmark_kind_t::LMC);
        auto o_ptr =  static_cast<const landmark_cst_t*>(&o);
        return (_n == o_ptr->_n);
      }

      bool operator<(const landmark_t& o) const {
        if (this->_kind != o.kind()) return true;

        assert (o.kind () == landmark_kind_t::LMC);
        return (_n < static_cast<const landmark_cst_t*>(&o)->_n);
      }

      void write(crab_os&o) const { o << _n; }

      std::size_t hash() const { return hash_value(_n);}

      N get_cst () const { return _n;}
    };

    template<class V, class N>
    class landmark_var: public landmark<V,N> {
      V _v;
      typedef landmark<V,N> landmark_t;
      typedef landmark_var<V,N> landmark_var_t;

     public:
      landmark_var (V v) : landmark_t(landmark_kind_t::LMV), _v(v) {}

      bool operator==(const landmark_t& o) const {
        if (this->_kind != o.kind()) return false;

        assert (o.kind () == landmark_kind_t::LMV);
        auto o_ptr = static_cast<const landmark_var_t*>(&o);
        return (_v == o_ptr->_v);
      }

      bool operator<(const landmark_t& o) const {
        if (this->_kind == o.kind()) {
          assert (o.kind () == landmark_kind_t::LMV);
          auto o_ptr =  static_cast<const landmark_var_t*>(&o);
          return (_v < o_ptr->_v);
        } else if (o.kind () == LMC) {
          return false;
        } else if (o.kind () == LMVP) {
          return true;
        } else
          CRAB_ERROR("unreachable!");
      }

      void write(crab_os&o) const { o << _v; }

      std::size_t hash() const { return hash_value(_v);}

      V get_var () const { return _v;}
    };

    template<class V, class N>
    class landmark_varprime: public landmark<V,N> {
      std::string _lm;
      V _v;
      typedef landmark<V,N> landmark_t;
      typedef landmark_varprime<V,N> landmark_var_prime_t;

     public:
      landmark_varprime (std::string lm, V v)
          : landmark_t(landmark_kind_t::LMVP), _lm(lm), _v(v) {}

      bool operator==(const landmark_t& o) const {
        if (this->_kind != o.kind()) return false;

        assert (o.kind () == landmark_kind_t::LMVP);
        auto o_ptr =  static_cast<const landmark_var_prime_t*>(&o);
        return (_v == o_ptr->_v);
      }

      bool operator<(const landmark_t& o) const {
        if (this->_kind != o.kind()) return false;

        assert (o.kind () == landmark_kind_t::LMVP);
        auto o_ptr =  static_cast<const landmark_var_prime_t*>(&o);
        return (_v < o_ptr->_v);
      }

      void write(crab_os&o) const { o << _lm << "'"; }

      std::size_t hash() const { return hash_value(_v);}

      V get_var () const { return _v;}
    };

    // Wrapper for landmark
    template<class V, class N>
    class landmark_ref {
      typedef landmark<V,N> landmark_t;
      typedef landmark_ref<V,N> landmark_ref_t;

     public:

      boost::shared_ptr<landmark_t> _ref;

      landmark_ref(V v, std::string name=""): _ref(nullptr) {
        if (name=="") {
          _ref = boost::static_pointer_cast<landmark_t>
              (boost::make_shared<landmark_var<V,N> >(landmark_var<V,N>(v)));
        } else {
          _ref = boost::static_pointer_cast<landmark_t>
              (boost::make_shared<landmark_varprime<V,N> >(landmark_varprime<V,N>(name, v)));
        }
      }
      landmark_ref(N n)
          : _ref(boost::static_pointer_cast<landmark_t>
                 (boost::make_shared<landmark_cst<V,N> >(landmark_cst<V,N>(n)))) { }

      landmark_kind_t kind() const { return _ref->kind();}

      bool operator==(const landmark_ref &o) const { return (*_ref == *(o._ref)); }

      bool operator<(const landmark_ref &o) const { return (*_ref < *(o._ref)); }

      void write(crab_os& o) const { _ref->write(o); }

      std::size_t hash () const { return _ref->hash();}
    };

    // super unsafe!
    template<class V, class N>
    inline V get_var(const landmark_ref<V,N>& lm) {
      assert (lm.kind() == LMVP);
      return boost::static_pointer_cast<const landmark_varprime<V,N> >(lm._ref)->get_var();
    }

    // super unsafe!
    template<class V, class N>
    inline N get_cst(const landmark_ref<V,N>& lm) {
      assert (lm.kind() == LMC);
      return boost::static_pointer_cast<const landmark_cst<V,N> >(lm._ref)->get_cst();
    }

    template<class V, class N>
    inline crab_os& operator<<(crab_os& o, const landmark_ref<V,N> &lm) {
      lm.write(o);
      return o;
    }

    template<class V, class N>
    inline std::size_t hash_value(const landmark_ref<V,N> &lm) {
      return lm.hash();
    }

    /*
      Reduced product of a numerical domain with a weighted array
      graph.

      FIXME: the set of array landmarks are chosen statically before
             starting the analysis (do_initialization). However, at
             anytime only those alive (using scalar domain) are
             considered.

             The main issue is that the landmarks are kept as global
             state. This is really error-prone. For instance,
             landmarks are reset each time a new CFG is analyzed. For
             a summary-based inter-procedural analysis might be ok but
             not, e.g., for inlining.
    */
    template<typename NumDom, typename Weight, bool IsDistWeight = false>
    class array_sparse_graph_domain:
        public writeable,
        public numerical_domain<typename NumDom::number_t, typename NumDom::varname_t>,
        public bitwise_operators<typename NumDom::number_t, typename NumDom::varname_t>,
        public division_operators<typename NumDom::number_t, typename NumDom::varname_t>,
        public array_operators<typename NumDom::number_t, typename NumDom::varname_t>,
        public pointer_operators<typename NumDom::number_t, typename NumDom::varname_t>,
	public boolean_operators<typename NumDom::number_t, typename NumDom::varname_t>
    {

     public:
      typedef typename NumDom::number_t Number;
      typedef typename NumDom::varname_t VariableName;

      // WARNING: assume NumDom::number_t = Weight::number_t and
      //                 NumDom::varname_t = Weight::varname_t
      using typename numerical_domain< Number, VariableName>::linear_expression_t;
      using typename numerical_domain< Number, VariableName>::linear_constraint_t;
      using typename numerical_domain< Number, VariableName>::linear_constraint_system_t;
      using typename numerical_domain< Number, VariableName>::variable_t;
      using typename numerical_domain< Number, VariableName>::number_t;
      using typename numerical_domain< Number, VariableName>::varname_t;
      typedef crab::pointer_constraint<VariableName> ptr_cst_t;
      typedef interval<Number> interval_t;

      typedef landmark_cst<VariableName,Number> landmark_cst_t;
      typedef landmark_var<VariableName,Number> landmark_var_t;
      typedef landmark_varprime<VariableName,Number> landmark_var_prime_t;
      typedef landmark_ref<VariableName,Number> landmark_ref_t;
      typedef array_sparse_graph<landmark_ref_t,Weight,IsDistWeight> array_sgraph_t;
      typedef array_sparse_graph_domain<NumDom,Weight,IsDistWeight> array_sgraph_domain_t;

      //// XXX: make this a template parameter later
      typedef crab::cfg::var_factory_impl::str_var_alloc_col::varname_t str_varname_t;
      typedef interval_domain<z_number, str_varname_t> str_interval_dom_t;
      typedef term::TDomInfo<z_number, varname_t, str_interval_dom_t> idom_info;
      typedef term_domain<idom_info> expression_domain_t;

     private:
      typedef typename array_sgraph_t::mut_val_ref_t mut_val_ref_t;

      // Quick wrapper to perform efficient unsat queries on the
      // scalar domain.
      struct solver_wrapper {
        // XXX: do not pass by reference
        NumDom _inv;
        solver_wrapper(NumDom inv): _inv(inv) { }
        bool is_unsat (linear_constraint_t cst) {
          // XXX: it might modify _inv so that's why we make a copy in
          // the constructor.
          return array_sgraph_domain_traits<NumDom>::is_unsat(_inv, cst);
        }
      };

      NumDom _scalar;
      expression_domain_t _expressions; // map each program variable to a symbolic expression
      array_sgraph_t _g;

      // A landmark is either a variable or number that may appear as
      // an array index. In addition, for each landmark l we keep
      // track of a prime landmark l' whose meaning is l'=l+1.

      /// === Static data
      typedef boost::unordered_map<landmark_ref_t,landmark_ref_t> lm_map_t;
      static lm_map_t var_landmarks;
      static lm_map_t cst_landmarks;

      // --- landmark iterators
      struct get_first : public std::unary_function<typename lm_map_t::value_type,
						    landmark_ref_t> {
        get_first () {}
        landmark_ref_t operator()(const typename lm_map_t::value_type &p) const
        { return p.first; }
      };
      struct get_second : public std::unary_function<typename lm_map_t::value_type,
						     landmark_ref_t> {
        get_second () {}
        landmark_ref_t operator()(const typename lm_map_t::value_type &p) const
        { return p.second; }
      };
      typedef boost::transform_iterator<get_first,
                                        typename lm_map_t::iterator> lm_iterator;
      typedef boost::transform_iterator<get_second,
                                        typename lm_map_t::iterator> lm_prime_iterator;
      typedef boost::iterator_range<lm_iterator> lm_range;
      typedef boost::iterator_range<lm_prime_iterator> lm_prime_range;

      lm_prime_iterator var_lm_prime_begin()
      { return boost::make_transform_iterator(var_landmarks.begin(), get_second());}
      lm_prime_iterator var_lm_prime_end()
      { return boost::make_transform_iterator(var_landmarks.end(), get_second());}
      lm_prime_range var_lm_primes()
      { return boost::make_iterator_range(var_lm_prime_begin(), var_lm_prime_end());}

      lm_prime_iterator cst_lm_prime_begin()
      { return boost::make_transform_iterator(cst_landmarks.begin(), get_second());}
      lm_prime_iterator cst_lm_prime_end()
      { return boost::make_transform_iterator(cst_landmarks.end(), get_second());}
      lm_prime_range cst_lm_primes()
      { return boost::make_iterator_range(cst_lm_prime_begin(), cst_lm_prime_end());}

     public:

      template<class CFG>
      static void do_initialization (CFG cfg) {

        typedef crab::analyzer::array_segmentation<CFG> array_segment_analysis_t;
        typedef typename array_segment_analysis_t::array_segment_domain_t
	  array_segment_domain_t;
        typedef crab::analyzer::array_constant_segment_visitor
	  <typename CFG::number_t, array_segment_domain_t>
	  array_cst_segment_visitor_t;

        std::set<landmark_ref_t> lms;

        // add variables
        array_segment_analysis_t analysis(cfg);
        analysis.exec();
        auto var_indexes = analysis.get_variables(cfg.entry());

        if (var_indexes.begin() == var_indexes.end()) {
          CRAB_WARN ("No variables found in the cfg. No array graph landmarks will be added\n");
          return;
        }
        lms.insert(var_indexes.begin(), var_indexes.end());

        // get variable factory
        auto &vfac = (*var_indexes.begin()).get_var_factory ();

        // add constants
        // make sure 0 is always considered as an array index
        lms.insert(landmark_ref_t(number_t(0)));
        typename array_cst_segment_visitor_t::constant_set_t constants;
        for (auto &bb: boost::make_iterator_range(cfg.begin(), cfg.end())) {
          auto var_indexes = analysis.get_variables(bb.label());
          // XXX: use some heuristics to choose "relevant" constants
          array_cst_segment_visitor_t vis(var_indexes);
          for (auto &s: boost::make_iterator_range(bb.begin(), bb.end()))
            s.accept(&vis);
          auto cst_indexes = vis.get_constants();
          lms.insert(cst_indexes.begin(), cst_indexes.end());
        }

        set_landmarks (lms, vfac);
      }

      template<class Range, class VarFactory>
      static void set_landmarks(const Range& lms, VarFactory& vfac) {

        var_landmarks.clear();
        cst_landmarks.clear();

        unsigned num_vl = 0;
        unsigned num_cl = 0;

        for (auto lm: lms) {
          switch (lm.kind()) {
            case LMV: {
              auto v = boost::static_pointer_cast<const landmark_var_t>(lm._ref)->get_var();
              varname_t v_prime = vfac.get(v.index());
              landmark_ref_t lm_prime(v_prime, v.str());
              var_landmarks.insert(std::make_pair(lm, lm_prime));
              num_vl++;
              break;
            }
            case LMC: {
              auto n = boost::static_pointer_cast<const landmark_cst_t>(lm._ref)->get_cst();
              varname_t v_prime = vfac.get();
              landmark_ref_t lm_prime(v_prime, n.get_str());
              cst_landmarks.insert(std::make_pair(lm, lm_prime));
              num_cl++;
              break;
            }
            default:
              CRAB_ERROR("A landmark can only be either variable or constant");
          }
        }
        CRAB_LOG("array-sgraph-domain-landmark",
                 crab::outs() << "Added " << num_vl << " variable landmarks "
                              << "and " << num_cl << " constant landmarks={";
                 bool first=true;
                 for (auto &l: var_landmarks) {
                   if (!first) crab::outs() << ",";
                   first=false;
                   crab::outs() << l.first;
                 }
                  for (auto &l: cst_landmarks) {
                   if (!first) crab::outs() << ",";
                   first=false;
                   crab::outs() << l.first;
                 }
                 crab::outs() << "}\n";
                 );
      }

     public: // public only for tests

      void add_landmark(VariableName v)
      {
        landmark_ref_t lm_v (v);
        landmark_ref_t lm_v_prime(v.get_var_factory().get(v.index()), v.str());
        // add pair  x -> x'
        var_landmarks.insert(std::make_pair(lm_v, lm_v_prime));
        // x' = x + 1
        _scalar += make_prime_relation(lm_v_prime, lm_v);

        // reduce between _scalar and the array graph
        if (!reduce(_scalar, _g)) {
          // FIXME: incremental version
          // TODO: we can assume that the scalar domain is zones so
          // that we can return the affected edges after each
          // operation and apply reduction only on those edges. That
          // would suffice for now. If the scalar domain is not zones
          // then we don't reduce incrementally.
          set_to_bottom();
        }

        CRAB_LOG("array-sgraph-domain-landmark",
                 crab::outs () << "Added landmark " << v << "\n";);
      }

      void remove_landmark(VariableName v) {
        array_forget (v);
        forget_prime_var (v);
        var_landmarks.erase (landmark_ref_t (v));

        CRAB_LOG("array-sgraph-domain-landmark",
                 crab::outs () << "Removed landmark " << v << "\n";);
      }

     private:

      // By active we mean current variables that are kept track by
      // the scalar domain.
      void get_active_landmarks(NumDom &scalar, std::vector<landmark_ref_t> & landmarks) const {
        landmarks.reserve(cst_landmarks.size());
        for (auto p: cst_landmarks) {
          landmarks.push_back (p.first);
          landmarks.push_back (p.second);
        }
        auto active_vars = array_sgraph_domain_traits<NumDom>::active_variables(scalar);
        for (auto v: active_vars) {
          auto it = var_landmarks.find(landmark_ref_t(v));
          if (it != var_landmarks.end()){
            landmarks.push_back(landmark_ref_t(v));
            landmarks.push_back(it->second);
          }
        }
      }

      void set_to_bottom(){
        _scalar = NumDom::bottom();
        _expressions = expression_domain_t::bottom();
        _g.set_to_bottom();
      }

      linear_expression_t make_expr (landmark_ref_t x) {
        switch (x.kind()) {
          case LMC:
            return boost::static_pointer_cast<landmark_cst_t>(x._ref)->get_cst();
          case LMV:
            return variable_t(boost::static_pointer_cast<landmark_var_t>(x._ref)->get_var());
          case LMVP:
            return variable_t(boost::static_pointer_cast<landmark_var_prime_t>
			      (x._ref)->get_var());
          default:
            CRAB_ERROR("unreachable!");
        }
      }

      // make constraint x < y
      linear_constraint_t make_lt_cst (landmark_ref_t x, landmark_ref_t y) {
        return linear_constraint_t(make_expr(x) <= make_expr(y) - 1);
      }

      // make constraint x <= y
      linear_constraint_t make_leq_cst (landmark_ref_t x, landmark_ref_t y) {
        return linear_constraint_t(make_expr(x) <= make_expr(y));
      }

      // make constraint x == y
      linear_constraint_t make_eq_cst (landmark_ref_t x, landmark_ref_t y) {
        return linear_constraint_t(make_expr(x) == make_expr(y));
      }

      // make constraint x' == x+1
      linear_constraint_t make_prime_relation(landmark_ref_t x_prime, landmark_ref_t x){
        return linear_constraint_t(make_expr(x_prime) == make_expr(x) + 1);
      }

      // return true if v is a landmark in the graph
      bool is_landmark (VariableName v) const {
        landmark_ref_t lm_v (v);
        auto it = var_landmarks.find(lm_v);
        return (it != var_landmarks.end());
      }

      // return true if n is a landmark in the graph
      bool is_landmark (z_number n) const {
        landmark_ref_t lm_n (n);
        auto it = cst_landmarks.find(lm_n);
        return (it != cst_landmarks.end());
      }

      // return the prime landmark of v
      landmark_ref_t get_landmark_prime (VariableName v) const {
        landmark_ref_t lm_v (v);
        auto it = var_landmarks.find(lm_v);
        assert (it != var_landmarks.end());
        return it->second;
      }

      // Return the weight from the edge (i, i') otherwise top
      Weight array_edge (VariableName i) {
        if (is_bottom()) return Weight::bottom();
        if (is_top() || !is_landmark (i)) return Weight::top();

        mut_val_ref_t wi;
        if (_g.lookup_edge(landmark_ref_t (i), get_landmark_prime(i), &wi))
          return (Weight) wi;
        else
          return Weight::top();
      }

      // Remove v from the edge (i,i')
      void array_edge_forget(VariableName i, VariableName v) {
        if (is_bottom()) return;

        if (!is_landmark (i)) return;

        mut_val_ref_t wi;
        landmark_ref_t lm_i (i);
        landmark_ref_t lm_i_prime = get_landmark_prime (i);
        if (_g.lookup_edge(lm_i, lm_i_prime, &wi)) {
          Weight w = (Weight) wi;
          w -= v;
          // XXX: update_edge closes the array graph
          _g.update_edge (lm_i, w, lm_i_prime);
        }
      }

      // Remove v from all vertices and edges
      void array_forget(VariableName v) {
        if (is_bottom()) return;
        if (!is_landmark (v)) return;

        _g -= landmark_ref_t(v);
        _g.remove_from_weights(v);
      }

      // Update the weight from the edge (i, i')
      void array_edge_update (VariableName i, Weight w)
      {
        if (is_bottom()) return;

        //--- strong update
        if (!is_landmark (i)) return;

        landmark_ref_t lm_i(i);
        landmark_ref_t lm_i_prime = get_landmark_prime(i);

        _g.update_edge(lm_i, w, lm_i_prime);
        mut_val_ref_t wi;
        if (!_g.lookup_edge(lm_i, lm_i_prime, &wi))
          return;

        //--- weak update
        // An edge (p,q) must be weakened if p <= i <= q and p < q
        solver_wrapper solve(_scalar);
        mut_val_ref_t w_pq;
        for(auto p : _g.verts ()) {
          for(auto e : _g.e_succs(p)) {
            auto q = e.vert;
            if ((p == lm_i) &&  (q == lm_i_prime))
              continue;
            if (_g.lookup_edge(p, q, &w_pq) && ((Weight) w_pq).is_bottom())
              continue;
            // we know already that p < q in the array graph

            // check p <= i
            if (solve.is_unsat(make_leq_cst(p, lm_i)))
              continue;
            // check i' <= q
            if (solve.is_unsat(make_leq_cst(lm_i_prime, q)))
              continue;

            w_pq = (Weight) w_pq | (Weight) wi;
          }
        }
      }

      // x := x op k
      template<typename VarOrNum>
      void apply_one_variable (operation_t op, VariableName x, VarOrNum k) {
        if (is_bottom()) return;

        if (!is_landmark (x)) {
          // If x is not a landmark we just apply the operation on the
          // scalar domain and return.
          apply_only_scalar(op, x, x, k);
          return;
        }

        landmark_ref_t lm_x (x);
        landmark_ref_t lm_x_prime = get_landmark_prime (x);

        /// --- Add x_old and x_old' to store old values of x and x'

        VariableName x_old = x.get_var_factory().get();
        VariableName x_old_prime = x.get_var_factory().get();
        landmark_ref_t lm_x_old (x_old);
        landmark_ref_t lm_x_old_prime (x_old_prime, x_old.str());
        var_landmarks.insert(std::make_pair(lm_x_old, lm_x_old_prime));
        // x_old = x
        _scalar.assign(x_old, linear_expression_t(x));
        // relation between x_old and x'
        _scalar += make_prime_relation(lm_x_old_prime, lm_x_old);
        //_scalar += make_eq_cst(lm_x_old_prime, lm_x_prime);

        /*** Incremental graph reduction ***/
        //// x_old  has all the x predecessors and successors
        _g.expand(lm_x, lm_x_old);
        //// x_old' has all the x' predecessors and successors
        _g.expand(lm_x_prime, lm_x_old_prime);
        //// edges between x and x_old
        _g.update_edge(lm_x, Weight::bottom(), lm_x_old);
        _g.update_edge(lm_x_old, Weight::bottom(), lm_x);
        //// edges between x' and x_old'
        _g.update_edge(lm_x_prime, Weight::bottom(), lm_x_old_prime);
        _g.update_edge(lm_x_old_prime, Weight::bottom(), lm_x_prime);
        //// edges between x_old and x_old'
        mut_val_ref_t w;
        if (_g.lookup_edge(lm_x, lm_x_prime, &w))
          _g.update_edge(lm_x_old, (Weight) w, lm_x_old_prime);
        _g.update_edge(lm_x_old_prime, Weight::bottom(), lm_x_old);

        /// --- Remove x and x'
        _g -= lm_x;
        _g -= lm_x_prime;

        /// --- Perform operation in the scalar domain
        _scalar.apply(op, x, x, k);

        //restore relation between x and x'
        _scalar.apply(OP_ADDITION, get_var(lm_x_prime), x, 1);
        //_scalar -= get_var(lm_x_prime);
        //_scalar += make_prime_relation(lm_x_prime, lm_x);

        if (!reduce(_scalar, _g)) { // FIXME: incremental version
          set_to_bottom();
          return;
        }

        /// --- Remove x_old and x_old'
        _g -= lm_x_old;
        _g -= lm_x_old_prime;
        _scalar -= x_old;
        _scalar -= x_old_prime;
        var_landmarks.erase(lm_x_old);
      }

      // remove v' from scalar and array graph
      void forget_prime_var(VariableName v) {
        if (!is_landmark(v)) return;

        landmark_ref_t lm_v_prime = get_landmark_prime (v);
        _scalar -= get_var(lm_v_prime);
        // XXX: v' cannot appear in the array weights so we do not
        //      need to call array_forget.
        _g -= lm_v_prime;
      }

      // perform the operation in the scalar domain assuming that
      // nothing can be done in the graph domain.
      template<class Op, class K>
      void apply_only_scalar(Op op, VariableName x, VariableName y, K k) {
        _scalar.apply(op, x, y, k);

        // Abstract x in the array graph
        if (is_landmark (x)){
          array_forget(x);     // remove x from the array graph
          forget_prime_var(x); // remove x' from scalar and array graph
          /// XXX: I think no need to reduce here
        }
      }

      // return a pair with the normalized offset and a bool that is
      // true if a new landmark was added in the array graph
      std::pair<VariableName,bool> normalize_offset (VariableName o, z_number n)
      {
        CRAB_LOG("array-sgraph-domain-norm",
                 crab::outs() << "BEFORE NORMALIZE OFFSET: expressions="
		              << _expressions << "\n");

        // --- create a fresh variable no such that no := o;
        VariableName no = o.get_var_factory().get();
        _expressions.assign (no, linear_expression_t (o));

        // -- apply no := no / n; in the expressions domain
        _expressions.apply (operation_t::OP_DIVISION, no, no, n);

        // -- simplify the expression domain
        bool simp_done = _expressions.simplify (no);

        CRAB_LOG("array-sgraph-domain-norm",
                 crab::outs() << "AFTER NORMALIZE OFFSET: expressions="
		              << _expressions << "\n");

        if (!simp_done) {
          CRAB_LOG("array-sgraph-domain-norm",
                   crab::outs() << "NO NORMALIZATION done using the expression abstraction\n");

          // cleanup of the expression abstraction
          _expressions -= no;

          bool added_lm = false;
          if (!is_landmark (o))
          { add_landmark (o); added_lm = true; }

          return std::make_pair(o, added_lm);
        }

        CRAB_LOG("array-sgraph-domain-norm",
                 crab::outs() << "NORMALIZATION DONE! using the expression abstraction\n");

        // -- propagate equalities from _expressions to _scalar
        product_domain_traits<expression_domain_t, NumDom>::push(no, _expressions, _scalar);

        // -- add landmark for the new array index
        add_landmark (no);

        // cleanup of the expression abstraction
        _expressions -= no;

        return std::make_pair(no, true);
      }

     public:

      // The reduction consists of detecting dead segments so it is
      // done only in one direction (scalar -> array graph). Note that
      // whenever an edge becomes bottom closure is also happening.
      // Return false if bottom is detected during the reduction.
      bool reduce(NumDom &scalar, array_sgraph_t &g) {
        crab::CrabStats::count (getDomainName() + ".count.reduce");
        crab::ScopedCrabStats __st__(getDomainName() + ".reduce");

        domain_traits<NumDom>::normalize(scalar);
        g.normalize();

        if (scalar.is_bottom() || g.is_bottom())
          return false;

        if (!scalar.is_top ()) {
          std::vector<landmark_ref_t> active_landmarks;
          get_active_landmarks(scalar, active_landmarks);
          solver_wrapper solve(scalar);
          for (auto lm_s : active_landmarks)
            for (auto lm_d : active_landmarks) {
              // XXX: we do not exploit the following facts:
              //   - i < i' is always sat
              //   - i' < i is always unsat
              //   - if i < j  unsat then i' < j unsat.
              //   - if i < j' unsat then i' < j unsat.
              if ((lm_s == lm_d) || solve.is_unsat (make_lt_cst(lm_s,lm_d))) {
                g.update_edge(lm_s, Weight::bottom(), lm_d);
              }
            }
        }
        return (!g.is_bottom());
      }

      static array_sgraph_domain_t top () {
        return array_sgraph_domain_t(false);
      }

      static array_sgraph_domain_t bottom () {
        return array_sgraph_domain_t(true);
      }

     public:

      array_sparse_graph_domain(bool is_bottom=false)
          : _scalar(NumDom::top()), _expressions(expression_domain_t::top ()),
            _g(array_sgraph_t::top()) {
        if (is_bottom)
          set_to_bottom();
      }

      array_sparse_graph_domain(const NumDom& s, const expression_domain_t& e,
                                const array_sgraph_t& g)
          : _scalar(s), _expressions(e), _g(g) {
        if (_scalar.is_bottom() || _expressions.is_bottom() || _g.is_bottom())
          set_to_bottom();
      }

      array_sparse_graph_domain(NumDom &&s, expression_domain_t &&e,
                                array_sgraph_t &&g)
          : _scalar(std::move(s)), _expressions (std::move(e)), _g(std::move(g)) {
        if (_scalar.is_bottom() || _expressions.is_bottom() || _g.is_bottom())
          set_to_bottom();
      }

      array_sparse_graph_domain(const array_sgraph_domain_t&o)
          : _scalar(o._scalar), _expressions (o._expressions), _g(o._g) {
        crab::CrabStats::count (getDomainName() + ".count.copy");
        crab::ScopedCrabStats __st__(getDomainName() + ".copy");
      }

      array_sparse_graph_domain(array_sgraph_domain_t &&o)
          : _scalar(std::move(o._scalar)),
            _expressions (std::move(o._expressions)),
            _g(std::move(o._g)) {
      }

      array_sgraph_domain_t& operator=(const array_sgraph_domain_t& o) {
        crab::CrabStats::count (getDomainName() + ".count.copy");
        crab::ScopedCrabStats __st__(getDomainName() + ".copy");
        if(this != &o) {
          _scalar = o._scalar;
          _expressions = o._expressions;
          _g = o._g;
        }
        return *this;
      }

      array_sgraph_domain_t& operator=(array_sgraph_domain_t &&o) {
        _scalar = std::move(o._scalar);
        _expressions = std::move(o._expressions);
        _g = std::move(o._g);
        return *this;
      }

      bool is_top() {
        return _scalar.is_top () && _expressions.is_top () && _g.is_top();
      }

      bool is_bottom() {
        return _scalar.is_bottom() || _expressions.is_bottom() || _g.is_bottom();
      }

      bool operator<=(array_sgraph_domain_t &o) {
        crab::CrabStats::count (getDomainName() + ".count.leq");
        crab::ScopedCrabStats __st__(getDomainName() + ".leq");

        CRAB_LOG("array-sgraph-domain",
                 crab::outs () << "Leq " << *this << " and\n"  << o << "=\n";);
        bool res = (_scalar <= o._scalar) &&
                   (_expressions <= o._expressions) &&
                   (_g <= o._g);
        CRAB_LOG("array-sgraph-domain", crab::outs () << res << "\n";);
        return res;
      }

      void operator|=(array_sgraph_domain_t o)  {
        *this = (*this | o);
      }

      array_sgraph_domain_t operator|(array_sgraph_domain_t &o){
        crab::CrabStats::count (getDomainName() + ".count.join");
        crab::ScopedCrabStats __st__(getDomainName() + ".join");

        CRAB_LOG("array-sgraph-domain",
                 crab::outs () << "Join " << *this << " and "  << o << "=\n");
        array_sgraph_domain_t join(_scalar | o._scalar,
                                   _expressions | o._expressions,
                                   _g | o._g);
        CRAB_LOG("array-sgraph-domain", crab::outs () << join << "\n";);
        return join;
      }

      template<typename Thresholds>
      array_sgraph_domain_t widening_thresholds (array_sgraph_domain_t& o,
                                                 const Thresholds & ts) {
        crab::CrabStats::count (getDomainName() + ".count.widening");
        crab::ScopedCrabStats __st__(getDomainName() + ".widening");

          CRAB_LOG("array-sgraph-domain",
                   crab::outs () << "Widening (w/ thresholds) " << *this << " and "
		                 << o << "=\n";);
        auto widen_scalar(_scalar.widening_thresholds(o._scalar,ts));
        auto widen_expr(_expressions.widening_thresholds(o._expressions,ts));
        auto widen_g(_g.widening_thresholds(o._g,ts));
        if (!reduce(widen_scalar, widen_g)) {
          CRAB_LOG("array-sgraph-domain", crab::outs () << "_|_\n";);
          return array_sgraph_domain_t::bottom();
        } else {
          array_sgraph_domain_t widen(widen_scalar, widen_expr, widen_g);
          CRAB_LOG("array-sgraph-domain", crab::outs () << widen << "\n";);
          return widen;
        }
      }

      array_sgraph_domain_t operator||(array_sgraph_domain_t &o){
        crab::CrabStats::count (getDomainName() + ".count.widening");
        crab::ScopedCrabStats __st__(getDomainName() + ".widening");

        CRAB_LOG("array-sgraph-domain",
                 crab::outs () << "Widening " << *this << " and "  << o << "=\n");
        auto widen_scalar(_scalar || o._scalar);
        auto widen_expr(_expressions || o._expressions);
        auto widen_g(_g || o._g);
        if (!reduce(widen_scalar, widen_g)) {
          CRAB_LOG("array-sgraph-domain", crab::outs () << "_|_\n";);
          return array_sgraph_domain_t::bottom();
        } else {
          array_sgraph_domain_t widen(widen_scalar, widen_expr, widen_g);
          CRAB_LOG("array-sgraph-domain", crab::outs () << widen << "\n";);
          return widen;
        }
      }

      array_sgraph_domain_t operator&(array_sgraph_domain_t &o){
        crab::CrabStats::count (getDomainName() + ".count.meet");
        crab::ScopedCrabStats __st__(getDomainName() + ".meet");

        CRAB_LOG("array-sgraph-domain",
                 crab::outs () << "Meet " << *this << " and "  << o << "=\n");
        auto meet_scalar(_scalar & o._scalar);
        auto meet_expr(_expressions & o._expressions);
        auto meet_g(_g & o._g);
        if (!reduce(meet_scalar, meet_g)) {
          CRAB_LOG("array-sgraph-domain", crab::outs () << "_|_\n";);
          return array_sgraph_domain_t::bottom();
        } else {
          array_sgraph_domain_t meet(meet_scalar, meet_expr, meet_g);
          CRAB_LOG("array-sgraph-domain", crab::outs () << meet << "\n";);
          return meet;
        }
      }

      array_sgraph_domain_t operator&&(array_sgraph_domain_t &o){
        crab::CrabStats::count (getDomainName() + ".count.narrowing");
        crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");

        CRAB_LOG("array-sgraph-domain",
                 crab::outs () << "Narrowing " << *this << " and "  << o << "=\n");
        auto narrow_scalar(_scalar && o._scalar);
        auto narrow_expr(_expressions && o._expressions);
        auto narrow_g(_g && o._g);
        if (!reduce(narrow_scalar, narrow_g)) {
          CRAB_LOG("array-sgraph-domain", crab::outs () << "_|_\n";);
          return array_sgraph_domain_t::bottom();
        } else {
          array_sgraph_domain_t narrow(narrow_scalar, narrow_expr, narrow_g);
          CRAB_LOG("array-sgraph-domain", crab::outs () << narrow << "\n";);
          return narrow;
        }
      }

      void operator-=(VariableName v) {
        crab::CrabStats::count (getDomainName() + ".count.forget");
        crab::ScopedCrabStats __st__(getDomainName() + ".forget");

        if (is_bottom())
          return;

        // remove v from scalar and array graph
        _scalar -= v;
        // remove v from expressions
        _expressions -= v;

        if (is_landmark (v)) {
          array_forget(v);
          // remove v' from scalar and array graph
          forget_prime_var(v);
        }
      }

      // remove all variables except [vIt,...vEt)
      template<typename Iterator>
      void project (Iterator vIt, Iterator vEt) {
        crab::CrabStats::count (getDomainName() + ".count.project");
        crab::ScopedCrabStats __st__(getDomainName() + ".project");

        if (is_bottom ()) return;
        if (vIt == vEt) return;

        std::set<VariableName> keep_vars (vIt, vEt);
        auto active_vars = array_sgraph_domain_traits<NumDom>::active_variables(_scalar);
        for (auto v: active_vars) {
          if (!keep_vars.count (v)) {
            array_forget (v);
            forget_prime_var (v);
          }
        }

        domain_traits<NumDom>::project(_scalar, vIt, vEt);
        domain_traits<expression_domain_t>::project(_expressions, vIt, vEt);

      }

      void operator+=(linear_constraint_system_t csts)
      {
        crab::CrabStats::count (getDomainName() + ".count.add_constraints");
        crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");

        if (is_bottom()) return;

        _scalar += csts;
        _expressions += csts;

        if (!reduce(_scalar, _g)) { // FIXME: incremental version
          set_to_bottom();
          return;
        }
        CRAB_LOG("array-sgraph-domain",
                 crab::outs() << "Assume("<< csts<< ") --- "<< *this<<"\n";);
      }

      void assign (VariableName x, linear_expression_t e)
      { assign (x, e, true); }

      // Perform the operation in the scalar (optionally expression)
      // domain and reduce.
      //
      // NOTE: if the assignment is something like i = i + k then we
      // will lose precision in the array graph. This kind of
      // assignments should be managed by the apply methods instead.
      void assign (VariableName x, linear_expression_t e, bool update_expressions)
      {
        crab::CrabStats::count (getDomainName() + ".count.assign");
        crab::ScopedCrabStats __st__(getDomainName() + ".assign");

        if (is_bottom()) return;

        if (auto y = e.get_variable()) {
          // skip x:=x
          if ((*y).name() == x)
            return;
        }

        _scalar.assign(x, e);
        if (update_expressions)
          _expressions.assign(x, e);

        if (is_landmark (x)) {
           array_forget (x);
           // remove x' from scalar and array graph
           forget_prime_var(x);
           // restore the relationship between x and x'
           _scalar.apply(OP_ADDITION, get_var(get_landmark_prime (x)), x, 1);
           // XXX: is it needed ??
           //_g.close_edge (landmark_ref_t(x), get_landmark_prime (x));
        }

        if (!reduce(_scalar, _g)) { // FIXME: incremental version
          set_to_bottom();
          return;
        }

        CRAB_LOG("array-sgraph-domain",
                 crab::outs() << "Assign "<<x<<" := "<<e<<" ==> "<<*this<<"\n";);
      }

      void apply (operation_t op, VariableName x, VariableName y, Number z) {
        if (x == y) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");
          _expressions.apply (op, x, y, z);
          apply_one_variable<Number> (op, x, z);
          CRAB_LOG("array-sgraph-domain",
                   crab::outs() << "Apply "<<x<<" := "<<y<<" "<<op<<" "<<z<<" ==> "
		                << *this<<"\n";);
        }
        else {
          switch (op) {
            case OP_ADDITION:
              assign (x, linear_expression_t(y) + linear_expression_t(z)); break;
            case OP_SUBTRACTION:
              assign (x, linear_expression_t(y) - linear_expression_t(z)); break;
            case OP_MULTIPLICATION:
              assign (x, linear_expression_t(y) * z); break;
            case OP_DIVISION:
              CRAB_WARN("Division operation not implemented in array-sgraph-domain\n");
            default: ;;
          }
        }
      }

      void apply(operation_t op, VariableName x, VariableName y, VariableName z)  {
        if (x==y) {
          crab::CrabStats::count (getDomainName() + ".count.apply");
          crab::ScopedCrabStats __st__(getDomainName() + ".apply");
          _expressions.apply (op, x, y, z);
          apply_one_variable<VariableName> (op, x, z);
          CRAB_LOG("array-sgraph-domain",
                   crab::outs() << "Apply "<<x<<" := "<<y<<" "<<op<<" "<<z<<" ==> "
		                << *this<<"\n";);
        }
        else {
          switch (op) {
            case OP_ADDITION:
              assign (x, linear_expression_t(y) + linear_expression_t(z)); break;
            case OP_SUBTRACTION:
              assign (x, linear_expression_t(y) - linear_expression_t(z)); break;
            case OP_MULTIPLICATION:
              CRAB_WARN("Mutiplication not implemented in array-sgraph-domain\n"); break;
            case OP_DIVISION:
              CRAB_WARN("Division not implemented in array-sgraph-domain\n"); break;
            default:;;
          }
        }
      }

      void apply(operation_t op, VariableName x, Number k)  {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");
        _expressions.apply (op, x, k);
        apply_one_variable <Number> (op, x, k);

        CRAB_LOG("array-sgraph-domain",
                 crab::outs() << "Apply "<<x<<" := "<<x<<" "<<op<<" "<<k<<" ==> "
		              << *this<<"\n";);
      }

      void apply(conv_operation_t op, VariableName x, VariableName y, unsigned width) {
        _expressions.apply (op, x, y, width);
        // assume unlimited precision so width is ignored.
        assign(x, variable_t (y), false);
      }

      void apply(conv_operation_t op, VariableName x, Number k, unsigned width) {
        _expressions.apply (op, x, k, width);
        // assume unlimited precision so width is ignored.
        assign(x, k, false);
      }

      // bitwise_operators_api
      void apply(bitwise_operation_t op, VariableName x, VariableName y, VariableName z) {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        _expressions.apply (op, x, y, z);
        // XXX: we give up soundly in the graph domain
        apply_only_scalar (op, x, y, z);
      }

      void apply(bitwise_operation_t op, VariableName x, VariableName y, Number k) {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        _expressions.apply (op, x, y, k);
        // XXX: we give up soundly in the graph domain
        apply_only_scalar (op, x, y, k);
      }

      // division_operators_api
      void apply(div_operation_t op, VariableName x, VariableName y, VariableName z) {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        _expressions.apply (op, x, y, z);
        // XXX: we give up soundly in the graph domain
        apply_only_scalar (op, x, y, z);
      }

      void apply(div_operation_t op, VariableName x, VariableName y, Number k) {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        _expressions.apply (op, x, y, k);
        // XXX: we give up soundly in the graph domain
        apply_only_scalar (op, x, y, k);
      }

      interval_t operator[](VariableName v)  {
        return _scalar[v];
      }

      // pointer_operators_api
      virtual void pointer_load (VariableName lhs, VariableName rhs) override {
        _scalar.pointer_load(lhs,rhs);
      }

      virtual void pointer_store (VariableName lhs, VariableName rhs) override {
        _scalar.pointer_store(lhs,rhs);
      }

      virtual void pointer_assign (VariableName lhs, VariableName rhs,
				   linear_expression_t offset) override {
        _scalar.pointer_assign (lhs,rhs,offset);
      }

      virtual void pointer_mk_obj (VariableName lhs, ikos::index_t address) override {
        _scalar.pointer_mk_obj (lhs, address);
      }

      virtual void pointer_function (VariableName lhs, VariableName func) override {
        _scalar.pointer_function (lhs, func);
      }

      virtual void pointer_mk_null (VariableName lhs) override {
        _scalar.pointer_mk_null (lhs);
      }

      virtual void pointer_assume (ptr_cst_t cst) override {
        _scalar.pointer_assume (cst);
      }

      virtual void pointer_assert (ptr_cst_t cst) override {
        _scalar.pointer_assert (cst);
      }

      // array_operators_api

      virtual void array_assume (VariableName a, variable_type a_ty,
                                 linear_expression_t lb_idx, linear_expression_t ub_idx,
                                 VariableName var) override {

        auto lb_var_opt = lb_idx.get_variable ();
        auto ub_var_opt = ub_idx.get_variable ();

        if (lb_idx.is_constant () && ub_idx.is_constant ())
          array_assume (a, a_ty, lb_idx.constant (), ub_idx.constant (), var);
        else if (lb_idx.is_constant () && ub_var_opt)
          array_assume (a, a_ty, lb_idx.constant (), (*ub_var_opt).name(), var);
        else if (lb_var_opt && ub_idx.is_constant ())
          array_assume (a, a_ty, (*lb_var_opt).name(), ub_idx.constant (), var);
        else if (lb_var_opt && ub_var_opt)
          array_assume (a, a_ty, (*lb_var_opt).name(), (*ub_var_opt).name(), var);
        else
          CRAB_WARN ("array_sparse_graph only supports assume_array with number or cst indexes");
      }

      virtual void array_load (VariableName lhs, VariableName a, crab::variable_type a_ty,
                               linear_expression_t i, z_number nbytes) override
      {

        crab::CrabStats::count (getDomainName() + ".count.load");
        crab::ScopedCrabStats __st__(getDomainName() + ".load");

        auto vi = i.get_variable ();
        if (!vi) {
          CRAB_WARN ("TODO: array load index must be a variable");
          return;
        }

        // -- normalization ensures that closure and reduction have
        // -- been applied.
        auto p = normalize_offset ((*vi).name(), nbytes);
        VariableName norm_idx = p.first;

        // #if 0
        // if (is_landmark (norm_idx)) {
        //   landmark_ref_t lm_norm_idx(norm_idx);
        //   landmark_ref_t lm_norm_idx_prime = get_landmark_prime (norm_idx);

        //   _g.close_edge (lm_norm_idx, lm_norm_idx_prime);
        //   crab::outs () << "#### 1 " << _g << "\n";

        //   Weight w;
        //   w += linear_constraint_t(linear_expression_t(lhs) == linear_expression_t(a));
        //   crab::outs () << "#### 2 " << w << "\n";
        //   //_g.update_edge_unclosed(lm_norm_idx, w, lm_norm_idx_prime);
        //   _g.update_edge(lm_norm_idx, w, lm_norm_idx_prime);
        // }
        // #endif

        Weight w = array_edge (norm_idx);

        if (a_ty == ARR_INT_TYPE) {
          // Only non-relational numerical invariants are
          // propagated from the graph domain to the expressions domain.
          _expressions.set (lhs, w[a]);
        }

        array_sparse_graph_impl::propagate_between_weight_and_scalar(w, a, a_ty, _scalar, lhs);

        // if normalize_offset created a landmark we remove it here to
        // keep smaller array graph
        if (p.second) remove_landmark (norm_idx);

        /// XXX: due to the above simplification we need to reduce
        /// only if the content of an array cell can be an index.
        if (is_landmark (lhs))
          if (!reduce(_scalar,_g)) // FIXME: incremental version
            set_to_bottom();

        CRAB_LOG("array-sgraph-domain",
                 crab::outs() << "Array read "<<lhs<<" := "<< a<<"["<<i<<"] ==> "
                               << *this <<"\n";);
      }

      virtual void array_store (VariableName a, crab::variable_type a_ty,
                                linear_expression_t i, VariableName val,
                                z_number nbytes, bool /*is_singleton*/) override {
        crab::CrabStats::count (getDomainName() + ".count.store");
        crab::ScopedCrabStats __st__(getDomainName() + ".store");

        auto vi = i.get_variable ();
        if (!vi) {
          CRAB_WARN ("TODO: array store index must be a variable");
          return;
        }

        Weight w = Weight::top ();
        array_sparse_graph_impl::propagate_between_weight_and_scalar(_scalar, val, a_ty, w, a);

        auto p = normalize_offset ((*vi).name(), nbytes);
        VariableName norm_idx = p.first;

        array_edge_forget (norm_idx, a);
        array_edge_update (norm_idx, w);

        // XXX: since we do not propagate from the array weights to
        // the scalar domain I think we don't need to reduce here.

        CRAB_LOG("array-sgraph-domain",
                 crab::outs() << "Array write "<<a<<"["<<i<<"] := "<<val<< " ==> "
                              << *this <<"\n";);
      }

      // T1 and T2 are either VariableName or z_number
      template<typename T1, typename T2>
      void array_assume (VariableName arr, variable_type arr_ty, T1 src, T2 dst,
			 VariableName val)
      {
        if (!is_landmark (src)) {
          crab::outs () << "WARNING no landmark found for " << src << "\n";
          return;
        }

        if (!is_landmark(dst)) {
          crab::outs () << "WARNING no landmark found for " << dst << "\n";
          return;
        }

        landmark_ref_t lm_src (src);
        landmark_ref_t lm_dst (dst);

        Weight w = Weight::top ();
        array_sparse_graph_impl::propagate_between_weight_and_scalar
	  (_scalar, val, arr_ty, w, arr);
        _g.update_edge(lm_src, w, lm_dst);
      }

      void write(crab_os& o) {
        #if 1
        NumDom copy_scalar(_scalar);
        array_sgraph_t copy_g(_g);
        // Remove all primed variables for pretty printing
        for(auto lm: var_lm_primes()) {
          copy_scalar -= get_var(lm);
          copy_g -= lm;
        }
        for(auto lm: cst_lm_primes()) {
          copy_scalar -= get_var(lm);
          copy_g -= lm;
        }
        o << "(" << copy_scalar  << ",";
        copy_g.write(o,false);  // we do not print bottom edges
        o << ")";
        //o << "##" << _expressions;
        #else
        o << "("
          << "S=" << _scalar  << ","
          << "E=" << _expressions  << ","
          << "G=" << _g
          << ")";
        #endif
      }

      // XXX: the array domain is disjunctive so it is not really
      // useful to express it through a conjunction of linear
      // constraints
      linear_constraint_system_t to_linear_constraint_system (){
        CRAB_WARN ("array-sgraph does not implement to_linear_constraint_system");
        return linear_constraint_system_t();
      }

      static std::string getDomainName () {
        std::string name ("ArraySparseGraph(" +
			  NumDom::getDomainName () +  "," +  Weight::getDomainName () + ")");
        return name;
      }

    };

    template<typename NumDom, typename Weight>
    class domain_traits<array_sparse_graph_domain<NumDom,Weight,false> > {

     public:
      // WARNING: assume NumDom::number_t = Weight::number_t and
      //                 NumDom::varname_t = Weight::varname_t
      typedef typename NumDom::number_t N;
      typedef typename NumDom::varname_t V;

      typedef array_sparse_graph_domain<NumDom,Weight,false> array_sgraph_domain_t;

      template<class CFG>
      static void do_initialization (CFG cfg) {
        array_sgraph_domain_t::do_initialization(cfg);
      }

      static void expand (array_sgraph_domain_t& inv, V x, V new_x) {
        CRAB_WARN ("array_graph_domain expand not implemented");
      }

      static void normalize (array_sgraph_domain_t& inv) {
        CRAB_WARN ("array_graph_domain normalize not implemented");
      }

      template <typename Iter>
      static void forget (array_sgraph_domain_t& inv, Iter it, Iter end){
        for (auto v: boost::make_iterator_range(it,end))
        { inv -= v; }
      }

      template <typename Iter>
      static void project (array_sgraph_domain_t& inv, Iter it, Iter end) {
        inv.project (it, end);
      }
    };

    // Static data allocation
    template<class Dom, class Wt, bool IsDistWt>
    boost::unordered_map<landmark_ref<typename Dom::varname_t, typename Dom::number_t>,
                         landmark_ref<typename Dom::varname_t, typename Dom::number_t> >
    array_sparse_graph_domain<Dom,Wt,IsDistWt>::var_landmarks;

    template<class Dom, class Wt, bool IsDistWt>
    boost::unordered_map<landmark_ref<typename Dom::varname_t, typename Dom::number_t>,
                         landmark_ref<typename Dom::varname_t, typename Dom::number_t> >
    array_sparse_graph_domain<Dom,Wt,IsDistWt>::cst_landmarks;

  } // end namespace domains

} // end namespace crab
#pragma GCC diagnostic pop
#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#include "sparse_solver.h"
#include <Eigen/PaStiXSupport>
#include <unsupported/Eigen/SparseExtra>

template<typename T> void test_pastix_T()
{
  PastixLLT< SparseMatrix<T, ColMajor>, Eigen::Lower > pastix_llt_lower;
  PastixLDLT< SparseMatrix<T, ColMajor>, Eigen::Lower > pastix_ldlt_lower;
  PastixLLT< SparseMatrix<T, ColMajor>, Eigen::Upper > pastix_llt_upper;
  PastixLDLT< SparseMatrix<T, ColMajor>, Eigen::Upper > pastix_ldlt_upper;
  PastixLU< SparseMatrix<T, ColMajor> > pastix_lu;

  check_sparse_spd_solving(pastix_llt_lower);
  check_sparse_spd_solving(pastix_ldlt_lower);
  check_sparse_spd_solving(pastix_llt_upper);
  check_sparse_spd_solving(pastix_ldlt_upper);
  check_sparse_square_solving(pastix_lu);
}

// There is no support for selfadjoint matrices with PaStiX.
// Complex symmetric matrices should pass though
template<typename T> void test_pastix_T_LU()
{
  PastixLU< SparseMatrix<T, ColMajor> > pastix_lu;
  check_sparse_square_solving(pastix_lu);
}

void test_pastix_support()
{
  CALL_SUBTEST_1(test_pastix_T<float>());
  CALL_SUBTEST_2(test_pastix_T<double>());
  CALL_SUBTEST_3( (test_pastix_T_LU<std::complex<float> >()) );
  CALL_SUBTEST_4(test_pastix_T_LU<std::complex<double> >());
}


#include <iostream>

#include "EKFBallModel.hpp"

/* Lin Alg Includes */
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace std;
using namespace Geometry2d;

Modeling::EKFBallModel::EKFBallModel(RobotModel::RobotMap* robotMap,
                                     Configuration* config)
    : BallModel(robotMap, config),
      _processNoiseSqrdPos(config, "EKFModelBall/Process Noise Position", 0.2),
      _processNoiseSqrdVel(config, "EKFModelBall/Process Noise Velocity", 1.0),
      _measurementNoiseSqrd(config, "EKFModelBall/Measurement Noise Position",
                            0.01) {
    typedef boost::numeric::ublas::vector<double> Vector;
    typedef boost::numeric::ublas::matrix<double> Matrix;

    _ekf = new ExtendedKalmanFilter(Q, R);
}

Modeling::EKFBallModel::~EKFBallModel() {
    if (_ekf) {
        delete _ekf;
    }
}

void Modeling::EKFBallModel::singleUpdate(float dtime) {
    raoBlackwellizedParticleFilter->update(observedPos.x, observedPos.y, dtime);
    RbpfState* bestState = raoBlackwellizedParticleFilter->getBestFilterState();
    Point posOld = pos;
    pos.x = bestState->X(0);
    pos.y = bestState->X(1);
    vel.x = bestState->X(2);
    vel.y = bestState->X(3);
    accel.x = bestState->X(4);
    accel.y = bestState->X(5);
}

void Modeling::EKFBallModel::update(float dtime) {
    if (_observations.size() >=
        1) {  // currently hacked to just handle a single update
        // pick the closest observation to the current estimate
        float bestDist = 99999;
        for (const observation_type& observation : _observations) {
            if (observation.pos.distTo(pos) < bestDist) {
                bestDist = observation.pos.distTo(pos);
                observedPos = observation.pos;
            }
        }
        float dtime = (float)(_observations.at(0).time - lastUpdatedTime) / 1e6;
        singleUpdate(dtime);
    }
}

void Modeling::EKFBallModel::initParams();


#pragma once
#include <numeric>
#include <iostream>
#include <Eigen/Dense>
#include <autoppl/math/ess.hpp>

namespace ppl {

template <class Derived>
inline auto mean(const Eigen::MatrixBase<Derived>& m)
{ return m.colwise().mean(); }

template <class Derived>
inline auto sd(const Eigen::MatrixBase<Derived>& m)
{
    assert(m.rows() > 1);
    auto var = (m.rowwise() - ppl::mean(m))
                    .colwise().squaredNorm() / (m.rows() - 1);
    return var.array().sqrt().matrix();
}

inline void summary(const std::string& header,
                    const Eigen::MatrixXd& m,
                    double warmup_time,
                    double sampling_time)
{
    std::cout << "Warmup: " << warmup_time << std::endl;
    std::cout << "Sampling: " << sampling_time << std::endl;

    std::cout << header << std::endl;

    Eigen::MatrixXd mean = ppl::mean(m);
    std::cout << "Mean:\n"
              << mean << std::endl;

    Eigen::MatrixXd sd = ppl::sd(m);
    std::cout << "SD:\n"
              << sd << std::endl;

    Eigen::MatrixXd ess = ppl::math::ess(m);
    std::cout << "ESS:\n"
              << ess << std::endl;

    Eigen::MatrixXd ess_per_s = ess / sampling_time;
    std::cout << "ESS/s:\n"
              << ess_per_s << std::endl;
}

} // namespace ppl