#pragma once

/*
   Implementation of the abstract transfer functions by reducing them
   to abstract domain operations.

   These are the main Crab statements for which we define their abstract
   transfer functions:

   ARITHMETIC and BOOLEAN
     x := y bin_op z;
     x := y;
     assume (cst)
     assert (cst);
     x := select (cond, y, z);

   ARRAYS
     a[i] = v;    (v can be either an integer or pointer)
     v = a[i];
     a := b       (a and b arrays)

   POINTERS
     *p = q;
     p = *q;
     p := q+n
     p := &obj;
     p := &fun
     p := null;

   FUNCTIONS
     x := foo(arg1,...,argn);
     return r;

   havoc (x);

 */

#include <boost/optional.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/noncopyable.hpp>

#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/cfg/cfg.hpp>
#include <crab/domains/domain_traits.hpp>
#include <crab/domains/linear_constraints.hpp>

namespace crab {

  namespace analyzer {

  /**
   * API abstract transformer
   **/
  template<typename Number, typename VariableName>
  class abs_transformer_api: public cfg::statement_visitor<Number, VariableName>
  {
   public:

    typedef Number number_t;
    typedef VariableName varname_t;

    typedef ikos::variable <number_t, VariableName > var_t;
    typedef ikos::linear_expression<number_t, VariableName > lin_exp_t;
    typedef ikos::linear_constraint<number_t, VariableName > lin_cst_t;
    typedef ikos::linear_constraint_system<number_t, VariableName > lin_cst_sys_t;

    typedef cfg::havoc_stmt<number_t,VariableName>        havoc_t;
    typedef cfg::unreachable_stmt<number_t,VariableName>  unreach_t;

    typedef cfg::binary_op<number_t,VariableName>         bin_op_t;
    typedef cfg::assignment<number_t,VariableName>        assign_t;
    typedef cfg::assume_stmt<number_t,VariableName>       assume_t;
    typedef cfg::select_stmt<number_t,VariableName>       select_t;
    typedef cfg::assert_stmt<number_t,VariableName>       assert_t;

    typedef cfg::int_cast_stmt<number_t,VariableName>     int_cast_t;
    typedef cfg::callsite_stmt<number_t,VariableName>     callsite_t;
    typedef cfg::return_stmt<number_t,VariableName>       return_t;

    typedef cfg::array_assume_stmt<number_t,VariableName> arr_assume_t;
    typedef cfg::array_store_stmt<number_t,VariableName>  arr_store_t;
    typedef cfg::array_load_stmt<number_t,VariableName>   arr_load_t;
    typedef cfg::array_assign_stmt<number_t,VariableName> arr_assign_t;
    typedef cfg::ptr_store_stmt<number_t,VariableName>    ptr_store_t;
    typedef cfg::ptr_load_stmt<number_t,VariableName>     ptr_load_t;

    typedef cfg::ptr_assign_stmt<number_t,VariableName>   ptr_assign_t;
    typedef cfg::ptr_object_stmt<number_t,VariableName>   ptr_object_t;
    typedef cfg::ptr_function_stmt<number_t,VariableName> ptr_function_t;
    typedef cfg::ptr_null_stmt<number_t,VariableName>     ptr_null_t;
    typedef cfg::ptr_assume_stmt<number_t,VariableName>   ptr_assume_t;
    typedef cfg::ptr_assert_stmt<number_t,VariableName>   ptr_assert_t;

    typedef cfg::bool_binary_op<number_t,VariableName>    bool_bin_op_t;
    typedef cfg::bool_assign_cst<number_t,VariableName>   bool_assign_cst_t;
    typedef cfg::bool_assign_var<number_t,VariableName>   bool_assign_var_t;
    typedef cfg::bool_assume_stmt<number_t,VariableName>  bool_assume_t;
    typedef cfg::bool_select_stmt<number_t,VariableName>  bool_select_t;
    typedef cfg::bool_assert_stmt<number_t,VariableName>  bool_assert_t;

   protected:

    virtual void exec (havoc_t&) { }
    virtual void exec (unreach_t&) { }
    virtual void exec (bin_op_t&)  { }
    virtual void exec (assign_t&) { }
    virtual void exec (assume_t&) { }
    virtual void exec (select_t&) { }
    virtual void exec (assert_t&) { }
    virtual void exec (int_cast_t&) { }
    virtual void exec (callsite_t&) { }
    virtual void exec (return_t&) { }
    virtual void exec (arr_assume_t&) { }
    virtual void exec (arr_store_t&) { }
    virtual void exec (arr_load_t&) { }
    virtual void exec (arr_assign_t&) { }
    virtual void exec (ptr_store_t&) { }
    virtual void exec (ptr_load_t&) { }
    virtual void exec (ptr_assign_t&) { }
    virtual void exec (ptr_object_t&) { }
    virtual void exec (ptr_function_t&) { }
    virtual void exec (ptr_null_t&) { }
    virtual void exec (ptr_assume_t&) { }
    virtual void exec (ptr_assert_t&) { }
    virtual void exec (bool_bin_op_t&)  { }
    virtual void exec (bool_assign_cst_t&) { }
    virtual void exec (bool_assign_var_t&) { }
    virtual void exec (bool_assume_t&) { }
    virtual void exec (bool_select_t&) { }
    virtual void exec (bool_assert_t&) { }

   public: /* visitor api */

    void visit (havoc_t &s) { exec (s); }
    void visit (unreach_t &s) { exec (s); }
    void visit (bin_op_t &s) { exec (s); }
    void visit (assign_t &s) { exec (s); }
    void visit (assume_t &s) { exec (s); }
    void visit (select_t &s) { exec (s); }
    void visit (assert_t &s) { exec (s); }
    void visit (int_cast_t &s) { exec (s); }
    void visit (callsite_t &s) { exec (s); }
    void visit (return_t &s) { exec (s); }
    void visit (arr_assume_t &s) { exec (s); }
    void visit (arr_store_t &s) { exec (s); }
    void visit (arr_load_t &s) { exec (s); }
    void visit (arr_assign_t &s) { exec (s); }
    void visit (ptr_store_t &s) { exec (s); }
    void visit (ptr_load_t &s) { exec (s); }
    void visit (ptr_assign_t &s) { exec (s); }
    void visit (ptr_object_t &s) { exec (s); }
    void visit (ptr_function_t &s) { exec (s); }
    void visit (ptr_null_t &s) { exec (s); }
    void visit (ptr_assume_t &s) { exec (s); }
    void visit (ptr_assert_t &s) { exec (s); }
    void visit (bool_bin_op_t &s) { exec (s); }
    void visit (bool_assign_cst_t &s) { exec (s); }
    void visit (bool_assign_var_t &s) { exec (s); }
    void visit (bool_assume_t &s) { exec (s); }
    void visit (bool_select_t &s) { exec (s); }
    void visit (bool_assert_t &s) { exec (s); }
  };

  /**
   * Abstract forward transformer for all statements. Function calls
   * which are ingnored in a sound manner.
   **/
  template<class AbsD>
  class intra_abs_transformer:
      public abs_transformer_api<typename AbsD::number_t, typename AbsD::varname_t>,
      public boost::noncopyable {

   public:
    typedef AbsD abs_dom_t;
    typedef typename abs_dom_t::number_t number_t;
    typedef typename abs_dom_t::varname_t varname_t;
    typedef typename abs_dom_t::variable_t variable_t;

   public:
    typedef abs_transformer_api <number_t, varname_t> abs_transform_api_t;
    using typename abs_transform_api_t::var_t;
    using typename abs_transform_api_t::lin_exp_t;
    using typename abs_transform_api_t::lin_cst_t;
    using typename abs_transform_api_t::lin_cst_sys_t;
    using typename abs_transform_api_t::havoc_t;
    using typename abs_transform_api_t::unreach_t;
    using typename abs_transform_api_t::bin_op_t;
    using typename abs_transform_api_t::assign_t;
    using typename abs_transform_api_t::assume_t;
    using typename abs_transform_api_t::select_t;
    using typename abs_transform_api_t::assert_t;
    using typename abs_transform_api_t::int_cast_t;
    using typename abs_transform_api_t::callsite_t;
    using typename abs_transform_api_t::arr_assume_t;
    using typename abs_transform_api_t::arr_load_t;
    using typename abs_transform_api_t::arr_store_t;
    using typename abs_transform_api_t::arr_assign_t;
    using typename abs_transform_api_t::ptr_load_t;
    using typename abs_transform_api_t::ptr_store_t;
    using typename abs_transform_api_t::ptr_assign_t;
    using typename abs_transform_api_t::ptr_object_t;
    using typename abs_transform_api_t::ptr_function_t;
    using typename abs_transform_api_t::ptr_null_t;
    using typename abs_transform_api_t::ptr_assume_t;
    using typename abs_transform_api_t::ptr_assert_t;
    using typename abs_transform_api_t::bool_bin_op_t;
    using typename abs_transform_api_t::bool_assign_cst_t;
    using typename abs_transform_api_t::bool_assign_var_t;
    using typename abs_transform_api_t::bool_assume_t;
    using typename abs_transform_api_t::bool_select_t;
    using typename abs_transform_api_t::bool_assert_t;

   protected:
    /// XXX: the transformer does not own m_inv.
    abs_dom_t *m_inv;
    bool m_ignore_assert;

   private:

    template <typename NumOrVar>
    void apply (abs_dom_t &inv, binary_operation_t op,
		variable_t x, variable_t y, NumOrVar z)
    {
      auto op1 = conv_op<ikos::operation_t> (op);
      auto op2 = conv_op<ikos::div_operation_t> (op);
      auto op3 = conv_op<ikos::bitwise_operation_t> (op);
      if (op1)      inv.apply (*op1, x, y, z);
      else if (op2) inv.apply (*op2, x, y, z);
      else if (op3) inv.apply (*op3, x, y, z);
      else CRAB_ERROR("unsupported binary operator", op);
    }

    abs_dom_t *get_inv () {
      if (!m_inv)
	CRAB_ERROR ("Invariant passed to transformer cannot be null!");
      return m_inv;
    }

   public:

    intra_abs_transformer (abs_dom_t* inv, bool ignore_assert = false):
      m_inv (inv), m_ignore_assert(ignore_assert) { }

    virtual ~intra_abs_transformer () { }

    void set (abs_dom_t& inv) { m_inv = &inv;}

    abs_dom_t& inv () { return *get_inv(); }

    void exec (bin_op_t& stmt) {
      auto op1 = stmt.left ();
      auto op2 = stmt.right ();
      if (op1.get_variable () && op2.get_variable ()) {
        apply(*get_inv(), stmt.op (),
	      stmt.lhs(), (*op1.get_variable()), (*op2.get_variable()));
      } else {
        assert (op1.get_variable () && op2.is_constant ());
        apply(*get_inv(), stmt.op (),
	      stmt.lhs(), (*op1.get_variable ()), op2.constant ());
      }
    }

    void exec (select_t& stmt) {
      bool already_bot = false;

      if (get_inv()->is_bottom()) {
	already_bot = true;
      }

      abs_dom_t inv1 (*get_inv());
      abs_dom_t inv2 (*get_inv());

      inv1 += stmt.cond ();
      inv2 += stmt.cond ().negate ();

      if (!already_bot &&
	  (inv1.is_bottom() && inv2.is_bottom())) {
	CRAB_ERROR("select condition and its negation cannot be false simultaneously ",
		   stmt);
      }

      if (inv2.is_bottom()) {
        inv1.assign (stmt.lhs(), stmt.left());
        *get_inv() = inv1;
      }
      else if (inv1.is_bottom ()) {
        inv2.assign (stmt.lhs(), stmt.right());
        *get_inv() = inv2;
      }
      else {
        inv1.assign (stmt.lhs(), stmt.left());
        inv2.assign (stmt.lhs(), stmt.right());
        *get_inv() = inv1 | inv2;
      }
    }

    void exec (assign_t& stmt) {
      get_inv()->assign(stmt.lhs(), stmt.rhs());
    }

    void exec (assume_t& stmt) {
      *get_inv() += stmt.constraint();
    }

    void exec (assert_t& stmt) {
      if (m_ignore_assert) return;

      abs_dom_t cst;
      cst += stmt.constraint();
      abs_dom_t meet = cst & *get_inv();
      if (meet.is_bottom ()) {
	// assertion does not definitely hold.
        *get_inv() = abs_dom_t::bottom ();
      } else {
	*get_inv() += stmt.constraint ();
      }
    }

    void exec (int_cast_t &stmt){
      if (auto op = conv_op<crab::domains::int_conv_operation_t>(stmt.op())) {
	get_inv()->apply(*op, stmt.dst(), stmt.src());
      } else {
	CRAB_ERROR("unsupported cast operator ", stmt.op());
      }
    }

    void exec (bool_assign_cst_t& stmt) {
      (*get_inv()).assign_bool_cst (stmt.lhs(), stmt.rhs ());
    }

    void exec (bool_assign_var_t& stmt) {
      (*get_inv()).assign_bool_var (stmt.lhs(), stmt.rhs(), stmt.is_rhs_negated());
    }

    void exec (bool_bin_op_t& stmt) {
      if (auto op = conv_op<domains::bool_operation_t> (stmt.op()))
	(*get_inv()).apply_binary_bool (*op, stmt.lhs(), stmt.left(), stmt.right());
    }

    void exec (bool_assume_t& stmt) {
      (*get_inv()).assume_bool (stmt.cond(), stmt.is_negated ());
    }

    void exec (bool_select_t& stmt) {
      abs_dom_t inv1 (*get_inv());
      abs_dom_t inv2 (*get_inv());
      const bool negate = true;
      inv1.assume_bool(stmt.cond(), !negate);
      inv2.assume_bool(stmt.cond(), negate);
      if (inv2.is_bottom()) {
        inv1.assign_bool_var(stmt.lhs(), stmt.left(), !negate);
        *get_inv() = inv1;
      }
      else if (inv1.is_bottom ()) {
        inv2.assign_bool_var(stmt.lhs(), stmt.right(), !negate);
        *get_inv() = inv2;
      }
      else {
        inv1.assign_bool_var(stmt.lhs(), stmt.left(), !negate);
        inv2.assign_bool_var(stmt.lhs(), stmt.right(), !negate);
        *get_inv() = inv1 | inv2;
      }
    }

    void exec (bool_assert_t& stmt) {
      if (m_ignore_assert) return;

      abs_dom_t inv;
      inv.assume_bool (stmt.cond(), false);
      abs_dom_t meet = inv & *get_inv();
      if (meet.is_bottom ()) {
	// assertion does not definitely hold.
        *get_inv() = abs_dom_t::bottom ();
      } else {
	(*get_inv()).assume_bool (stmt.cond(), false);
      }
    }

    void exec (havoc_t& stmt)
    { (*get_inv()) -= stmt.variable(); }

    void exec (unreach_t& stmt)
    { *get_inv() = abs_dom_t::bottom (); }

    void exec (arr_assume_t &stmt) {
      get_inv()->array_assume (stmt.array (), stmt.lb_index (), stmt.ub_index (), stmt.val ());
    }

    void exec (arr_store_t &stmt) {
      get_inv()->array_store (stmt.array(), stmt.index (), stmt.value (),
			      stmt.elem_size(), stmt.is_singleton ());
    }

    void exec (arr_load_t  &stmt) {
      get_inv()->array_load (stmt.lhs (), stmt.array (), stmt.index(), stmt.elem_size());
    }

    void exec (arr_assign_t  &stmt) {
      get_inv()->array_assign (stmt.lhs (), stmt.rhs ());
    }

    void exec (ptr_null_t & stmt)
    { get_inv()->pointer_mk_null (stmt.lhs ()); }

    void exec (ptr_object_t & stmt)
    { get_inv()->pointer_mk_obj (stmt.lhs (), stmt.rhs()); }

    void exec (ptr_assign_t & stmt)
    { get_inv()->pointer_assign (stmt.lhs (), stmt.rhs (), stmt.offset ()); }

    void exec (ptr_function_t & stmt)
    { get_inv()->pointer_function (stmt.lhs (), stmt.rhs ()); }

    void exec (ptr_load_t & stmt)
    { get_inv()->pointer_load (stmt.lhs (), stmt.rhs ()); }

    void exec (ptr_store_t & stmt)
    { get_inv()->pointer_store (stmt.lhs (), stmt.rhs ()); }

    void exec (ptr_assume_t& stmt)
    { get_inv()->pointer_assume (stmt.constraint ()); }

    void exec (ptr_assert_t& stmt) {
      if (m_ignore_assert) return;
      get_inv()->pointer_assert (stmt.constraint ());
    }

    virtual void exec (callsite_t &cs) {
      for (auto vt: cs.get_lhs())
	(*get_inv()) -= vt;  // havoc
    }
  };

  /**
   * Abstract transformer to compute necessary preconditions of error
   * states.
   **/
  template<class AbsD, class InvT>
  class intra_necessary_preconditions_abs_transformer:
      public abs_transformer_api<typename AbsD::number_t,
				 typename AbsD::varname_t>,
      public boost::noncopyable {

   public:
    typedef AbsD abs_dom_t;
    typedef typename abs_dom_t::number_t number_t;
    typedef typename abs_dom_t::varname_t varname_t;
    typedef typename abs_dom_t::variable_t variable_t;
    typedef cfg::statement<number_t, varname_t> statement_t;
    typedef abs_transformer_api <number_t, varname_t> abs_transform_api_t;
    using typename abs_transform_api_t::var_t;
    using typename abs_transform_api_t::lin_exp_t;
    using typename abs_transform_api_t::lin_cst_t;
    using typename abs_transform_api_t::lin_cst_sys_t;
    using typename abs_transform_api_t::havoc_t;
    using typename abs_transform_api_t::unreach_t;
    using typename abs_transform_api_t::bin_op_t;
    using typename abs_transform_api_t::assign_t;
    using typename abs_transform_api_t::assume_t;
    using typename abs_transform_api_t::select_t;
    using typename abs_transform_api_t::assert_t;
    using typename abs_transform_api_t::int_cast_t;
    using typename abs_transform_api_t::callsite_t;
    using typename abs_transform_api_t::arr_assume_t;
    using typename abs_transform_api_t::arr_load_t;
    using typename abs_transform_api_t::arr_store_t;
    using typename abs_transform_api_t::arr_assign_t;
    using typename abs_transform_api_t::ptr_load_t;
    using typename abs_transform_api_t::ptr_store_t;
    using typename abs_transform_api_t::ptr_assign_t;
    using typename abs_transform_api_t::ptr_object_t;
    using typename abs_transform_api_t::ptr_function_t;
    using typename abs_transform_api_t::ptr_null_t;
    using typename abs_transform_api_t::ptr_assume_t;
    using typename abs_transform_api_t::ptr_assert_t;
    using typename abs_transform_api_t::bool_bin_op_t;
    using typename abs_transform_api_t::bool_assign_cst_t;
    using typename abs_transform_api_t::bool_assign_var_t;
    using typename abs_transform_api_t::bool_assume_t;
    using typename abs_transform_api_t::bool_select_t;
    using typename abs_transform_api_t::bool_assert_t;

   protected:

    // used to compute the (necessary) preconditions
    abs_dom_t *m_pre;
    // used to refine the preconditions: map from statement_t to
    // abs_dom_t.
    InvT &m_invariants;
    bool m_ignore_assert;
   public:

    intra_necessary_preconditions_abs_transformer(abs_dom_t* post, InvT &invars,
						  bool ignore_assert = false)

      : m_pre (post), m_invariants (invars), m_ignore_assert (ignore_assert) {
      if (!m_pre)
	CRAB_ERROR ("Postcondition cannot be null!");
    }

    virtual ~intra_necessary_preconditions_abs_transformer () { }

    abs_dom_t& preconditions () { return *m_pre; }

    void exec (bin_op_t& stmt) {

      auto op = conv_op<ikos::operation_t> (stmt.op ());
      if (!op) {
	//CRAB_WARN ("backward operation ", stmt.op(), " not implemented");
	(*m_pre) -= stmt.lhs();
	return;
      }

      auto op1 = stmt.left ();
      auto op2 = stmt.right ();
      abs_dom_t invariant = m_invariants[&stmt];

      CRAB_LOG("backward",
	       crab::outs () << "** " << stmt.lhs () << " := "
	                     << op1 << " " << *op << " " << op2 << "\n"
 	                     << "FORWARD INV=" << invariant << "\n"
	                     << "POST=" << *m_pre << "\n");

      if (op1.get_variable () && op2.get_variable ()) {
        m_pre->backward_apply (*op,
			       stmt.lhs (),
			       (*op1.get_variable ()),
			       (*op2.get_variable ()),
			       invariant);
      } else {
        assert (op1.get_variable () && op2.is_constant ());
        m_pre->backward_apply (*op,
			       stmt.lhs(),
			       (*op1.get_variable ()),
			       op2.constant(),
			       invariant);
      }
      CRAB_LOG("backward",
	       crab::outs () << "PRE=" << *m_pre << "\n");

    }

    // select(x := cond ? e1: e2, post) can be reduced to
    //   pre: goto b_then;
    //   pre: goto b_else;
    //   b_then:
    //     assume(cond);
    //     x := e1;
    //     goto post;
    //   b_else:
    //     assume(not(cond));
    //     x := e2;
    //     goto post;
    //   post: ....
    void exec (select_t& stmt) {
      abs_dom_t old_pre = m_invariants[&stmt];

      // -- one of the two branches is false
      abs_dom_t then_inv (old_pre);
      then_inv += stmt.cond();
      if (then_inv.is_bottom()) {
        m_pre->backward_assign (stmt.lhs(),stmt.right(),old_pre);
	*m_pre += stmt.cond().negate();
	return;
      }

      abs_dom_t else_inv (old_pre);
      else_inv += stmt.cond ().negate();
      if (else_inv.is_bottom()) {
        m_pre->backward_assign (stmt.lhs(),stmt.left(),old_pre);
	*m_pre += stmt.cond ();
	return;
      }

      // -- both branches can be possible so we join them
      abs_dom_t pre_then (*m_pre);
      pre_then.backward_assign(stmt.lhs(),stmt.left(),old_pre);
      pre_then += stmt.cond();

      abs_dom_t pre_else (*m_pre);
      pre_else.backward_assign(stmt.lhs(),stmt.right(),old_pre);
      pre_else += stmt.cond().negate();

      *m_pre = pre_then | pre_else;
    }

    // x := e
    void exec (assign_t& stmt) {
      abs_dom_t invariant = m_invariants[&stmt];

      CRAB_LOG("backward",
	       auto rhs = stmt.rhs();
	       crab::outs () << "** " << stmt.lhs () << " := " << rhs << "\n"
 	                     << "FORWARD INV=" << invariant << "\n"
	                     << "POST=" << *m_pre << "\n");

      m_pre->backward_assign (stmt.lhs(), stmt.rhs(), invariant);
      CRAB_LOG("backward",
	       crab::outs () << "PRE=" << *m_pre << "\n");
    }

    // assume(c)
    // the precondition must contain c so forward and backward are the same.
    void exec (assume_t& stmt) {
      CRAB_LOG("backward",
	       auto csts = stmt.constraint();
	       crab::outs () << "** assume(" << csts << ")\n"
	                     << "POST=" << *m_pre << "\n");

      *m_pre += stmt.constraint();

      CRAB_LOG("backward",
	       crab::outs () << "PRE=" << *m_pre << "\n");

    }

    // We are interested in computing preconditions of the error
    // states. Thus, we propagate backwards the negation of the
    // condition which represents the error states.
    void exec (assert_t& stmt) {
      if (m_ignore_assert) return;

      CRAB_LOG("backward",
	       auto csts = stmt.constraint();
	       crab::outs () << "** assert(" << csts << ")\n"
	                     << "POST=" << *m_pre << "\n");

      *m_pre = abs_dom_t::top();
      *m_pre += stmt.constraint().negate ();

      CRAB_LOG("backward",
	       crab::outs () << "PRE=" << *m_pre << "\n");
    }

    // We are interested in computing preconditions of the error states.
    // Thus, an unreachable state is always safe so we propagate true.
    void exec (unreach_t& stmt) {
      *m_pre = abs_dom_t::top ();
    }

    // x := *
    // x can be anything before the assignment
    void exec (havoc_t& stmt) {
      *m_pre -= stmt.variable();
    }

    // ret := *
    virtual void exec (callsite_t &cs) {
      for (auto vt: cs.get_lhs())
	*m_pre -= vt;
    }

    void exec (int_cast_t &stmt) {
      abs_dom_t invariant = m_invariants[&stmt];
      m_pre->backward_assign(stmt.dst(), stmt.src(), invariant);
    }

    void exec (bool_assign_cst_t &stmt)
    { *m_pre -= stmt.lhs(); }
    void exec (bool_assign_var_t &stmt)
    { *m_pre -= stmt.lhs(); }
    void exec (bool_bin_op_t &stmt)
    { *m_pre -= stmt.lhs(); }
    void exec (bool_select_t &stmt)
    { *m_pre -= stmt.lhs(); }

    void exec (bool_assume_t &stmt) {
      // same as forward
      m_pre->assume_bool (stmt.cond(), stmt.is_negated ());
    }

    void exec (bool_assert_t &stmt) {
      if (m_ignore_assert) return;

      *m_pre = abs_dom_t::top();
      m_pre->assume_bool (stmt.cond(), true /*negated*/);
    }

    void exec (arr_load_t  &stmt)
    { *m_pre -= stmt.lhs(); }
    // NOT IMPLEMENTED
    void exec (arr_assume_t &stmt) { }
    void exec (arr_store_t &stmt) { }
    void exec (arr_assign_t  &stmt) { }
    void exec (ptr_null_t &stmt) { }
    void exec (ptr_object_t &stmt) { }
    void exec (ptr_assign_t &stmt) { }
    void exec (ptr_function_t &stmt) { }
    void exec (ptr_load_t &stmt) { }
    void exec (ptr_store_t &stmt) { }
    void exec (ptr_assume_t &stmt) { }
    void exec (ptr_assert_t &stmt) { }
  };

  // Helper for function calls.
  template <typename AbsDom>
  static void unify (AbsDom &inv, variable_type ty,
		     typename AbsDom::variable_t lhs, typename AbsDom::variable_t rhs) {

    typedef typename AbsDom::linear_expression_t linear_expression_t;
    typedef typename AbsDom::variable_t variable_t;
    typedef typename AbsDom::number_t number_t;
    switch (ty)
    {
      case BOOL_TYPE:
	inv.assign_bool_var(lhs, rhs, false);
	break;
      case INT_TYPE:
      case REAL_TYPE:
	inv.assign (lhs, rhs);
	break;
      case PTR_TYPE:
	inv.pointer_assign (lhs, rhs, number_t (0));
	break;
      case ARR_BOOL_TYPE:
      case ARR_INT_TYPE:
      case ARR_REAL_TYPE:
      case ARR_PTR_TYPE:
	inv.array_assign (lhs, rhs);
	break;
      default:
	CRAB_ERROR ("unsuported type");
    }
  }

  /**
   * Abstract transformer specialized for computing summaries.
   * class SumTable stores the summaries
   **/
  template<class SumTable>
  class bu_summ_abs_transformer:
      public intra_abs_transformer <typename SumTable::abs_domain_t> {

   public:

    typedef typename SumTable::abs_domain_t summ_abs_domain_t;
    typedef summ_abs_domain_t abs_dom_t;
    typedef typename abs_dom_t::number_t number_t;

   private:

    typedef intra_abs_transformer<abs_dom_t> intra_abs_transform_t;
    typedef typename intra_abs_transform_t::abs_transform_api_t abs_transform_api_t;
    typedef typename abs_dom_t::varname_t varname_t;
    typedef typename abs_dom_t::variable_t variable_t;
    typedef typename abs_dom_t::linear_expression_t linear_expression_t;

    SumTable* m_sum_tbl;

  public:

    using typename abs_transform_api_t::callsite_t;

    static void reuse_summary (abs_dom_t& caller,
                               const callsite_t& cs,
                               const typename SumTable::Summary& summ) {
      // Before a summary is plug-in we rename it with unique variable
      // names so we avoid naming clashes in cases like for instance
      // summary variables have same names as lhs of callsites.

      // error if cs and function declaration associated with summ are
      // not type consistent
      summ.check_type_consistency (cs);

      CRAB_LOG("inter",
               crab::outs () << "    Reuse summary at " << cs << "\n";
               crab::outs () << "    Summary:" << summ << "\n";);

      std::set<variable_t> actuals, formals;
      // --- matching formal and actual parameters
      auto inputs = summ.get_renamed_inputs ();
      unsigned i=0;
      // XXX: propagating down
      for (auto p : inputs) {
        auto a = cs.get_arg_name (i);
        if (!(a == p)) {
          CRAB_LOG ("inter",
                    crab::outs () << "\t\tPropagate from caller to callee "
                                  << p << ":=" << a << "\n");
	  unify (caller, cs.get_arg_type(i), p, a);
        }
        ++i;
        actuals.insert (a); formals.insert (p);
      }

      // --- meet caller's inv with summ
      auto sum_inv = summ.get_renamed_sum ();
      caller = caller & sum_inv;
      CRAB_LOG("inter",
	       crab::outs() << "\t\tAfter meet caller and summary: "
	                    <<  caller << "\n");

      // --- matching callsite's lhs and callee's return value
      // XXX: propagate from the return values in the callee to the
      // lhs variables of the callsite in the caller.
      auto const &caller_vts = cs.get_lhs ();
      auto const &callee_outs = summ.get_renamed_outputs ();
      assert (caller_vts.size () == callee_outs.size ());

      auto caller_it = caller_vts.begin();
      auto caller_et = caller_vts.end();
      auto callee_it = callee_outs.begin();

      // XXX: propagating up
      for (; caller_it != caller_et; ++caller_it, ++callee_it){
        auto vt = *caller_it;
        auto r = *callee_it;
        CRAB_LOG ("inter",
                  crab::outs () << "\t\tPropagate from callee to caller "
		                << vt << ":=" << r << "\n");
	unify (caller, vt.get_type(), vt, r);
        actuals.insert (vt); formals.insert (r);
      }

      // --- remove from caller only formal parameters so we can keep
      //     as much context from the caller as possible
      std::set<variable_t> vs;
      boost::set_difference (formals, actuals, std::inserter(vs, vs.end ()));
      domains::domain_traits<abs_dom_t>::forget (caller, vs.begin (), vs.end ());

      CRAB_LOG ("inter",
                crab::outs() << "\t\tAfter forgetting formal parameters {";
                for (auto v: vs) crab::outs () << v << ";";
                crab::outs () << "}=" <<  caller << "\n");
    }

   public:

    bu_summ_abs_transformer(abs_dom_t* inv, SumTable* sum_tbl)
        : intra_abs_transform_t(inv), m_sum_tbl (sum_tbl) { }

    virtual void exec (callsite_t &cs) override
    {
      if (!m_sum_tbl)
        CRAB_WARN ("The summary table is empty: ignored analysis of callsite");
      else
      {
	if (m_sum_tbl->hasSummary (cs)) {
	  auto summ = m_sum_tbl->get (cs);
	  reuse_summary (*(this->m_inv), cs, summ);
	}
	else
	{
	  CRAB_LOG("inter",
		   crab::outs() << "\tSummary not found for " << cs << "\n");
	  for (auto vt: cs.get_lhs())
	    *(this->m_inv) -= vt;  // havoc
	}
      }
    }
  };

  /// Conversion between domains
  template<typename Domain>
  inline void convert_domains (Domain from, Domain& to) { to = from; }

  template<typename Domain1, typename Domain2>
  inline void convert_domains (Domain1 from, Domain2& to) {
    CRAB_LOG("inter",
	     crab::outs () << "Converting from "
	                   << Domain1::getDomainName () << " to "
	                   << Domain2::getDomainName () << " might lose precision if "
	                   << Domain1::getDomainName () << " is more precise than "
	                   << Domain2::getDomainName () << "\n");

    for (auto cst : from.to_linear_constraint_system ())
    { to += cst; }
  }

  /**
   * Abstract transformer specialized for performing top-down forward
   * traversal while reusing summaries at the callsites.
   * class CallCtxTable stores the calling context.
   **/
  template<class SumTable, class CallCtxTable>
  class td_summ_abs_transformer:
      public intra_abs_transformer<typename CallCtxTable::abs_domain_t> {

   public:

    typedef typename SumTable::abs_domain_t     summ_abs_domain_t;
    typedef typename CallCtxTable::abs_domain_t call_abs_domain_t;
    typedef call_abs_domain_t abs_dom_t;
    typedef typename abs_dom_t::number_t number_t;

   private:

    typedef intra_abs_transformer<abs_dom_t> intra_abs_transform_t;

   public:

    typedef typename intra_abs_transform_t::abs_transform_api_t abs_transform_api_t;
    using typename abs_transform_api_t::callsite_t;

   private:

    SumTable* m_sum_tbl;
    CallCtxTable* m_call_tbl;

   public:

    td_summ_abs_transformer (abs_dom_t* inv,
			     SumTable* sum_tbl, CallCtxTable* call_tbl)
        : intra_abs_transform_t(inv),
          m_sum_tbl (sum_tbl), m_call_tbl (call_tbl) { }

    virtual void exec (callsite_t &cs) override
    {
      if (!m_sum_tbl) {
        CRAB_WARN ("The summary table is empty");
      } else if (m_sum_tbl->hasSummary (cs)) {
        auto summ = m_sum_tbl->get (cs);

        // error if cs and function declaration associated with summ
        // are not type consistent
        summ.check_type_consistency (cs);

        CRAB_LOG ("inter",
                  crab::outs () << "    Pluging caller context into callee\n"
		                << "    Summary: " << summ << "\n");

        ///////
        /// Generate the callee context and store it.
        ///////
	typedef typename abs_dom_t::variable_t variable_t;

        abs_dom_t callee_ctx_inv (*(this->m_inv));
        // --- matching formal and actual parameters
        // XXX: propagating down
        unsigned i=0;
        auto inputs = summ.get_inputs ();
        for (variable_t p : inputs) {
          variable_t a = cs.get_arg_name (i);
          if (!(a == p))
	    unify (callee_ctx_inv, cs.get_arg_type(i), p, a);
          ++i;
        }

        // --- project only onto formal parameters
        domains::domain_traits<abs_dom_t>::project (callee_ctx_inv,
                                                    inputs.begin (),
                                                    inputs.end ());
        // --- store the callee context
        CRAB_LOG ("inter",
                  crab::outs() << "\t\tCallee context stored: "
                               << callee_ctx_inv << "\n");
        m_call_tbl->insert (cs, callee_ctx_inv);

        /////
        // Generate the continuation at the caller
        /////

        // --- convert this->m_inv to the language of summ_abs_dom_t (summ)
        summ_abs_domain_t caller_ctx_inv;
        convert_domains(*(this->m_inv), caller_ctx_inv);
	// forget the variables of the lhs of the callsite, otherwise
	// caller_ctx_inv and m_inv may be inconsistent if the lhs
	// variables are constrained before the callsite (e.g., x=-5;
	// x := abs(x);)
	for (auto vt: cs.get_lhs())
	{ *(this->m_inv) -= vt; }

        CRAB_LOG ("inter",
                  crab::outs() << "\t\tCaller context: " <<  caller_ctx_inv << "\n");

        // --- reuse summary to do the continuation
        bu_summ_abs_transformer<SumTable>::
	  reuse_summary (caller_ctx_inv, cs, summ);
        CRAB_LOG ("inter",
                  crab::outs() << "\t\tCaller context after plugin summary: "
                               << caller_ctx_inv << "\n");

        // --- convert back inv to the language of abs_dom_t
        convert_domains(caller_ctx_inv, *(this->m_inv));
        CRAB_LOG ("inter",
                  crab::outs() << "\t\tCaller continuation after "
  		               <<  cs << "=" <<  *(this->m_inv) << "\n");
        return;
      } else {
        // --- no summary found: do nothing
      }

      // We could not reuse a summary so we just havoc lhs of the call
      // site.
      for (auto vt: cs.get_lhs())
      { *(this->m_inv) -= vt; }
    }
  };

  } // end namespace
} // end namespace


#ifndef CAFFE_SAMPLE_SELECTOR_HPP_
#define CAFFE_SAMPLE_SELECTOR_HPP_

#include <map>
#include <string>
#include <vector>
#include <boost/thread.hpp>
#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/internal_thread.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/util/math_functions.hpp"
namespace caffe {
  template <typename Dtype>
  class SampleSelector {
   public:
    explicit SampleSelector(const LayerParameter& param);
    void ProcessLabelSelectParam();
    void ReadLabelProbMappingFile(const string& file_name);
    void ComputeLabelSkipRate();
    bool AcceptGivenLabel(const int label);
    int  GetConvertedLabel(const int label);
    const float*  Get_Label_prob_gpu_data();
	const float*  Get_Label_prob_cpu_data();
	void  Compute_label_prob_fromBlob(Blob<Dtype>* labelBlob);
  protected:
    void InitRand();
    unsigned int PrefetchRand();
    const LayerParameter param_;
    //const LabelSelectParameter param_;
    std::map <int, float> label_prob_map_;
    std::map <int, unsigned int> label_skip_rate_map_;
    std::map <int, int> label_mapping_map_;
    unsigned  int num_labels_;
    unsigned  int num_labels_with_prob_ ;
    bool      balancing_label_;
    unsigned int num_top_label_balance_;
    bool  rest_of_label_mapping_;
    bool  map2order_label_;
    bool  ignore_rest_of_label_;
    int   rest_of_label_mapping_label_;
    float rest_of_label_prob_;
    shared_ptr<Caffe::RNG> rng_;
    Blob<int> label_skip_rate_blob_;
    Blob<float> label_prob_weight_blob_;
  };
}
#endif


#include <core/clams/trajectory.h>
#include <core/stream_sequence/stream_sequence_base.h>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

using namespace std;
namespace bfs = boost::filesystem;
using namespace Eigen;
using namespace clams;

int main(int argc, char** argv)
{
  namespace bpo = boost::program_options;
  bpo::options_description opts_desc("Allowed options");
  bpo::positional_options_description p;

  string sseq_path;
  string src;
  string dst;
  opts_desc.add_options()
    ("help,h", "produce help message")
    ("sseq", bpo::value(&sseq_path)->required(), "StreamSequence")
    ("src", bpo::value(&src)->required(), "Freiburg trajectory file")
    ("dst", bpo::value(&dst)->required(), "Clams trajectory file")
    ;

  p.add("sseq", 1);
  p.add("src", 1);
  p.add("dst", 1);

  bpo::variables_map opts;
  bpo::store(bpo::command_line_parser(argc, argv).options(opts_desc).positional(p).run(), opts);
  bool badargs = false;
  try { bpo::notify(opts); }
  catch(...) { badargs = true; }
  if(opts.count("help") || badargs) {
    cout << "Usage: " << argv[0] << " [OPTS] SSEQ SRC DST" << endl;
    cout << endl;
    cout << opts_desc << endl;
    return 1;
  }

  // StreamSequenceBase::Ptr sseq = StreamSequenceBase::initializeFromDirectory(sseq_path);
  Trajectory traj;
  // traj.resize(sseq->size());
  if(!bfs::is_regular_file(bfs::path(sseq_path + "/assoc.txt")))
  {
    ROS_ERROR("assoc.txt not found");
    return 1;
  }

  ifstream frei;
  ifstream assoc;
  frei.open(src.c_str());
  assoc.open((sseq_path + "/assoc.txt").c_str());
  // read all timestamp from assoc
  vector<double> timestamps;
  while(true) {
    double t1, t2;
    std::string f1, f2;
    assoc >> t1 >> f1 >> t2 >> f2;
    if(assoc.eof())
      break;
    // cout << t1 << f1 << t2 << f2 << "\n";
    timestamps.push_back(t1);
  }
  assoc.close();
  traj.resize(timestamps.size());
  while(true) {
    double timestamp, tx, ty, tz, qx, qy, qz, qw;
    frei >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    if(frei.eof())
      break;

    double dt = timestamp;
    // size_t idx = sseq->seek(timestamp, &dt);
    size_t idx = 0;
    for(size_t i = 0; i < timestamps.size(); ++i)
    {
      if(fabs(timestamps.at(i) - timestamp) < dt)
      {
	idx = i;
	dt = fabs(timestamps.at(i) - timestamp);
      }
    }

    ROS_ASSERT(dt < 1e-6);

    Quaternion<double> rotation(qw, qx, qy, qz);
    Translation<double, 3> translation(tx, ty, tz);
    Affine3d transform = translation * rotation;
    traj.set(idx, transform);
  }

  traj.save(dst);
  cout << "Saved to " << dst << endl;
  frei.close();
  return 0;
}


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/make_shared.hpp>
#include <memory>

#include "tudat/astro/aerodynamics/testApolloCapsuleCoefficients.h"
#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"
#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"
#include "tudat/io/basicInputOutput.h"
#include <limits>
#include <string>

#include <Eigen/Core>

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_propagation_stopping_conditions )

std::shared_ptr< propagators::PropagationTerminationSettings > getTerminationSettings( const int testType )
{

    // Define stopping conditions, depending on test case.
    std::shared_ptr< propagators::PropagationTerminationSettings > terminationSettings;
    switch( testType )
    {
    // Stop at given time.
    case 0:
        terminationSettings = std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 );
        break;
        // Stop at given Mach number
    case 1:
        terminationSettings = std::make_shared< propagators::PropagationDependentVariableTerminationSettings >(
                    std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                        propagators::mach_number_dependent_variable, "Apollo" ), 3.0, 1 );
        break;
        // Stop at given altitude
    case 2:
        terminationSettings = std::make_shared< propagators::PropagationDependentVariableTerminationSettings >(
                    std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                        propagators::altitude_dependent_variable, "Apollo" ), 10.0E3, 1 );
        break;
        // Stop at given density
    case 3:
        terminationSettings = std::make_shared< propagators::PropagationDependentVariableTerminationSettings >(
                    std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                        propagators::local_density_dependent_variable, "Apollo" ), 1.1, 0);
        break;
        // Stop when a single of the conditions 0-3 is fulfilled.
    case 4:
    {
        std::vector< std::shared_ptr< propagators::PropagationTerminationSettings > > constituentSettings;
        constituentSettings.push_back( getTerminationSettings( 0 ) );
        constituentSettings.push_back( getTerminationSettings( 1 ) );
        constituentSettings.push_back( getTerminationSettings( 2 ) );
        constituentSettings.push_back( getTerminationSettings( 3 ) );

        terminationSettings = std::make_shared< propagators::PropagationHybridTerminationSettings >(
                    constituentSettings, 1 );
        break;
    }
        // Stop when all of the conditions 0-3 is fulfilled.
    case 5:
    {
        std::vector< std::shared_ptr< propagators::PropagationTerminationSettings > > constituentSettings;
        constituentSettings.push_back( getTerminationSettings( 0 ) );
        constituentSettings.push_back( getTerminationSettings( 1 ) );
        constituentSettings.push_back( getTerminationSettings( 2 ) );
        constituentSettings.push_back( getTerminationSettings( 3 ) );

        terminationSettings = std::make_shared< propagators::PropagationHybridTerminationSettings >(
                    constituentSettings, 0 );
        break;
    }
    }
    return terminationSettings;
}

void performSimulation( const int testType )
{
    using namespace ephemerides;
    using namespace interpolators;
    using namespace numerical_integrators;
    using namespace spice_interface;
    using namespace simulation_setup;
    using namespace basic_astrodynamics;
    using namespace orbital_element_conversions;
    using namespace propagators;
    using namespace aerodynamics;
    using namespace basic_mathematics;
    using namespace input_output;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation start epoch.
    const double simulationStartEpoch = 0.0;

    // Set simulation end epoch.
    const double simulationEndEpoch = 3300.0;

    // Set numerical integration fixed step size.
    const double fixedStepSize = 1.0;

    // Set Keplerian elements for Capsule.
    Vector6d apolloInitialStateInKeplerianElements;
    apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3;
    apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005;
    apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
    apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = unit_conversions::convertDegreesToRadians( 235.7 );
    apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = unit_conversions::convertDegreesToRadians( 23.4 );
    apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

    // Convert apollo state from Keplerian elements to Cartesian elements.
    const Eigen::Vector6d apolloInitialState = convertKeplerianToCartesianElements(
                apolloInitialStateInKeplerianElements,
                getBodyGravitationalParameter( "Earth" ) );

    // Define simulation body settings.
    BodyListSettings bodySettings =
            getDefaultBodySettings( { "Earth" }, simulationStartEpoch - 10.0 * fixedStepSize,
                                    simulationEndEpoch + 10.0 * fixedStepSize, "SSB", "J2000"  );
    bodySettings.at( "Earth" )->ephemerisSettings = std::make_shared< simulation_setup::ConstantEphemerisSettings >(
                Eigen::Vector6d::Zero( ), "SSB", "J2000" );
    bodySettings.at( "Earth" )->gravityFieldSettings =
            std::make_shared< simulation_setup::GravityFieldSettings >(
                central_spice );
    bodySettings.at( "Earth" )->rotationModelSettings->resetOriginalFrame( "J2000" );

    // Create Earth object
    simulation_setup::SystemOfBodies bodies = simulation_setup::createSystemOfBodies( bodySettings );

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    // Create vehicle objects.
    bodies.createEmptyBody( "Apollo" );

    // Create vehicle aerodynamic coefficients
    bodies.at( "Apollo" )->setAerodynamicCoefficientInterface(
                unit_tests::getApolloCoefficientInterface( ) );
    bodies.at( "Apollo" )->setConstantBodyMass( 5.0E3 );

    // Define acceleration model settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfApollo;
    accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) );
    accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) );
    accelerationMap[ "Apollo" ] = accelerationsOfApollo;

    bodiesToPropagate.push_back( "Apollo" );
    centralBodies.push_back( "Earth" );

    // Set initial state
    Eigen::Vector6d systemInitialState = apolloInitialState;

    // Create acceleration models and propagation settings, using current test case to retrieve stop settings..
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState,
              getTerminationSettings( testType ) );
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, simulationStartEpoch, fixedStepSize );

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings, true, false, false );

    std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution =
            dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    // Check whether propagation has stopped at given conditions
    switch( testType )
    {
    // Check whether propagation stopped when passing t=3200.0
    case 0:
    {

        BOOST_CHECK_EQUAL( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) &&
                           ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 ), true );
        break;
    }
    case 1:
    {
        // Compute Mach number for last two time steps, and check whether last step was first to pass below Mach=3
        std::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel =
                dynamicsSimulator.getDynamicsStateDerivative( );
        stateDerivativeModel->computeStateDerivative(
                    (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second );
        std::shared_ptr< AtmosphericFlightConditions > flightConditions =
                std::dynamic_pointer_cast< AtmosphericFlightConditions >( bodies.at( "Apollo" )->getFlightConditions( ) );
        double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );
        stateDerivativeModel->computeStateDerivative(
                    (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second );
        double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );

        BOOST_CHECK_EQUAL( ( lastMachNumber <= 3.0 ) && ( secondToLastMachNumber > 3.0 ), true );

        break;
    }
    case 2:
    {
        // Compute altitude for last two time steps, and check whether last step was first to pass below altitude = 10 km
        std::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel =
                dynamicsSimulator.getDynamicsStateDerivative( );
        stateDerivativeModel->computeStateDerivative(
                    (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second );
        std::shared_ptr< FlightConditions > flightConditions = bodies.at( "Apollo" )->getFlightConditions( );
        double secondToLastAltitude = flightConditions->getCurrentAltitude( );
        stateDerivativeModel->computeStateDerivative(
                    (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second );
        double lastAltitude = flightConditions->getCurrentAltitude( );

        BOOST_CHECK_EQUAL( ( lastAltitude <= 10.0E3 ) && ( secondToLastAltitude > 10.0E3 ), true );

        break;
    }
    case 3:
    {
        // Compute density for last two time steps, and check whether last step was first to pass below density = 1.1 kg/m3
        std::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel =
                dynamicsSimulator.getDynamicsStateDerivative( );
        stateDerivativeModel->computeStateDerivative(
                    (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second );
        std::shared_ptr< AtmosphericFlightConditions > flightConditions =
                std::dynamic_pointer_cast< AtmosphericFlightConditions >( bodies.at( "Apollo" )->getFlightConditions( ) );
        double secondToLastDensity = flightConditions->getCurrentDensity( );
        stateDerivativeModel->computeStateDerivative(
                    (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second );
        double lastDensity = flightConditions->getCurrentDensity( );

        BOOST_CHECK_EQUAL( ( lastDensity >= 1.1 ) && ( secondToLastDensity < 1.1 ), true );

        break;
    }
        // Check whether at least a single of the conditions for case 0-3 was first reached at last time step.
    case 4:
    {
        std::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel =
                dynamicsSimulator.getDynamicsStateDerivative( );
        stateDerivativeModel->computeStateDerivative(
                    (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second );
        std::shared_ptr< AtmosphericFlightConditions > flightConditions =
                std::dynamic_pointer_cast< AtmosphericFlightConditions >( bodies.at( "Apollo" )->getFlightConditions( ) );

        double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );
        double secondToLastAltitude = flightConditions->getCurrentAltitude( );
        double secondToLastDensity = flightConditions->getCurrentDensity( );

        stateDerivativeModel->computeStateDerivative(
                    (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second );

        double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );
        double lastAltitude = flightConditions->getCurrentAltitude( );
        double lastDensity = flightConditions->getCurrentDensity( );

        BOOST_CHECK_EQUAL( ( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) && ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 ) ) ||
                           ( ( lastMachNumber <= 3.0 ) && ( secondToLastMachNumber > 3.0 ) ) ||
                           ( ( lastAltitude <= 10.0E3 ) && ( secondToLastAltitude > 10.0E3 ) ) ||
                           ( ( lastDensity >= 1.1 ) && ( secondToLastDensity < 1.1 ) ), 1 );

        break;
    }
        // Check whether all conditions for case 0-3 was first reached at last time step.
    case 5:
    {
        std::shared_ptr< DynamicsStateDerivativeModel< double, double > > stateDerivativeModel =
                dynamicsSimulator.getDynamicsStateDerivative( );
        stateDerivativeModel->computeStateDerivative(
                    (--(--( numericalSolution.end( ) ) ) )->first, (--(--( numericalSolution.end( ) ) ) )->second );
        std::shared_ptr< AtmosphericFlightConditions > flightConditions =
                std::dynamic_pointer_cast< AtmosphericFlightConditions >( bodies.at( "Apollo" )->getFlightConditions( ) );

        double secondToLastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );
        double secondToLastAltitude = flightConditions->getCurrentAltitude( );
        double secondToLastDensity = flightConditions->getCurrentDensity( );

        stateDerivativeModel->computeStateDerivative(
                    (--( numericalSolution.end( ) ) )->first, (--( numericalSolution.end( ) ) )->second );

        double lastMachNumber = flightConditions->getCurrentAirspeed( ) / flightConditions->getCurrentSpeedOfSound( );
        double lastAltitude = flightConditions->getCurrentAltitude( );
        double lastDensity = flightConditions->getCurrentDensity( );

        BOOST_CHECK_EQUAL( ( ( (--( numericalSolution.end( ) ) )->first >= 3200.0 ) ) &&
                           ( ( lastMachNumber <= 3.0 ) ) &&
                           ( ( lastAltitude <= 10.0E3 ) ) &&
                           ( ( lastDensity >= 1.1 ) ), 1 );

        BOOST_CHECK_EQUAL( ( (--(--( numericalSolution.end( ) ) ) )->first < 3200.0 )||
                           ( ( secondToLastMachNumber > 3.0 ) ) ||
                           ( ( secondToLastAltitude > 10.0E3 ) ) ||
                           ( ( secondToLastDensity < 1.1 ) ), 1 );

        break;
    }

    }
}

//! Test to perform propagation of Apollo capsule for various stopping conditions, and checking whether the final state
//! corresponds to condition that was given.
BOOST_AUTO_TEST_CASE( testPropagationStoppingConditions )
{
    for( unsigned int i = 0; i < 6; i++ )
    {
        performSimulation( i );
    }
}

//! Test to see if environment is updated to evaluate stopping conditions if required
//! (test uses altitude above Earth for Kepler orbit dynamics)
BOOST_AUTO_TEST_CASE( testPropagationStoppingConditionsWithDependentVariableUpdate )
{
    using namespace tudat;
    using namespace tudat::simulation_setup;
    using namespace tudat::propagators;
    using namespace tudat::numerical_integrators;
    using namespace tudat::orbital_element_conversions;
    using namespace tudat::basic_mathematics;
    using namespace tudat::unit_conversions;

    spice_interface::loadStandardSpiceKernels( );

    // Create body objects.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Earth" );
    BodyListSettings bodySettings =
            getDefaultBodySettings( bodiesToCreate );
    bodySettings.at( "Earth" )->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                Eigen::Vector6d::Zero( ) );

    // Create bodies
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );
    bodies.createEmptyBody( "Asterix" );

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;
    bodiesToPropagate.push_back( "Asterix" );
    centralBodies.push_back( "Earth" );

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
    accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::point_mass_gravity ) );
    accelerationMap[ "Asterix" ] = accelerationsOfAsterix;

    // Create acceleration models and propagation settings.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Set initial conditions for the Asterix satellite that will be propagated in this simulation.
    // The initial conditions are given in Keplerian elements and later on converted to Cartesian
    // elements.

    // Set Keplerian elements for Asterix.
    Eigen::Vector6d asterixInitialStateInKeplerianElements =Eigen::Vector6d::Zero( );
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7000.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.5;
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 180.0 );

    // Convert Asterix state from Keplerian elements to Cartesian elements.
    double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements( asterixInitialStateInKeplerianElements,
                earthGravitationalParameter );

    // Define termination conditions
    std::shared_ptr< PropagationTerminationSettings > stoppingCondition =
            std::make_shared< PropagationDependentVariableTerminationSettings >(
                std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                propagators::altitude_dependent_variable, "Asterix", "Earth" ), 25.0E3, 1 );

    // Create simulation object and propagate dynamics.
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, stoppingCondition );
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, 0.0, 10.0 );
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodies, integratorSettings, propagatorSettings );
    std::map< double, Eigen::VectorXd > integrationResult = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    // Check whether termination done correctly
    auto stateIterator = integrationResult.rbegin( );
    BOOST_CHECK_EQUAL(
               ( stateIterator->second.segment( 0, 3 ).norm( ) -
                 tudat::spice_interface::getAverageRadius( "Earth" ) ) < 25.0E3, true );
    stateIterator++;
    BOOST_CHECK_EQUAL(
               ( stateIterator->second.segment( 0, 3 ).norm( ) -
                 tudat::spice_interface::getAverageRadius( "Earth" ) ) < 25.0E3, false );
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


//
// Copyright © 2019 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "PeriodicCounterSelectionCommandHandler.hpp"
#include "ProfilingUtils.hpp"

#include <boost/numeric/conversion/cast.hpp>

namespace armnn
{

namespace profiling
{

using namespace std;
using boost::numeric_cast;

void PeriodicCounterSelectionCommandHandler::ParseData(const Packet& packet, CaptureData& captureData)
{
    std::vector<uint16_t> counterIds;
    uint32_t sizeOfUint32 = numeric_cast<uint32_t>(sizeof(uint32_t));
    uint32_t sizeOfUint16 = numeric_cast<uint32_t>(sizeof(uint16_t));
    uint32_t offset = 0;

    if (packet.GetLength() > 0)
    {
        if (packet.GetLength() >= 4)
        {
            captureData.SetCapturePeriod(ReadUint32(reinterpret_cast<const unsigned char*>(packet.GetData()), offset));

            unsigned int counters = (packet.GetLength() - 4) / 2;

            if (counters > 0)
            {
                counterIds.reserve(counters);
                offset += sizeOfUint32;
                for(unsigned int pos = 0; pos < counters; ++pos)
                {
                    counterIds.emplace_back(ReadUint16(reinterpret_cast<const unsigned char*>(packet.GetData()),
                                            offset));
                    offset += sizeOfUint16;
                }
            }

            captureData.SetCounterIds(counterIds);
        }
    }
}

void PeriodicCounterSelectionCommandHandler::operator()(const Packet& packet)
{
    CaptureData captureData;

    ParseData(packet, captureData);

    vector<uint16_t> counterIds = captureData.GetCounterIds();

    m_CaptureDataHolder.SetCaptureData(captureData.GetCapturePeriod(), counterIds);

    m_CaptureThread.Start();

    // Write packet to Counter Stream Buffer
    m_SendCounterPacket.SendPeriodicCounterSelectionPacket(captureData.GetCapturePeriod(), captureData.GetCounterIds());
}

} // namespace profiling

} // namespace armnn


#include "config.h"

#include <fstream>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <cmath> // round

#include <iostream>
namespace hft{

BasicContract::BasicContract(const std::string& sym, const std::string& loc_sym,
                             const std::string& sec_type, const std::string& currency,
                             const std::string& exch)
    : m_sym(uppercase(sym)), m_loc_sym(uppercase(loc_sym)), m_sec_type(uppercase(sec_type)),
    m_currency(uppercase(currency)), m_exch(uppercase(exch))
{
}

std::string BasicContract::sym() const { return m_sym; }
std::string BasicContract::loc_sym() const { return m_loc_sym; }
std::string BasicContract::sec_type() const { return m_sec_type; }
std::string BasicContract::currency() const { return m_currency; }
std::string BasicContract::exch() const { return m_exch; }

std::string BasicContract::uppercase(std::string strToConvert)
{
    std::transform(strToConvert.begin(), strToConvert.end(), strToConvert.begin(), ::toupper);
    boost::algorithm::trim(strToConvert);
    return strToConvert;
}

FutTradingContract::FutTradingContract(const std::string& sym,
                                       const std::string& loc_sym,
                                       const std::string& sec_type,
                                       const std::string& currency,
                                       const std::string& exch,
                                       float min_tick,
                                       float commiss_per_contract,
                                       unsigned multiplier,
                                       unsigned chillness,
                                       unsigned num_contracts)
    : BasicContract(sym, loc_sym, sec_type, currency, exch),
    m_mt(min_tick),
    m_cpc(commiss_per_contract),
    m_mlt(multiplier),
    m_c(chillness),
    m_nc(num_contracts)
{
}

FutCalendarSpreadConfig::FutCalendarSpreadConfig(const std::string& file)
{

    std::ifstream fs(file);
    if( fs.good() ){

        std::string _root, _st, _exch, _lsn, _lsf, _mt, _cpc, _ncpl, _mult, _chill, _ms, line;
        while( std::getline(fs, line) ){

            if( !line.empty() ){ // todo: think of a better number

                // split up data
                std::istringstream stream(line);
                std::getline(stream, _root, ',');
                std::getline(stream, _st, ',');
                std::getline(stream, _exch, ',');
                std::getline(stream, _lsn, ',');
                std::getline(stream, _lsf, ',');
                std::getline(stream, _mt, ',');
                std::getline(stream, _cpc, ',');
                std::getline(stream, _ncpl, ',');
                std::getline(stream, _mult, ',');
                std::getline(stream, _chill, ',');
                std::getline(stream, _ms, ',');

                // clean strings
                makeUpperAndTrim(_root);
                makeUpperAndTrim(_st);
                makeUpperAndTrim(_exch);
                makeUpperAndTrim(_lsn);
                makeUpperAndTrim(_lsf);
                makeUpperAndTrim(_mt);
                makeUpperAndTrim(_cpc);
                makeUpperAndTrim(_ncpl);
                makeUpperAndTrim(_mult);
                makeUpperAndTrim(_chill);
                makeUpperAndTrim(_ms);

                // store the info
                root = _root;
                sec_type = _st;
                exch = _exch;
                lsym_near = _lsn;
                lsym_far = _lsf;
                min_tick = std::stof(_mt);
                commiss_per_contract = std::stof(_cpc);
                ncontracts_per_leg = std::stoi(_ncpl);
                multiplier = std::stoi(_mult);
                chillness = std::stoi(_chill);
                mid_spread = std::stod(_ms);

            }
        }

    }else{
        throw std::runtime_error("could not open symbol information file\n");
    }

    // check everything
    checkLSyms();

    if( std::abs(mid_spread/min_tick - std::round(mid_spread/ min_tick)) > 0.0)
        throw std::runtime_error("spread must be a multiple of theminimum tick\n");
}

std::map<char,int> FutCalendarSpreadConfig::create_map()
{
    std::map<char,int> m;
    m['F'] = 0; // january
    m['G'] = 1; // feburary
    m['H'] = 2; // march
    m['J'] = 3; // apri
    m['K'] = 4; // may
    m['M'] = 5; // june
    m['N'] = 6; // july
    m['Q'] = 7; // august
    m['U'] = 8; // sept
    m['V'] = 9; // oct
    m['X'] = 10; // nov
    m['Z'] = 11; // dec
    return m;
}

std::map<char,int> FutCalendarSpreadConfig::contract_months = FutCalendarSpreadConfig::create_map();

void FutCalendarSpreadConfig::checkLSyms() const
{
    char front_month = lsym_near.at(3);
    char far_month   = lsym_far.at(3);
    int front_int = contract_months[front_month];
    int far_int = contract_months[far_month];

    int front_year = lsym_near.at(4) - '0';
    int far_year   = lsym_far.at(4) - '0';

    // front month can be greater, but that must be because it's in an earlier year
    if(front_int > far_int && front_year >= far_year){
        throw std::runtime_error("front month local symbol must come first in ctor\n");
    }
}

void FutCalendarSpreadConfig::makeUpperAndTrim(std::string& s)
{
    transform(s.begin(), s.end(), s.begin(), ::toupper);
    boost::algorithm::trim(s);
}

FutSymsConfig::FutSymsConfig(const std::string& file)
{

    std::ifstream fs(file);
    unsigned int dyn_sym_unique_id (4000);
    if( fs.good() ){

        std::string _root, _st, _exch, _ls, _mt, _cpc, _mult, _chill, _nc, _curr, line;
        while( std::getline(fs, line) ){

            if( !line.empty() ){

                // split up data
                std::istringstream stream(line);
                std::getline(stream, _root, ',');
                std::getline(stream, _st, ',');
                std::getline(stream, _exch, ',');
                std::getline(stream, _ls, ',');
                std::getline(stream, _mt, ',');
                std::getline(stream, _cpc, ',');
                std::getline(stream, _mult, ',');
                std::getline(stream, _chill, ',');
                std::getline(stream, _nc, ',');
                std::getline(stream, _curr, ',');

                // store the info
                m_contracts.push_back(FutTradingContract(_root, _ls, _st, _curr, _exch,
                                                         std::stof(_mt),
                                                         std::stof(_cpc),
                                                         std::stoi(_mult),
                                                         std::stoi(_chill),
                                                         std::stoi(_nc)));

                // set order and trade ids
                m_unique_order_ids.insert(
                        std::pair<std::string,unsigned int>(
                            BasicContract::uppercase(_ls),
                                dyn_sym_unique_id++));
                m_unique_trade_ids.insert(
                        std::pair<std::string,unsigned int>(
                            BasicContract::uppercase(_ls),
                                dyn_sym_unique_id++));
            }
        }

    }else{
        throw std::runtime_error("could not open symbol information file\n");
    }
}

std::map<char,int> FutSymsConfig::create_map()
{
    std::map<char,int> m;
    m['F'] = 0; // january
    m['G'] = 1; // feburary
    m['H'] = 2; // march
    m['J'] = 3; // apri
    m['K'] = 4; // may
    m['M'] = 5; // june
    m['N'] = 6; // july
    m['Q'] = 7; // august
    m['U'] = 8; // sept
    m['V'] = 9; // oct
    m['X'] = 10; // nov
    m['Z'] = 11; // dec
    return m;
}

std::map<char,int> FutSymsConfig::contract_months = FutSymsConfig::create_map();

} //namespace hft


#ifndef INCLUDE_STATE_HPP
#define INCLUDE_STATE_HPP

#include <array>
#include <iostream>
#include <tuple>
#include <vector>

#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

#include <util/action_unpack.hpp>
#include <util/next_actions.hpp>
#include <util/single_next_actions.hpp>
#include <util/union_find.hpp>

#include <agent.hpp>
#include <color.hpp>
#include <field.hpp>
#include <types.hpp>

namespace procon30 {
template <size_t _agent_count, size_t _height, size_t _width>
class normal_state {
private:
  static constexpr size_t _hash_length = _height * _width;
  size_t _turn, _max_turn;
  field _field;
  std::vector<agent> _allies, _enemies;

  field_state_hash<_hash_length> _ally_hash, _enemy_hash;

  bool can_use_state(const normal_state& state) const {
    boost::unordered_set<std::tuple<size_t, size_t>> check_hash;
    const auto& ally_hash = state.ally_hash();
    const auto& enemy_hash = state.enemy_hash();

    for (size_t i = 0; i < state.agent_count(); ++i) {
      const auto& ally = state.allies().at(i);

      // エージェントが場外にいないか?
      if (!(0 <= ally.y() && ally.y() < _height && 0 <= ally.x() && ally.x() < _width)) break;

      // 相手チームマスとかぶってないか
      if (enemy_hash.test(_width * ally.y() + ally.x())) break;

      check_hash.insert(std::make_tuple(ally.y(), ally.x()));
    }

    for (size_t i = 0; i < state.agent_count(); ++i) {
      const auto& enemy = state.enemies().at(i);

      // エージェントが場外にいないか?
      if (!(0 <= enemy.y() && enemy.y() < _height && 0 <= enemy.x() && enemy.x() < _width)) break;

      // 自チームマスとかぶってないか
      if (ally_hash.test(_width * enemy.y() + enemy.x())) break;

      check_hash.insert(std::make_tuple(enemy.y(), enemy.x()));
    }

    return check_hash.size() == state.agent_count() * 2;
  }

public:
  normal_state(size_t turn, size_t max_turn, const field& field, const std::vector<agent>& allies,
               const std::vector<agent>& enemies)
      : _turn(turn),
        _max_turn(max_turn),
        _field(field),
        _allies(allies),
        _enemies(enemies),
        _ally_hash(),
        _enemy_hash() {}

  normal_state(size_t turn, size_t max_turn, const procon30::field& field, const std::vector<agent>& allies,
               const std::vector<agent>& enemies, const field_state_hash<_hash_length>& ally_hash,
               const field_state_hash<_hash_length>& enemy_hash)
      : _turn(turn),
        _max_turn(max_turn),
        _field(field),
        _allies(allies),
        _enemies(enemies),
        _ally_hash(ally_hash),
        _enemy_hash(enemy_hash) {}

  size_t turn() const { return _turn; }
  size_t max_turn() const { return _max_turn; }
  size_t agent_count() const { return _agent_count; }

  const field& field() const { return _field; }

  const std::vector<agent>& allies() const { return _allies; }

  const std::vector<agent>& enemies() const { return _enemies; }

  auto next_states() const {
    static constexpr std::array<int32, 9> dy = {0, 0, 1, 1, 1, 0, -1, -1, -1}, dx = {0, 1, 1, 0, -1, -1, -1, 0, 1};
    std::vector<normal_state> ret;

    if (this->turn() >= this->max_turn()) return ret;

    // パターンの列挙
    constexpr auto pattern = util::next_actions<_agent_count>();
    for (size_t j = 0; j < pattern.size(); ++j) {
      auto&& action = pattern.at(j);
      auto&& unpack = util::action_unpack<_agent_count>(action);
      std::vector<agent> next_allies = this->allies();
      std::vector<agent> next_enemies = this->enemies();
      field_state_hash<_hash_length> next_ally_hash = this->ally_hash();
      field_state_hash<_hash_length> next_enemy_hash = this->enemy_hash();
      const auto& field = this->field();

      for (size_t i = 0; i < _agent_count * 2; ++i) {
        size_t ny = dy.at(unpack.at(i)), nx = dx.at(unpack.at(i));
        if (i < _agent_count) {  // 自チームの列挙
          agent& next_agent = next_allies.at(i);

          // 移動
          next_agent.move(ny, nx);

          // フィールド範囲チェック
          if (!(0 <= next_agent.y() && next_agent.y() < _height && 0 <= next_agent.x() && next_agent.x() < _width))
            continue;

          // 移動先が相手マスであれば戻って相手マスを削除する行動とする
          if (this->enemy_hash().test(_width * next_agent.y() + next_agent.x())) {
            next_enemy_hash.reset(_width * next_agent.y() + next_agent.x());
            next_agent.move(-ny, -nx);
          } else {
            next_ally_hash.set(_width * next_agent.y() + next_agent.x());
          }
        } else {  // 相手チームの列挙
          agent& next_agent = next_enemies.at(i - _agent_count);

          // 移動
          next_agent.move(ny, nx);

          // フィールド範囲チェック
          if (!(0 <= next_agent.y() && next_agent.y() < _height && 0 <= next_agent.x() && next_agent.x() < _width))
            continue;

          // 移動先が相手マスであれば戻って相手マスを削除する行動とする
          if (this->ally_hash().test(_width * next_agent.y() + next_agent.x())) {
            next_ally_hash.reset(_width * next_agent.y() + next_agent.x());
            next_agent.move(-ny, -nx);
          } else {
            next_enemy_hash.set(_width * next_agent.y() + next_agent.x());
          }
        }
      }

      auto&& next_state = normal_state(this->turn() + 1, this->max_turn(), field, next_allies, next_enemies,
                                       next_ally_hash, next_enemy_hash);

      if (can_use_state(next_state)) {
        ret.emplace_back(next_state);
      }
    }

    return ret;
  }

  int32 tile_point(color c) const {
    int32 ret = 0;
    const auto& field = this->field();
    for (size_t i = 0; i < _hash_length; ++i) {
      const size_t y = i / _width, x = i % _width;
      const auto& board = c == color::ally ? this->ally_hash() : this->enemy_hash();
      if (board.test(i)) ret += field.at(y, x);
    }

    return ret;
  }

  int32 area_point(color c) const {
    static constexpr std::array<int32, 4> dy = {0, 1, 0, -1}, dx = {1, 0, -1, 0};

    int32 ret = 0;
    const auto& board = c == color::ally ? this->ally_hash() : this->enemy_hash();

    auto is_between = [&](size_t y, size_t x) {
      bool ret = false;
      // 上方向に自マスがあるか
      for (int32 i = y - 1; i >= 0; --i) {
        if (board.test(i * _width + x)) {
          ret = true;
          break;
        }
      }

      if (!ret) return false;
      ret = false;

      // 下方向に自マスがあるか
      for (int32 i = y + 1; i < _height; ++i) {
        if (board.test(i * _width + x)) {
          ret = true;
          break;
        }
      }

      if (!ret) return false;
      ret = false;

      // 左方向に自マスがあるか
      for (int32 i = x - 1; i >= 0; --i) {
        if (board.test(y * _width + i)) {
          ret = true;
          break;
        }
      }

      if (!ret) return false;
      ret = false;

      // 右方向に自マスがあるか
      for (int32 i = x + 1; i < _width; ++i) {
        if (board.test(y * _width + i)) {
          ret = true;
          break;
        }
      }

      return ret;
    };

    util::union_find uf(_height * _width);
    for (size_t i = 0; i < _height; ++i) {
      for (size_t j = 0; j < _width; ++j) {
        if (board.test(i * _width + j)) continue;
        for (size_t k = 0; k < 4; ++k) {
          int32 ny = dy[k] + i, nx = dx[k] + j;
          if (!(0 <= ny && ny < _height && 0 <= nx && nx < _width)) continue;
          if (board.test(ny * _width + nx)) continue;
          uf.unite(i * _width + j, ny * _width + nx);
        }
      }
    }

    boost::unordered_map<int32, std::vector<int32>> area_cand;
    for (size_t i = 0; i < _height; ++i) {
      for (size_t j = 0; j < _width; ++j) {
        if (board.test(i * _width + j)) continue;
        area_cand[uf.root(i * _width + j)].emplace_back(i * _width + j);
      }
    }

    for (auto&& [root, candidates] : area_cand) {
      bool is_area = true;
      int32 local_point = 0;
      for (auto&& cand : candidates) {
        if (!is_between(cand / _width, cand % _width)) {
          is_area = false;
          break;
        }
        local_point += this->field().at(cand / _width, cand % _width);
      }
      if (is_area) ret += local_point;
    }

    return ret;
  }

  int32 all_point(color c) const {
    return tile_point(c) + area_point(c);
  }

  const field_state_hash<_hash_length>& ally_hash() const { return _ally_hash; }
  const field_state_hash<_hash_length>& enemy_hash() const { return _enemy_hash; }
  field_state_hash<_hash_length>& ally_hash() { return _ally_hash; }
  field_state_hash<_hash_length>& enemy_hash() { return _enemy_hash; }
};

}  // namespace procon30

#endif


#include <boost/uuid/sha1.hpp>
#include <stdio.h>
#include <errno.h>

using namespace boost::uuids::detail;

void sha1sum(const char* fileName)
{
  sha1 sha;

  FILE * file = fopen(fileName, "rb");
  if (NULL == file)
  {
    fprintf(stderr, "Could not open file %s, errno(%d)\n", fileName, errno);
    return;
  }

  unsigned char buf[256];
  while(!feof(file))
  {
    int count = fread(buf, sizeof(unsigned char), 256, file);
    sha.process_bytes(buf, count);
  }

  unsigned int digest[5];
  sha.get_digest(digest);

  for (int i = 0; i < 5 ; i++)
  {
    printf("%08x", digest[i]);
  }

  printf("  %s\n", fileName);

  fclose(file);
}

int main(int argc, char* argv[])
{
  for (int i = 1; i < argc; i++)
  {
    sha1sum(argv[i]);
  }
  return 0;
}


/* ========================================================================= *
 *                                                                           *
 *                               OpenMesh                                    *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openmesh.org                               *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenMesh.                                            *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. Redistributions in binary form must reproduce the above copyright      *
 *    notice, this list of conditions and the following disclaimer in the    *
 *    documentation and/or other materials provided with the distribution.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
 *                                                                           *
 * ========================================================================= */

/** This file contains all code required to use Eigen3 vectors as Mesh
 *  vectors
 */
#pragma once

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

namespace OpenMesh {
  template <typename _Scalar, int _Rows, int _Cols, int _Options>
  struct vector_traits<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options>> {
      static_assert(_Rows != Eigen::Dynamic && _Cols != Eigen::Dynamic,
                    "Should not use dynamic vectors.");
      static_assert(_Rows == 1 || _Cols == 1, "Should not use matrices.");

      using vector_type = Eigen::Matrix<_Scalar, _Rows, _Cols, _Options>;
      using value_type = _Scalar;
      static const size_t size_ = _Rows * _Cols;
      static size_t size() { return size_; }
};

} // namespace OpenMesh

namespace Eigen {

  template <typename Derived>
  typename Derived::Scalar dot(const MatrixBase<Derived> &x,
                               const MatrixBase<Derived> &y) {
      return x.dot(y);
  }

  template <typename Derived>
  typename MatrixBase< Derived >::PlainObject cross(const MatrixBase<Derived> &x, const MatrixBase<Derived> &y) {
      return x.cross(y);
  }

  template <typename Derived>
  typename Derived::Scalar norm(const MatrixBase<Derived> &x) {
      return x.norm();
  }

  template <typename Derived>
  typename Derived::Scalar sqrnorm(const MatrixBase<Derived> &x) {
      return x.dot(x);
  }

  template <typename Derived>
  MatrixBase<Derived> &normalize(MatrixBase<Derived> &x) {
      x /= x.norm();
      return x;
  }

  template <typename Derived>
  MatrixBase<Derived> &vectorize(MatrixBase<Derived> &x,
                                 typename Derived::Scalar const &val) {
      x.fill(val);
      return x;
  }

} // namespace Eigen


#include "label_model.hpp"
#include "main_window.cpp"

#include <point_cloud_object_detection_and_recognition/pcodar_params.hpp>
#include <point_cloud_object_detection_and_recognition/point_cloud_builder.hpp>
#include <point_cloud_object_detection_and_recognition/point_cloud_clusterer.hpp>

#include <eigen_conversions/eigen_msg.h>

#include <cv_bridge/cv_bridge.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <rosbag/bag.h>

#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_ros/transform_listener.h>

#include <boost/progress.hpp>

namespace
{
Eigen::Affine3d odom_to_eigen(const nav_msgs::Odometry::ConstPtr odom)
{
  geometry_msgs::Transform g_transform;
  g_transform.translation.x = odom->pose.pose.position.x;
  g_transform.translation.y = odom->pose.pose.position.y;
  g_transform.translation.z = odom->pose.pose.position.z;
  g_transform.rotation = odom->pose.pose.orientation;
  Eigen::Affine3d e_transform;
  tf::transformMsgToEigen(g_transform, e_transform);
  return e_transform;
}

int get_index_of_object(const geometry_msgs::Point32& point, const std::vector<mil_msgs::PerceptionObject>& objects)
{
  for (size_t i = 0; i != objects.size(); ++i)
  {
    const auto& object = objects[i];
    const double yaw = tf::getYaw(object.pose.orientation);
    const Eigen::Rotation2D<double> rot(yaw);
    const Eigen::Vector2d pos = Eigen::Vector2d(object.pose.position.x, object.pose.position.y);

    const double offset_x = object.scale.x / 2.0;
    const double offset_y = object.scale.y / 2.0;

    Eigen::Vector2d a(-offset_x, -offset_y);
    Eigen::Vector2d b(offset_x, -offset_y);
    Eigen::Vector2d d(-offset_x, offset_y);
    Eigen::Vector2d p(point.x, point.y);

    p = p - pos;
    a = (rot * a);
    b = (rot * b);
    d = (rot * d);

    const Eigen::Vector2d ab = b - a;
    const Eigen::Vector2d ad = d - a;
    const Eigen::Vector2d ap = p - a;

    const double ab_ap = ab.dot(ap);
    const double ab_ab = ab.dot(ab);
    const double ad_ap = ad.dot(ap);
    const double ad_ad = ad.dot(ad);

    if (0 < ab_ap && ab_ap < ab_ab && 0 < ad_ap && ad_ap < ad_ad)
    {
      return (int)(i);
    }
  }
  return -1;
}

}  // anonymous namespace

rosbag::View make_view()
{
}
label_model::label_model(int argc, char* argv[], std::shared_ptr<id_to_labeled_object> object_map_ptr)
  : object_map_ptr_(object_map_ptr)
{
  // Setup ROS
  ros::init(argc, argv, "point_cloud_object_detector");
  ros::Time::init();

  // Set up node handler
  auto nh = ros::NodeHandle(ros::this_node::getName());
  nh_ = std::make_shared<ros::NodeHandle>(nh);

  // Get bag from command-line
  input_bag_name_ = argv[1];
  output_bag_name_ = argv[2];

  bag_.open(input_bag_name_, rosbag::bagmode::Read);

  // This doesn't take into account camera distortion - for this application, it is ok, since we aren't using
  // fisheyes and it's not a performance based application.
  std::vector<std::string> image_topics = { "/odom", "/stereo/right/image_raw" };

  std::vector<std::string> odom_topics = { "/odom" };
  rosbag::View view_odom(bag_, rosbag::TopicQuery(odom_topics));
  last_odom_ = view_odom.begin()->instantiate<nav_msgs::Odometry>();
  v_ = std::make_shared<rosbag::View>(bag_, rosbag::TopicQuery(image_topics));

  bag_iter_ = v_->begin();
}

void label_model::populate_map()
{
  tf2_ros::Buffer tfBuffer(ros::Duration(10.0));
  tf2_ros::TransformListener tf_listener(tfBuffer);
  geometry_msgs::TransformStamped g_velodyne_to_baselink;
  geometry_msgs::TransformStamped g_baselink_to_stereoright;

  ROS_INFO_STREAM("Getting the proper transforms");

  while (true)
  {
    try
    {
      g_velodyne_to_baselink = tfBuffer.lookupTransform("base_link", "velodyne", ros::Time(0));
      g_baselink_to_stereoright = tfBuffer.lookupTransform("stereo_right_cam", "base_link", ros::Time(0));

      break;
    }
    catch (tf2::TransformException& ex)
    {
    }
  }

  // TODO(tbianchi) : Fix this lol
  intrinsics_ << 704, 0, 476, 0, 0, 701, 300, 0, 0, 0, 1, 0;
  tf::transformMsgToEigen(g_velodyne_to_baselink.transform, e_velodyne_to_baselink_);
  tf::transformMsgToEigen(g_baselink_to_stereoright.transform, e_baselink_to_stereoright_);

  ROS_INFO_STREAM("Done getting the proper transforms");

  std::vector<std::string> topics;
  topics.push_back(std::string("/velodyne_points"));
  topics.push_back(std::string("/odom"));

  rosbag::View view(bag_, rosbag::TopicQuery(topics));
  std::vector<std::string> odom_topics = { "/odom" };
  rosbag::View view_odom(bag_, rosbag::TopicQuery(odom_topics));

  nav_msgs::Odometry::ConstPtr last_odom = view_odom.begin()->instantiate<nav_msgs::Odometry>();
  pcodar::point_cloud_builder pc_builder(false);

  ROS_INFO_STREAM("Building mega point cloud");

  const int number_pcloud_messages = view.size() - view_odom.size();

  std::cout << "\nLOADING BAG INTO LABELER" << std::endl;

  boost::progress_display show_progress(number_pcloud_messages);

  for (rosbag::MessageInstance& message : view)
  {
    if (message.getTopic() == "/velodyne_points")
    {
      sensor_msgs::PointCloud2::ConstPtr point_cloud = message.instantiate<sensor_msgs::PointCloud2>();

      const Eigen::Affine3d e_baselink_to_enu = odom_to_eigen(last_odom);

      Eigen::Affine3d e_velodyne_to_enu = e_baselink_to_enu * e_velodyne_to_baselink_;

      pc_builder.add_point_cloud(*point_cloud, e_velodyne_to_enu);
      ++show_progress;
    }
    else
    {
      last_odom = message.instantiate<nav_msgs::Odometry>();
    }
  }
  ROS_INFO_STREAM("Done mega building point cloud");

  ROS_INFO_STREAM("Constructing objects");

  mega_cloud_ = pc_builder.get_point_cloud();
  auto objects = pcodar::get_point_cloud_clusters(mega_cloud_);

  std::ranlux48 gen;
  std::uniform_int_distribution<int> uniform_0_255(0, 255);

  uint16_t id_counter = 1;
  for (const auto& o : objects)
  {
    course_object c_object;

    const int r = uniform_0_255(gen);
    const int g = uniform_0_255(gen);
    const int b = uniform_0_255(gen);

    const Eigen::Vector4d position(o.pose.position.x, o.pose.position.y, o.pose.position.z, 1);
    const Eigen::Vector4d p1 = position + Eigen::Vector4d(o.scale.x / 2.0, o.scale.y / 2.0, o.scale.z / 2.0, 0);
    const Eigen::Vector4d p2 = position + Eigen::Vector4d(o.scale.x / 2.0, o.scale.y / 2.0, -o.scale.z / 2.0, 0);
    const Eigen::Vector4d p3 = position + Eigen::Vector4d(o.scale.x / 2.0, -o.scale.y / 2.0, o.scale.z / 2.0, 0);
    const Eigen::Vector4d p4 = position + Eigen::Vector4d(o.scale.x / 2.0, -o.scale.y / 2.0, -o.scale.z / 2.0, 0);
    const Eigen::Vector4d p5 = position + Eigen::Vector4d(-o.scale.x / 2.0, o.scale.y / 2.0, o.scale.z / 2.0, 0);
    const Eigen::Vector4d p6 = position + Eigen::Vector4d(-o.scale.x / 2.0, o.scale.y / 2.0, -o.scale.z / 2.0, 0);
    const Eigen::Vector4d p7 = position + Eigen::Vector4d(-o.scale.x / 2.0, -o.scale.y / 2.0, o.scale.z / 2.0, 0);
    const Eigen::Vector4d p8 = position + Eigen::Vector4d(-o.scale.x / 2.0, -o.scale.y / 2.0, -o.scale.z / 2.0, 0);
    std::vector<Eigen::Vector4d> vec_of_points = { position, p1, p2, p3, p4, p5, p6, p7, p8 };

    c_object.p_object = o;
    c_object.p_object.id = id_counter++;
    c_object.p_object.classification = "unknown";
    c_object.bounding_box_points = vec_of_points;
    c_object.color = cv::Scalar(b, g, r);
    c_object.visualize = true;

    object_map_ptr_->insert({ c_object.p_object.id, c_object });
  }
  ROS_INFO_STREAM("Done constructing objects");
}

bool label_model::has_next_image()
{
  while (bag_iter_ != v_->end())
  {
    rosbag::MessageInstance message = *bag_iter_;

    if (message.getTopic() == "/stereo/right/image_raw")
    {
      last_image_ = message.instantiate<sensor_msgs::Image>();
      ++bag_iter_;
      return true;
    }
    else
    {
      last_odom_ = message.instantiate<nav_msgs::Odometry>();
    }
    ++bag_iter_;
  }

  return false;
}
cv::Mat label_model::get_next_image()
{
  cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(last_image_, sensor_msgs::image_encodings::BGR8);
  cv::Mat cv_image = cv_ptr->image;
  const Eigen::Affine3d e_enu_to_baselink = odom_to_eigen(last_odom_).inverse();
  const Eigen::Affine3d e_enu_to_stereoright = e_baselink_to_stereoright_ * e_enu_to_baselink;

  for (const auto& id_course_object_pair : *object_map_ptr_)
  {
    const course_object& c_object = id_course_object_pair.second;

    if (!c_object.visualize)
    {
      continue;
    }

    const std::vector<Eigen::Vector4d> b = c_object.bounding_box_points;
    const Eigen::Vector4d position = b[0];
    const auto position_baselink = e_enu_to_baselink * position;

    const Eigen::Vector2d axis(1.0, 0.0);
    Eigen::Vector2d obj_2d_pos(position_baselink[0], position_baselink[1]);
    obj_2d_pos.normalize();

    const double similarity_score = axis.dot(obj_2d_pos);
    // If the points in baselink are in front of the boat, and within an angle threshold
    if (position_baselink[0] > 2.0 && similarity_score > .9)
    {
      // Convert the points to image coordinates, and draw said point on the image.
      Eigen::MatrixXd points(4, 8);
      points.col(0) = b[1];
      points.col(1) = b[2];
      points.col(2) = b[3];
      points.col(3) = b[4];
      points.col(4) = b[5];
      points.col(5) = b[6];
      points.col(6) = b[7];
      points.col(7) = b[8];

      const Eigen::Matrix4d ee = e_enu_to_stereoright.matrix();

      points = intrinsics_ * ee * points;
      points.row(0) = points.row(0).cwiseQuotient(points.row(2));
      points.row(1) = points.row(1).cwiseQuotient(points.row(2));

      const Eigen::MatrixXd min_matrix = points.rowwise().minCoeff();
      const Eigen::MatrixXd max_matrix = points.rowwise().maxCoeff();

      int min_x = min_matrix(0, 0);
      int min_y = min_matrix(1, 0);
      int max_x = max_matrix(0, 0);
      int max_y = max_matrix(1, 0);

      min_x = std::min(std::max(0, min_x), cv_image.cols);
      min_y = std::min(std::max(0, min_y), cv_image.rows);
      max_x = std::min(std::max(0, max_x), cv_image.cols);
      max_y = std::min(std::max(0, max_y), cv_image.rows);

      for (int i = 0; i != points.cols(); ++i)
      {
        const auto& p = points.col(i);
        cv::Mat roi = cv_image(cv::Rect(cv::Point(min_x, min_y), cv::Point(max_x, max_y)));
        cv::Mat color(roi.size(), CV_8UC3, c_object.color);
        double alpha = 0.05;
        cv::addWeighted(color, alpha, roi, 1.0 - alpha, 0.0, roi);
        cv::putText(cv_image, std::to_string(c_object.p_object.id) + ":" + c_object.p_object.classification,
                    cv::Point(min_x, max_y - 10), 0, 0.5, cv::Scalar(255, 255, 255));
      }
    }
  }
  return cv_image;
}

void label_model::restart()
{
  // TODO(tbianchi) put all the topics as consts
  std::vector<std::string> image_topics = { "/odom", "/stereo/right/image_raw" };
  bag_iter_ = v_->begin();
}

void label_model::generate_bag()
{
  rosbag::Bag bag_new;
  bag_new.open(output_bag_name_, rosbag::bagmode::Write);

  std::vector<std::string> topics;
  topics.push_back(std::string("/velodyne_points"));
  topics.push_back(std::string("/odom"));

  rosbag::View view(bag_, rosbag::TopicQuery(topics));
  std::vector<std::string> odom_topics = { "/odom" };
  rosbag::View view_odom(bag_, rosbag::TopicQuery(odom_topics));

  nav_msgs::Odometry::ConstPtr last_odom = view_odom.begin()->instantiate<nav_msgs::Odometry>();

  std::vector<mil_msgs::PerceptionObject> objects;
  for (const auto& id_object_pair : *object_map_ptr_)
  {
    objects.push_back(id_object_pair.second.p_object);
  }

  const int number_pcloud_messages = view.size() - view_odom.size();

  std::cout << "\nSAVING BAG" << std::endl;

  // TODO(tbianchi) Make this a popup thingy
  boost::progress_display show_progress(number_pcloud_messages);

  for (rosbag::MessageInstance& message : view)
  {
    if (message.getTopic() == "/velodyne_points")
    {
      auto object_copy = objects;

      const sensor_msgs::PointCloud2::ConstPtr point_cloud2 = message.instantiate<sensor_msgs::PointCloud2>();
      const Eigen::Affine3d e_baselink_to_enu = odom_to_eigen(last_odom);
      const Eigen::Affine3d e_velodyne_to_enu = e_baselink_to_enu * e_velodyne_to_baselink_;
      const auto point_cloud = pcodar::transform_point_cloud(*point_cloud2, e_velodyne_to_enu);

      mil_msgs::PerceptionObjectArray objects_in_frame;
      mil_msgs::PerceptionObject spurious_points;

      // TODO(tbianchi) fix this so that it uses smart enum
      spurious_points.classification = "spurious";

      std::set<uint32_t> indices;
      for (const auto& p : point_cloud.points)
      {
        geometry_msgs::Point32 g_p;
        g_p.x = p.x;
        g_p.y = p.y;
        g_p.z = p.z;
        int index_of_object = get_index_of_object(g_p, object_copy);
        if (index_of_object == -1)
        {
          spurious_points.points.push_back(g_p);
        }
        else
        {
          object_copy[index_of_object].points.push_back(g_p);
          indices.insert(index_of_object);
        }
      }
      objects_in_frame.objects.push_back(spurious_points);
      for (const uint32_t ind : indices)
      {
        objects_in_frame.objects.push_back(object_copy[ind]);
      }

      bag_new.write("labels", message.getTime(), objects_in_frame);
      ++show_progress;
    }
    else
    {
      last_odom = message.instantiate<nav_msgs::Odometry>();
    }
  }
  bag_new.close();
}