/*******************************************************************************
 *
 * Standard domain of numerical congruences extended with bitwise
 * operations.
 *
 * Author: Alexandre C. D. Wimmers (alexandre.c.wimmers@nasa.gov)
 *
 * Contributors: Jorge A. Navas (jorge.a.navaslaserna@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <crab/common/types.hpp>
#include <crab/common/stats.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>

#include <boost/optional.hpp>

namespace ikos {

template <typename Number>
class congruence {
  typedef interval<Number> interval_t;

public:
  typedef congruence<Number> congruence_t;

private:
  bool _is_bottom;

  /// A congruence is denoted by aZ + b, where b \in Z and a \in N.
  /// The abstract state aZ + b represents all numbers that are
  /// congruent to b modulo a.
  Number _a; // modulo
  Number _b; // remainder

  // Notes about the % operator
  //
  // The semantics of r = n % d is to set r to "n mod d". The sign of
  // the d is ignored and r is always non-negative.
  //
  // We assume that n % d (also n /d) raises a runtime error if d==0.

  void normalize(void) {
    // Set to standard form: 0 <= b < a for a != 0
    if (_a != 0) {
      _b = _b % _a;
    }
  }

  // if true then top (1Z + 0) else bottom
  congruence(bool b): _is_bottom(!b), _a(1), _b(0) {}

  congruence(int n): _is_bottom(false), _a(0), _b(n) {}

  congruence(Number a, Number b)
    : _is_bottom(false),
      _a(a),
      _b(b) {
    normalize();
  }

  Number abs(Number x) const { return x < 0 ? -x : x; }

  Number max(Number x, Number y) const { return x.operator<=(y) ? y : x; }

  Number min(Number x, Number y) const { return x.operator<(y) ? x : y; }

  Number gcd(Number x, Number y, Number z) const  { return gcd(x, gcd(y, z)); }
  // Not to be called explicitly outside of gcd
  Number gcd_helper(Number x, Number y) const {
    return (y == 0) ? x : gcd_helper(y, x % y);
  }
  Number gcd(Number x, Number y) const { return gcd_helper(abs(x), abs(y)); }

  Number lcm(Number x, Number y) const {
    Number tmp = gcd(x, y);
    return abs(x * y) / tmp;
  }

  bool is_zero() const {
    return !is_bottom() && _a == 0 && _b == 0;
  }

  bool all_ones() const {
    return !is_bottom() && _a == 0 && _b == -1;
  }

  interval_t to_interval() const {
    assert(singleton());
    return interval_t(*(singleton()));
  }

public:

  static congruence_t top() { return congruence(true); }

  static congruence_t bottom() { return congruence(false); }

  congruence(): _is_bottom(false), _a(1), _b(0) {}

  congruence(Number n): _is_bottom(false), _a(0), _b(n) {}

  congruence(const congruence_t& o)
      : _is_bottom(o._is_bottom), _a(o._a), _b(o._b) {}

  congruence_t operator=(congruence_t o) {
    _is_bottom = o._is_bottom;
    _a = o._a;
    _b = o._b;
    return *this;
  }

  bool is_bottom() const { return _is_bottom; }

  bool is_top() const { return _a == 1; }

  boost::optional<Number> singleton() const {
    if (!this->is_bottom() && _a == 0) {
      return boost::optional<Number>(_b);
    } else {
      return boost::optional<Number>();
    }
  }

  Number get_modulo() const { return _a;}

  Number get_remainder() const { return _b;}

  bool operator==(congruence_t o) const {
    return (is_bottom() == o.is_bottom() && _a == o._a && _b == o._b) ;
  }

  bool operator!=(congruence_t x) const { return !this->operator==(x); }

  /** Lattice Operations **/

  bool operator<=(congruence_t o) {
    if (is_bottom()) {
      return true;
    } else if (o.is_bottom()) {
      return false;
    } else if (_a == 0 && o._a == 0) {
      return  (_b == o._b);
    } else if (_a == 0) {
      if ((_b % o._a) == (o._b % o._a)) {
        return true;
      }
    } else if (o._a == 0) {
      if (_b % _a == (o._b % _a)) {
        return false;
      }
    }
    return (_a % o._a == 0) && (_b % o._a == o._b % o._a);
  }

  congruence_t operator|(congruence_t o) {
    if (is_bottom()) {
      return o;
    } else if (o.is_bottom()) {
      return *this;
    } else if (is_top() || o.is_top()) {
      return top();
    } else {
      return congruence_t(gcd(_a, o._a, abs(_b - o._b)), min(_b, o._b));
    }
  }

  congruence_t operator&(congruence_t o) {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    }

    // lcm has meaning only if both a and o.a are not 0
    if (_a == 0 && o._a == 0) {
      if (_b == o._b) {
        return *this;
      } else {
        return bottom();
      }
    } else if (_a == 0) {
      // b & a'Z + b' iff \exists k such that a'*k + b' = b iff ((b - b') %a' == 0)
      if ((_b - o._b) % o._a == 0) {
        return *this;
      } else {
        return bottom();
      }
    } else if (o._a == 0) {
      // aZ+b & b' iff \exists k such that a*k+b  = b' iff ((b'-b %a) == 0)
      if ((o._b - _b) % _a == 0) {
        return o;
      } else {
        return bottom();
      }
    } else {
      // pre: a and o.a != 0
      Number x = gcd(_a, o._a);
      if (_b % x == (o._b % x)) {
        // the part max(b,o.b) needs to be verified. What we really
        // want is to find b'' such that
        // 1) b'' % lcm(a,a') == b  % lcm(a,a'), and
        // 2) b'' % lcm(a,a') == b' % lcm(a,a').
        // An algorithm for that is provided in Granger'89.
        return congruence_t(lcm(_a, o._a), max(_b, o._b));
      } else {
        return congruence_t::bottom();
      }
    }

  }

  congruence_t operator||(congruence_t o) {
    // Equivalent to join, domain is flat
    return *this | o;
  }

  congruence_t operator&&(congruence_t o) {
    // Simply refines top element
    return (is_top()) ? o : *this;
  }

  /** Arithmetic Operators **/

  congruence_t operator+(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b + o._b);
  }

  congruence_t operator-(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b - o._b);
  }

  congruence_t operator-() {
    if (this->is_bottom() || this->is_top())
      return *this;
    else
      return congruence_t(_a, -_b + _a);
  }

  congruence_t operator*(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if ((this->is_top() || o.is_top()) && _a != 0 && o._a != 0)
      return congruence_t::top();
    else
      return congruence_t(gcd(_a * o._a, _a * o._b, o._a * _b), _b * o._b);
  }

  // signed division
  congruence_t operator/(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      /*
         aZ+b / 0Z+b':
            if b'|a then  (a/b')Z + b/b'
            else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0)
          return congruence_t(_a / o._b, _b / o._b);
        else
          return congruence_t::top();
      }

      /*
         0Z+b / a'Z+b':
            if N>0   (b div N)Z + 0
            else     0Z + 0

           where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n > 0) {
          return congruence_t(_b / n, 0);
        } else {
          return congruence_t(0, 0);
        }
      }

      /*
        General case: no singleton
      */
      return congruence_t::top();
    }
  }

  // signed remainder operator
  congruence_t operator%(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      /*
         aZ+b mod 0Z+b':
             if b'|a then  (a/b')Z + b/b'
             else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0) {
          return congruence_t(0, _b % o._b);
        } else {
          return congruence_t(gcd(_a, o._b), _b);
        }
      }

      /*
          0Z+b mod a'Z+b':
           if N<=0           then 0Z+b
           if (b div N) == 1 then gcd(b',a')Z + b
           if (b div N) >= 2 then N(b div N)Z  + b

         where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n <= 0) {
          return congruence_t(_a, _b);
        } else if (_b == n) {
          return congruence_t(gcd(o._b, o._a), _b);
        } else if ((_b / n) >= 2) {
          return congruence_t(_b, _b);
        } else {
          CRAB_ERROR("unreachable");
        }
      }

      /*
          general case: no singleton
      */
      return congruence_t(gcd(_a, o._a, o._b), _b);
    }
  }

  /**
      Bitwise operators.
      They are very imprecise because we ignore bitwidth.

      Bitwise operation can be implemented more precisely based on
      Stefan Bygde's paper: Static WCET analysis based on abstract
      interpretation and counting of elements, Vasteras : School of
      Innovation, Design and Engineering, Malardalen University (2010).
   **/

  congruence_t And(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero() || o.is_zero()) {
	return congruence_t(0);
      } else if (all_ones()) {
	return o;
      } else if (o.all_ones()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b & o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Or(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (all_ones() || o.all_ones()) {
	return congruence_t(-1);
      } else if (is_zero()) {
	return o;
      } else if (o.is_zero()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b | o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Xor(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero()) {
	return o;
      } else if (o.is_zero()) {
	return *this;
      } else if (_a == 0 && o._a == 0) {
	return congruence_t(_b ^ o._b);
      } else {
	return top();
      }
    }
  }

  congruence_t Shl(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton

	if (o._b < 0) {
	  return bottom();
	}

        // aZ + b << 0Z + b'  = (a*2^b')Z + b*2^b'
        Number x = Number(1) << o._b;
        return congruence_t(_a * x, _b * x);
      } else {

        Number x = Number(1) << o._b;
        Number y = Number(1) << o._a;
        // aZ + b << a'Z + b' = (gcd(a, b * (2^a' - 1)))*(2^b')Z + b*(2^b')
        return congruence_t(gcd(_a, _b * (y - 1)) * x, _b * x);
      }
    }
  }

  congruence_t AShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton
	// aZ + b >> 0Z + b'
	if (o._b < 0) {
	  return congruence_t::bottom();
	}
      }

      if (singleton() && o.singleton()) {
	interval_t res = to_interval().AShr(o.to_interval());
	if (boost::optional<Number> n = res.singleton()) {
	  return congruence(*n);
	}
      }

      return congruence_t::top();
    }
  }

  congruence_t LShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) {
	// aZ + b >> 0Z + b'
	if (o._b < 0) {
	  return congruence_t::bottom();
	}
      }

      if (singleton() && o.singleton()) {
	interval_t res = to_interval().LShr(o.to_interval());
	if (boost::optional<Number> n = res.singleton()) {
	  return congruence(*n);
	}
      }

      return congruence_t::top();
    }
  }

  // division and remainder operations

  congruence_t SDiv(congruence_t x) { return this->operator/(x); }

  congruence_t UDiv(congruence_t x) { return congruence_t::top(); }

  congruence_t SRem(congruence_t x) { return this->operator%(x); }

  congruence_t URem(congruence_t x) { return congruence_t::top(); }

  void write(crab::crab_os& o) const {
    if (is_bottom()) {
      o << "_|_";
      return;
    }

    if (_a == 0) {
      o << _b;
      return;
    }

    o << _a << "Z+" << _b;

  }
}; // end class congruence

template<typename Number>
inline crab::crab_os& operator<<(crab::crab_os& o, const congruence<Number>& c) {
  c.write(o);
  return o;
}

template <typename Number>
inline congruence<Number> operator+(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) + x;
}

template <typename Number>
inline congruence<Number> operator+(
    congruence<Number> x, Number c) {
  return x + congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator*(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) * x;
}

template <typename Number>
inline congruence<Number> operator*(
    congruence<Number> x, Number c) {
  return x * congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator/(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) / x;
}

template <typename Number>
inline congruence<Number> operator/(
    congruence<Number> x, Number c) {
  return x / congruence<Number>(c);
}

template <typename Number>
inline congruence<Number> operator-(
    Number c, congruence<Number> x) {
  return congruence<Number>(c) - x;
}

template <typename Number>
inline congruence<Number> operator-(
    congruence<Number> x, Number c) {
  return x - congruence<Number>(c);
}

template <typename Number,
	  typename VariableName,
	  typename CongruenceCollection>
class equality_congruence_solver {
  // TODO: check correctness of the solver. Granger provides a sound
  // and more precise solver for equality linear congruences (see
  // Theorem 4.4).
private:
  typedef congruence<Number> congruence_t;
  typedef variable<Number, VariableName> variable_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef linear_constraint_system<Number, VariableName>
      linear_constraint_system_t;

  typedef std::vector<linear_constraint_t> cst_table_t;
  typedef typename linear_constraint_t::variable_set_t variable_set_t;

  std::size_t m_max_cycles;
  bool m_is_contradiction;
  cst_table_t m_cst_table;
  variable_set_t m_refined_variables;
  std::size_t m_op_count;

private:
  bool refine(variable_t v, congruence_t i,
              CongruenceCollection& env) {
    congruence_t old_i = env[v];
    congruence_t new_i = old_i & i;
    if (new_i.is_bottom()) {
      return true;
    }
    if (old_i != new_i) {
      env.set(v, new_i);
      m_refined_variables += v;
      ++(m_op_count);
    }
    return false;
  }

  congruence_t compute_residual(linear_constraint_t cst,
                                variable_t pivot,
                                CongruenceCollection& env) {
    congruence_t residual(cst.constant());
    for (typename linear_constraint_t::iterator it = cst.begin();
         it != cst.end();
         ++it) {
      variable_t v = it->second;
      if (!(v == pivot)) {
        residual = residual - (it->first * env[v]);
        ++(m_op_count);
      }
    }
    return residual;
  }

  bool propagate(linear_constraint_t cst, CongruenceCollection& env) {
    for (typename linear_constraint_t::iterator it = cst.begin();
         it != cst.end();
         ++it) {
      Number c = it->first;
      variable_t pivot = it->second;
      congruence_t rhs =
          compute_residual(cst, pivot, env) / congruence_t(c);

      if (cst.is_equality()) {
        if (refine(pivot, rhs, env)) {
	  return true;
	}
      } else if (cst.is_inequality() || cst.is_strict_inequality()) {
        // Inequations (>=, <=, >, and <) do not work well with
        // congruences because for any number n there is always x and y
        // \in gamma(aZ+b) such that n < x and n > y.
        //
        // The only cases we can catch is when all the expressions
        // are constants. We do not bother because any product
        // with intervals or constants should get those cases.
        continue;
      } else {
        // TODO: cst is a disequation
      }
    }
    return false;
  }

  bool solve_system(CongruenceCollection& env) {
    std::size_t cycle = 0;
    do {
      ++cycle;
      m_refined_variables.clear();
      for (typename cst_table_t::iterator it = m_cst_table.begin();
           it != m_cst_table.end();
           ++it) {
        if (propagate(*it, env)) {
	  return true;
	}
      }
    } while (m_refined_variables.size() > 0 &&
             cycle <= m_max_cycles);
    return false;
  }

public:
  equality_congruence_solver(linear_constraint_system_t csts,
                             std::size_t max_cycles)
      : m_max_cycles(max_cycles), m_is_contradiction(false) {
    for (typename linear_constraint_system_t::iterator it = csts.begin();
         it != csts.end();
         ++it) {
      linear_constraint_t cst = *it;
      if (cst.is_contradiction()) {
        m_is_contradiction = true;
        return;
      } else if (cst.is_tautology()) {
        continue;
      } else {
        m_cst_table.push_back(cst);
      }
    }
  }

  void run(CongruenceCollection& env) {
    if (m_is_contradiction) {
      env.set_to_bottom();
    } else {
      if (solve_system(env)) {
	env.set_to_bottom();
      }
    }
  }

}; // class equality_congruence_solver

template <typename Number, typename VariableName>
class congruence_domain final:
  public crab::domains::abstract_domain<congruence_domain<Number,VariableName>> {
public:
  typedef congruence<Number> congruence_t;

private:
  // note that this is assuming that all variables have the same bit
  // width which is unrealistic.
  typedef congruence_domain<Number, VariableName>
  congruence_domain_t;
  typedef crab::domains::abstract_domain<congruence_domain_t>
  abstract_domain_t;

public:
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  typedef Number number_t;
  typedef VariableName varname_t;
  using typename abstract_domain_t::pointer_constraint_t;

private:
  typedef separate_domain<variable_t, congruence_t> separate_domain_t;
  typedef equality_congruence_solver<number_t, varname_t,
				     separate_domain_t> solver_t;

public:
  typedef typename separate_domain_t::iterator iterator;

private:
  separate_domain_t _env;

private:
  congruence_domain(separate_domain_t env) : _env(env) {}

public:
  void set_to_top() {
    congruence_domain abs(separate_domain_t::top());
    std::swap(*this, abs);
  }

  void set_to_bottom() {
    congruence_domain abs(separate_domain_t::bottom());
    std::swap(*this, abs);
  }

  congruence_domain() : _env(separate_domain_t::top()) {}

  congruence_domain(const congruence_domain_t& e)
      : _env(e._env) {
    crab::CrabStats::count(getDomainName() + ".count.copy");
    crab::ScopedCrabStats __st__(getDomainName() + ".copy");
  }

  congruence_domain_t& operator=(const congruence_domain_t& o) {
    crab::CrabStats::count(getDomainName() + ".count.copy");
    crab::ScopedCrabStats __st__(getDomainName() + ".copy");
    if (this != &o)
      this->_env = o._env;
    return *this;
  }

  iterator begin() { return this->_env.begin(); }

  iterator end() { return this->_env.end(); }

  bool is_bottom() { return this->_env.is_bottom(); }

  bool is_top() { return this->_env.is_top(); }

  bool operator<=(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.leq");
    crab::ScopedCrabStats __st__(getDomainName() + ".leq");
    return this->_env <= e._env;
  }

  void operator|=(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.join");
    crab::ScopedCrabStats __st__(getDomainName() + ".join");
    this->_env = this->_env | e._env;
  }

  congruence_domain_t operator|(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.join");
    crab::ScopedCrabStats __st__(getDomainName() + ".join");
    return this->_env | e._env;
  }

  congruence_domain_t operator&(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.meet");
    crab::ScopedCrabStats __st__(getDomainName() + ".meet");
    return this->_env & e._env;
  }

  congruence_domain_t operator||(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.widening");
    crab::ScopedCrabStats __st__(getDomainName() + ".widening");
    return this->_env || e._env;
  }

  congruence_domain_t widening_thresholds(congruence_domain_t other,
					  const crab::iterators::thresholds<number_t>&) {
    return (*this || other);
  }

  congruence_domain_t operator&&(congruence_domain_t e) {
    crab::CrabStats::count(getDomainName() + ".count.narrowing");
    crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");
    return this->_env && e._env;
  }

  void set(variable_t v, congruence_t i) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");
    this->_env.set(v, i);
  }

  void set(variable_t v, number_t n) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");
    this->_env.set(v, congruence_t(n));
  }

  void operator-=(variable_t v) {
    crab::CrabStats::count(getDomainName() + ".count.forget");
    crab::ScopedCrabStats __st__(getDomainName() + ".forget");
    this->_env -= v;
  }

  void operator-=(std::vector<variable_t> vs) {
    for (typename std::vector<variable_t>::iterator it = vs.begin(),
	   end = vs.end(); it != end; ++it) {
      this->operator-=* it;
    }
  }

  congruence_t operator[](variable_t v) { return this->_env[v]; }

  congruence_t operator[](linear_expression_t expr) {
    congruence_t r(expr.constant());
    for (typename linear_expression_t::iterator it = expr.begin();
         it != expr.end();
         ++it) {
      congruence_t c(it->first);
      r = r + (c * this->_env[it->second]);
    }
    return r;
  }

  void add(linear_constraint_system_t csts) {
    crab::CrabStats::count(getDomainName() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");
    const std::size_t threshold = 10;
    if (!this->is_bottom()) {
      solver_t solver(csts, threshold);
      solver.run(this->_env);
    }
  }

  void operator+=(linear_constraint_system_t csts) {
    this->add(csts);
  }

  congruence_domain_t operator+(linear_constraint_system_t csts) {
    congruence_domain_t e(this->_env);
    e += csts;
    return e;
  }

  void assign(variable_t x, linear_expression_t e) {
    crab::CrabStats::count(getDomainName() + ".count.assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".assign");

    congruence_t r = e.constant();
    for (typename linear_expression_t::iterator it = e.begin(); it != e.end();
         ++it) {
      r = r + (it->first * this->_env[it->second]);
    }
    this->_env.set(x, r);
  }

  void apply(operation_t op, variable_t x, variable_t y, variable_t z) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_ADDITION:
        xi = yi + zi;
        break;
      case OP_SUBTRACTION:
        xi = yi - zi;
        break;
      case OP_MULTIPLICATION:
        xi = yi * zi;
        break;
      case OP_SDIV:
        xi = yi / zi;
        break;
      case OP_UDIV:
        xi = yi.UDiv(zi);
        break;
      case OP_SREM:
        xi = yi.SRem(zi);
        break;
      case OP_UREM:
        xi = yi.URem(zi);
        break;
    default:
      CRAB_ERROR("Operation ", op, " not supported");
    }
    this->_env.set(x, xi);
  }

  void apply(operation_t op, variable_t x, variable_t y, number_t k) {
    crab::CrabStats::count(getDomainName() + ".count.apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".apply");

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_ADDITION:
        xi = yi + zi;
        break;
      case OP_SUBTRACTION:
        xi = yi - zi;
        break;
      case OP_MULTIPLICATION:
        xi = yi * zi;
        break;
      case OP_SDIV:
        xi = yi / zi;
        break;
      case OP_UDIV:
        xi = yi.UDiv(zi);
        break;
      case OP_SREM:
        xi = yi.SRem(zi);
        break;
      case OP_UREM:
        xi = yi.URem(zi);
        break;
      default:
	CRAB_ERROR("Operation ", op, " not supported");
    }
    this->_env.set(x, xi);
  }

  // backward operations
  void backward_assign(variable_t x, linear_expression_t e,
		       congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_assign");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      assign(*this, x, e, inv);
  }

  void backward_apply(operation_t op,
		      variable_t x, variable_t y, number_t z,
		      congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_apply");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      apply(*this, op, x, y, z, inv);
  }

  void backward_apply(operation_t op,
		      variable_t x, variable_t y, variable_t z,
		      congruence_domain_t inv) {
    crab::CrabStats::count(getDomainName() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(getDomainName() + ".backward_apply");

    crab::domains::BackwardAssignOps<congruence_domain_t>::
      apply(*this, op, x, y, z, inv);
  }

  // cast operations

  void apply(crab::domains::int_conv_operation_t /*op*/,
	     variable_t dst, variable_t src) {
    // ignore widths
    assign(dst, src);
  }

  // bitwise operations

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

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_AND: {
        xi = yi.And(zi);
        break;
      }
      case OP_OR: {
        xi = yi.Or(zi);
        break;
      }
      case OP_XOR: {
        xi = yi.Xor(zi);
        break;
      }
      case OP_SHL: {
        xi = yi.Shl(zi);
        break;
      }
      case OP_LSHR: {
        xi = yi.LShr(zi);
        break;
      }
      case OP_ASHR: {
        xi = yi.AShr(zi);
        break;
      }
      default: { CRAB_ERROR("unreachable"); }
    }
    this->_env.set(x, xi);
  }

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

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

    switch (op) {
      case OP_AND: {
        xi = yi.And(zi);
        break;
      }
      case OP_OR: {
        xi = yi.Or(zi);
        break;
      }
      case OP_XOR: {
        xi = yi.Xor(zi);
        break;
      }
      case OP_SHL: {
        xi = yi.Shl(zi);
        break;
      }
      case OP_LSHR: {
        xi = yi.LShr(zi);
        break;
      }
      case OP_ASHR: {
        xi = yi.AShr(zi);
        break;
      }
      default: { CRAB_ERROR("unreachable"); }
    }
    this->_env.set(x, xi);
  }

  /*
     Begin unimplemented operations

     congruence_domain implements only standard abstract operations of
     a numerical domain.  The implementation of boolean, array, or
     pointer operations is empty because they should never be called.
  */

  // boolean operations
  void assign_bool_cst(variable_t lhs, linear_constraint_t rhs) {}
  void assign_bool_var(variable_t lhs, variable_t rhs, bool is_not_rhs) {}
  void apply_binary_bool(crab::domains::bool_operation_t op,
			 variable_t x,variable_t y,variable_t z) {}
  void assume_bool(variable_t v, bool is_negated) {}
  // backward boolean operations
  void backward_assign_bool_cst(variable_t lhs, linear_constraint_t rhs,
				congruence_domain_t invariant){}
  void backward_assign_bool_var(variable_t lhs, variable_t rhs, bool is_not_rhs,
				congruence_domain_t invariant) {}
  void backward_apply_binary_bool(crab::domains::bool_operation_t op,
				  variable_t x,variable_t y,variable_t z,
				  congruence_domain_t invariant) {}
  // array operations
  void array_init(variable_t a, linear_expression_t elem_size,
		  linear_expression_t lb_idx, linear_expression_t ub_idx,
		  linear_expression_t val) {}
  void array_load(variable_t lhs,
		  variable_t a, linear_expression_t elem_size,
		  linear_expression_t i) {}
  void array_store(variable_t a, linear_expression_t elem_size,
		   linear_expression_t i, linear_expression_t v,
		   bool is_strong_update) {}
  void array_store(variable_t a_new, variable_t a_old,
		   linear_expression_t elem_size,
		   linear_expression_t i, linear_expression_t v,
		   bool is_strong_update) {}
  void array_store_range(variable_t a, linear_expression_t elem_size,
			 linear_expression_t i, linear_expression_t j,
			 linear_expression_t v) {}
  void array_store_range(variable_t a_new, variable_t a_old,
			 linear_expression_t elem_size,
			 linear_expression_t i, linear_expression_t j,
			 linear_expression_t v) {}
  void array_assign(variable_t lhs, variable_t rhs) {}
  // backward array operations
  void backward_array_init(variable_t a, linear_expression_t elem_size,
			   linear_expression_t lb_idx, linear_expression_t ub_idx,
			   linear_expression_t val, congruence_domain_t invariant) {}
  void backward_array_load(variable_t lhs,
			   variable_t a, linear_expression_t elem_size,
			   linear_expression_t i, congruence_domain_t invariant) {}
  void backward_array_store(variable_t a, linear_expression_t elem_size,
			    linear_expression_t i, linear_expression_t v,
			    bool is_strong_update, congruence_domain_t invariant) {}
  void backward_array_store(variable_t a_new, variable_t a_old,
			    linear_expression_t elem_size,
			    linear_expression_t i, linear_expression_t v,
			    bool is_strong_update, congruence_domain_t invariant) {}
  void backward_array_store_range(variable_t a, linear_expression_t elem_size,
				  linear_expression_t i, linear_expression_t j,
				  linear_expression_t v, congruence_domain_t invariant) {}
  void backward_array_store_range(variable_t a_new, variable_t a_old,
				  linear_expression_t elem_size,
				  linear_expression_t i, linear_expression_t j,
				  linear_expression_t v, congruence_domain_t invariant) {}
  void backward_array_assign(variable_t lhs, variable_t rhs, congruence_domain_t invariant) {}
  // pointer operations
  void pointer_load(variable_t lhs, variable_t rhs)  {}
  void pointer_store(variable_t lhs, variable_t rhs) {}
  void pointer_assign(variable_t lhs, variable_t rhs, linear_expression_t offset) {}
  void pointer_mk_obj(variable_t lhs, ikos::index_t address) {}
  void pointer_function(variable_t lhs, varname_t func) {}
  void pointer_mk_null(variable_t lhs) {}
  void pointer_assume(pointer_constraint_t cst) {}
  void pointer_assert(pointer_constraint_t cst) {}
  /* End unimplemented operations */

  void forget(const variable_vector_t& variables) {
    if (is_bottom() || is_top()) {
      return;
    }
    for (variable_t var: variables){
      this->operator-=(var);
    }
  }

  void project(const variable_vector_t& variables){
    crab::CrabStats::count(getDomainName() + ".count.project");
    crab::ScopedCrabStats __st__(getDomainName() + ".project");

    if (is_bottom() || is_top()) {
      return;
    }

    separate_domain_t env;
    for (variable_t var : variables){
      env.set(var, this->_env[var]);
    }
    std::swap(_env, env);
  }

  void expand(variable_t x, variable_t new_x) {
    crab::CrabStats::count(getDomainName() + ".count.expand");
    crab::ScopedCrabStats __st__(getDomainName() + ".expand");

    if (is_bottom() || is_top()) {
      return;
    }

    set(new_x , this->_env[x]);
  }

  void normalize() {}

  void minimize() {}

  void write(crab::crab_os& o) {
    crab::CrabStats::count(getDomainName() + ".count.write");
    crab::ScopedCrabStats __st__(getDomainName() + ".write");

    this->_env.write(o);
  }

  linear_constraint_system_t to_linear_constraint_system() {
    crab::CrabStats::count(getDomainName() + ".count.to_linear_constraint_system");
    crab::ScopedCrabStats __st__(getDomainName() + ".to_linear_constraint_system");

    linear_constraint_system_t csts;
    if (is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (iterator it = this->_env.begin(); it != this->_env.end(); ++it) {
      variable_t v = it->first;
      congruence_t c = it->second;
      boost::optional<number_t> n = c.singleton();
      if (n) {
        csts += (v == *n);
      }
    }
    return csts;
  }

  disjunctive_linear_constraint_system_t to_disjunctive_linear_constraint_system() {
    auto lin_csts = to_linear_constraint_system();
    if (lin_csts.is_false()) {
      return disjunctive_linear_constraint_system_t(true /*is_false*/);
    } else if (lin_csts.is_true()) {
      return disjunctive_linear_constraint_system_t(false /*is_false*/);
    } else {
      return disjunctive_linear_constraint_system_t(lin_csts);
    }
  }

  static std::string getDomainName() {
    return "Congruences";
  }

}; // class congruence_domain

} // namespace ikos

namespace crab {
namespace domains {

  template <typename Number, typename VariableName>
  struct abstract_domain_traits<ikos::congruence_domain<Number, VariableName>> {
    typedef Number number_t;
    typedef VariableName varname_t;
  };

}
}


#ifndef MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP
#define MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP

#include <Eigen/Core>
#include <functional>
#include <mathtoolbox/bfgs.hpp>
#include <mathtoolbox/l-bfgs.hpp>
#include <stdexcept>

namespace mathtoolbox
{
    namespace optimization
    {
        enum class Algorithm
        {
            Bfgs,
            LBfgs
        };

        enum class Type
        {
            Min,
            Max
        };

        struct Setting
        {
            Algorithm                                              algorithm          = Algorithm::Bfgs;
            Eigen::VectorXd                                        x_init             = Eigen::VectorXd(0);
            std::function<double(const Eigen::VectorXd&)>          f                  = nullptr;
            std::function<Eigen::VectorXd(const Eigen::VectorXd&)> g                  = nullptr;
            double                                                 epsilon            = 1e-05;
            unsigned int                                           max_num_iterations = 1000;
            Type                                                   type               = Type::Min;
        };

        struct Result
        {
            Eigen::VectorXd x_star;
            unsigned        num_iterations;
        };

        /// \brief Run optimization with the specified setting
        ///
        /// \details This is a utility function to run various optimization types in a consitent interface.
        inline Result RunOptimization(const Setting& input)
        {
            const auto f = (input.type == Type::Min) ? input.f : [&input](const Eigen::VectorXd& x) -> double {
                return -input.f(x);
            };
            const auto g = (input.type == Type::Min) ? input.g : [&input](const Eigen::VectorXd& x) -> Eigen::VectorXd {
                return -input.g(x);
            };

            switch (input.algorithm)
            {
                case Algorithm::Bfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunBfgs(input.x_init,
                            f,
                            g,
                            input.epsilon,
                            input.max_num_iterations,
                            result.x_star,
                            result.num_iterations);
                    return result;
                }
                case Algorithm::LBfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunLBfgs(input.x_init,
                             f,
                             g,
                             input.epsilon,
                             input.max_num_iterations,
                             result.x_star,
                             result.num_iterations);
                    return result;
                }
            }
        }
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_NUMERICAL_OPTIMIZATION_HPP


#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <vector>
#include <cmath>
#include <stack>
#include <AtlBase.h>
#include <iomanip>
#include "OVUtil.h"
#include "OVCommon.h"

namespace ov
{

const double PI = 3.1415927;
const double ROT_EPS = 1e-15;

std::string
GetFileName(const std::string& s)
{
    char sep1 = '/', sep2 = '\\';

    size_t i = s.rfind(sep1, s.length());
    size_t j = s.rfind(sep2, s.length());

    if (i != std::string::npos)
        return(s.substr(i + 1, s.length() - i));
    else if (j != std::string::npos)
        return(s.substr(j + 1, s.length() - j));
    else
        return("");
}

std::string
GetBaseName(const std::string& s)
{
    char sep = '.';
    size_t i = s.rfind(sep, s.length());

    if (i != std::string::npos)
        return(s.substr(0, i));
    else
        return("");
}

std::string
GetExt(const std::string& s)
{
    char sep = '.';

    size_t i = s.rfind(sep, s.length());

    if (i != std::string::npos)
        return(s.substr(i + 1, s.length() - i));
    else
        return("");
}

std::string
GetDir(const std::string& s)
{
    char sep1 = '/', sep2 = '\\';

    size_t i = s.rfind(sep1, s.length());
    size_t j = s.rfind(sep2, s.length());

    if (i != std::string::npos)
        return(s.substr(0, i + 1));
    else if (j != std::string::npos)
        return(s.substr(0, j + 1));
    else
        return("");
}

// Simulate a trackball by projecting the previous and current points onto
// a virtual sphere, and then compute the related rotation matrix.
// August '88 issue of Siggraph's "Computer Graphics," pp. 121-129
Mat3
Trackball(const Vec2& prePos, const Vec2& curPos)
{
    if (prePos == curPos)
        return Mat3::Identity();

    Vec3 p1(prePos(0), prePos(1), GetZValueFrom2DPoint(TRACKBALLSIZE, prePos));
    Vec3 p2(curPos(0), curPos(1), GetZValueFrom2DPoint(TRACKBALLSIZE, curPos));
    Vec3 axis = (p1.cross(p2)).normalized();

    // Rotation amout
    double t = (p1 - p2).norm() / 2.0 / TRACKBALLSIZE;
    t = (t > 1) ? 1 : (t < -1) ? -1 : t;
    double phi = 2.0 * asin(t);
    return FromAxisAngleToRotationMatrix(axis * phi);
}

// Project a 2D point onto a sphere of radius r or a hyperboic sheet
// to get the z value
double
GetZValueFrom2DPoint(double r, const Vec2& pos)
{
    double d = pos.norm();
    if (d < r / sqrt(2))
        return sqrt(r * r - d * d);
    else
        return r * r / 2.0 / d;
}

Vec3
FromRotationMatirxToAxisAngle(const Mat3& R)
{
    double a = acos((R.trace() - 1) / 2);
    Vec3 r;
    if (a < ROT_EPS)
    {
        r << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
        r *= 0.5;
    }
    else if (a >(PI - ROT_EPS))
    {
        Mat3 S = 0.5 * (R - Mat3::Identity());
        double b = sqrt(S(0, 0) + 1);
        double c = sqrt(S(1, 1) + 1);
        double d = sqrt(S(2, 2) + 1);
        if (b > ROT_EPS)
        {
            c = S(1, 0) / b;
            d = S(2, 0) / b;
        }
        else if (c > ROT_EPS)
        {
            b = S(0, 1) / c;
            d = S(2, 1) / c;
        }
        else
        {
            b = S(0, 2) / d;
            c = S(1, 2) / d;
        }
        r << b, c, d;
    }
    else
    {
        r << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
        r *= (a / 2 / sin(a));
    }
    return r;
}

Mat3
FromAxisAngleToRotationMatrix(const Vec3& r)
{
    double rx, ry, rz;
    rx = r(0);
    ry = r(1);
    rz = r(2);
    Mat3 I = Mat::Identity(3, 3);
    Mat3 W = (Mat3() << 0, -rz, ry, rz, 0, -rx, -ry, rx, 0).finished();
    Mat3 W2 = W * W;
    const double a = r.norm();
    if (a < ROT_EPS)
        return I + W + 0.5 * W2;
    else
        return I + W * sin(a) / a + W2 * (1 - cos(a)) / (a * a);
}

wxCheckBox*
CreateCheckBoxAndAddToSizer(wxWindow* parent,
    wxSizer *sizer,
    wxString labelStr,
    wxWindowID id)
{
    wxCheckBox *checkbox = new wxCheckBox(parent, id, labelStr);
    sizer->Add(checkbox, 0, wxEXPAND | wxALL, 3);

    return checkbox;
}

Mat
LoadMatrix(std::string fileName)
{
    std::ifstream file;
    file.open(fileName, std::ios::in);

    if (!file.is_open())
        return Mat();

    std::string line;
    bool finishdOneLine = false;
    int rows = 0, cols = 0;

    // First round: find the row and column numbers
    while (!std::getline(file, line, '\n').eof())
    {
        std::istringstream reader(line);
        while (!reader.eof() && !finishdOneLine)
        {
            double val;
            reader >> val;
            if (reader.fail())
                break;
            ++cols;
        }
        ++rows;
        finishdOneLine = true;
    }

    file.clear();
    file.seekg(0);
    Mat mat(rows, cols);
    int i = 0, j = 0;
    // Second round: fill in the matrix
    while (!std::getline(file, line, '\n').eof())
    {
        std::istringstream reader(line);
        while (!reader.eof())
        {
            double val;
            reader >> val;
            if (reader.fail())
                break;
            mat(i, j++) = val;
        }
        ++i;
        j = 0;
    }

    return mat;
}

std::string
ZeroPadNumber(int num, int width)
{
    std::ostringstream ss;
    ss << std::setw(width) << std::setfill('0') << num;
    return ss.str();
}

bool
IsDirectoryExists(std::string dirName)
{
    DWORD attribs = ::GetFileAttributes(std::wstring(dirName.begin(), dirName.end()).c_str());
    if (attribs == INVALID_FILE_ATTRIBUTES)
        return false;
    return (attribs & FILE_ATTRIBUTE_DIRECTORY) != 0;
}

void
CreateDirectorys(std::string path)
{
    std::string dir = GetDir(path);
    std::stack<std::string> dirQueue;
    while (!IsDirectoryExists(dir)&& !dir.empty())
    {
        dirQueue.push(dir);
        dir.resize(dir.size() - 1);
        dir = GetDir(dir);
    }
    while (!dirQueue.empty())
    {
        dir = dirQueue.top();
        dirQueue.pop();
        CreateDirectory(std::wstring(dir.begin(), dir.end()).c_str(), NULL);
    }
}

} // namespace ov


// KernelFiltering_Test.cpp : Этот файл содержит функцию "main". Здесь начинается и заканчивается выполнение программы.
//

#include "pch.h"
#include <iostream>
#include "LinkingTest.h"

#include <array>
#include "lodepng.h"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
using namespace boost::numeric::ublas;

#include "pngAs8bitMatrix.h"

#include "kernelGaussianLowPass.h"

//void runLinkingTest() {
//	// Initialize a Fibonacci relation sequence.
//// Write out the sequence values until overflow.	fibonacci_init(1, 1);
//
//	do {
//		std::cout << fibonacci_index() << ": "
//			<< fibonacci_current() << std::endl;
//	} while (fibonacci_next());
//	// Report count of values written before overflow.
//	std::cout << fibonacci_index() + 1 <<
//		" Fibonacci sequence values fit in an " <<
//		"unsigned 64-bit integer." << std::endl;
//}

int main()
{
	matrix<uint8_t> inputImage = readPNGRedAsMatrix("..\\..\\test_data\\subp_147_5-157_5.png");

	matrix<uint8_t> outputImage1 = matrix<uint8_t>(inputImage.size1(), inputImage.size2());
	applyLowPassGaussFilterToMatrix<uint8_t, double>(&outputImage1, &inputImage, 1, 1, 0.8);
	auto r1 = writePNGRedAsMatrix("..\\..\\test_data\\_lp_1_1_08_subp_147_5-157_5.png", &outputImage1);

	matrix<uint8_t> outputImage2 = matrix<uint8_t>(inputImage.size1(), inputImage.size2());
	apply2dLowPassGaussFilterToMatrix<uint8_t, double>(&outputImage2, &inputImage, 1, 3, 0.3, 1.0);
	auto r2 = writePNGRedAsMatrix("..\\..\\test_data\\_2dlp_1_3_03_10_subp_147_5-157_5.png", &outputImage2);

	return 0;
}

// Запуск программы: CTRL+F5 или меню "Отладка" > "Запуск без отладки"
// Отладка программы: F5 или меню "Отладка" > "Запустить отладку"

// Советы по началу работы
//   1. В окне обозревателя решений можно добавлять файлы и управлять ими.
//   2. В окне Team Explorer можно подключиться к системе управления версиями.
//   3. В окне "Выходные данные" можно просматривать выходные данные сборки и другие сообщения.
//   4. В окне "Список ошибок" можно просматривать ошибки.uin
//   5. Последовательно выберите пункты меню "Проект" > "Добавить новый элемент", чтобы создать файлы кода, или "Проект" > "Добавить существующий элемент", чтобы добавить в проект существующие файлы кода.
//   6. Чтобы снова открыть этот проект позже, выберите пункты меню "Файл" > "Открыть" > "Проект" и выберите SLN-файл.


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SVD>
#include <iostream>
#include <random>  // Requires C++ 11

#include <Spectra/contrib/PartialSVDSolver.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;
typedef Eigen::SparseMatrix<double> SpMatrix;

// Generate random sparse matrix
SpMatrix gen_sparse_data(int m, int n, double prob = 0.5)
{
    SpMatrix mat(m, n);
    std::default_random_engine gen;
    gen.seed(0);
    std::uniform_real_distribution<double> distr(0.0, 1.0);
    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            if (distr(gen) < prob)
                mat.insert(i, j) = distr(gen) - 0.5;
        }
    }
    return mat;
}

template <typename MatType>
void run_test(const MatType& mat, int k, int m)
{
    PartialSVDSolver<double, MatType> svds(mat, k, m);
    int nconv = svds.compute();

    INFO("nconv = " << nconv);
    REQUIRE(nconv == k);

    Vector svals = svds.singular_values();
    Matrix U = svds.matrix_U(k);
    Matrix V = svds.matrix_V(k);

    // SVD solver from Eigen
    // Requires dense matrices
    Matrix mat_dense = Matrix(mat);
    Eigen::JacobiSVD<Matrix> svd(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Vector svals_eigen = svd.singularValues();
    Matrix U_eigen = svd.matrixU();
    Matrix V_eigen = svd.matrixV();

    double err = (svals - svals_eigen.head(k)).array().abs().maxCoeff();
    INFO("Residual of singular values = " << err);
    REQUIRE(err == Approx(0.0).margin(1e-9));

    err = (U.array().abs() - U_eigen.leftCols(k).array().abs()).abs().maxCoeff();
    INFO("Residual of left singular vectors = " << err);
    REQUIRE(err == Approx(0.0).margin(1e-9));

    err = (V.array().abs() - V_eigen.leftCols(k).array().abs()).abs().maxCoeff();
    INFO("Residual of right singular vectors = " << err);
    REQUIRE(err == Approx(0.0).margin(1e-9));
}

TEST_CASE("Partial SVD of tall dense matrix [1000x100]", "[svds_dense_tall]")
{
    std::srand(123);

    const Matrix A = Matrix::Random(1000, 100);
    int k = 5;
    int m = 10;

    run_test<Matrix>(A, k, m);
}

TEST_CASE("Partial SVD of wide dense matrix [1000x100]", "[svds_dense_wide]")
{
    std::srand(123);

    const Matrix A = Matrix::Random(100, 1000);
    int k = 5;
    int m = 10;

    run_test<Matrix>(A, k, m);
}

TEST_CASE("Partial SVD of tall sparse matrix [1000x100]", "[svds_sparse_tall]")
{
    std::srand(123);

    const SpMatrix A = gen_sparse_data(1000, 100, 0.1);
    int k = 5;
    int m = 10;

    run_test<SpMatrix>(A, k, m);
}

TEST_CASE("Partial SVD of wide sparse matrix [1000x100]", "[svds_sparse_wide]")
{
    std::srand(123);

    const SpMatrix A = gen_sparse_data(100, 1000, 0.1);
    int k = 5;
    int m = 10;

    run_test<SpMatrix>(A, k, m);
}


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 5/18/2020, 3:01:31 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr long double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Point -----

class Point
{
public:
  ll x, v;

  Point() {}
  Point(ll x, ll v) : x{x}, v{v} {}

  long double now(long double t) const { return x + v * t; }
};

long double cross(Point const &p, Point const &q)
{
  if (q.v == p.v)
  {
    return 0.0;
  }
  return (static_cast<long double>(p.x) - q.x) / (q.v - p.v);
}

bool operator<(Point const &p, Point const &q)
{
  assert(p.v == q.v);
  return p.x < q.x;
}

// ----- Axis -----

class Axis
{
  vector<vector<Point>> points;
  vector<Point> candidates;
  vector<long double> snapshots;

public:
  Axis(vector<Point> const &V) : points(3)
  {
    for (auto const &p : V)
    {
      points[p.v].push_back(p);
    }
    for (auto i = 0; i < 3; ++i)
    {
      if (points[i].empty())
      {
        continue;
      }
      candidates.push_back(*max_element(points[i].begin(), points[i].end()));
      candidates.push_back(*min_element(points[i].begin(), points[i].end()));
    }
    for (auto it = candidates.begin(); it != candidates.end(); ++it)
    {
      for (auto it2 = it + 1; it2 != candidates.end(); ++it2)
      {
        long double tmp{cross(*it, *it2)};
        if (tmp > 0)
        {
          snapshots.push_back(tmp);
        }
      }
    }
  }

  long double delta(long double t) const;

  vector<long double> const &timer() const { return snapshots; }
};

long double Axis::delta(long double t) const
{
  vector<long double> V;
  for (auto const &p : candidates)
  {
    V.push_back(p.now(t));
  }
  return *max_element(V.begin(), V.end()) - *min_element(V.begin(), V.end());
}

// ----- main() -----

int main()
{
  vector<Point> PX, PY;
  int N;
  cin >> N;
  for (auto i = 0; i < N; ++i)
  {
    ll x, y;
    char c;
    cin >> x >> y >> c;
    if (c == 'R')
    {
      PX.emplace_back(x, 2);
      PY.emplace_back(y, 1);
    }
    else if (c == 'L')
    {
      PX.emplace_back(x, 0);
      PY.emplace_back(y, 1);
    }
    else if (c == 'U')
    {
      PX.emplace_back(x, 1);
      PY.emplace_back(y, 2);
    }
    else
    {
      PX.emplace_back(x, 1);
      PY.emplace_back(y, 0);
    }
  }
  Axis X(PX), Y(PY);
  vector<long double> timer;
  copy(X.timer().begin(), X.timer().end(), back_inserter(timer));
  copy(Y.timer().begin(), Y.timer().end(), back_inserter(timer));
  long double ans{X.delta(0) * Y.delta(0)};
  for (auto t : timer)
  {
    ch_min(ans, X.delta(t) * Y.delta(t));
#if DEBUG == 1
    cerr << "t = " << t << ", X.delta = " << X.delta(t) << ", Y.delta = " << Y.delta(t) << endl;
#endif
  }
  cout << fixed << setprecision(15) << ans << endl;
}


#ifndef JOINT_HPP
#define JOINT_HPP
#include <armadillo>
#include "aux.hh"
#include "body.hh"
#include "matrix_tool.hh"

class Joint
{
public:
    Joint(unsigned int TypeIn, arma::vec piIn, arma::vec pjIn, arma::vec qiIn,
          arma::vec qjIn, Body *i_In, Body *j_In);
    ~Joint(){};
    void Build_C();
    void Build_Cq();
    void Build_GAMMA();
    void update();

    arma::mat get_Cqi();
    arma::mat get_Cqj();
    arma::vec get_GAMMA();
    arma::vec get_Pi();
    arma::vec get_Pj();
    arma::vec get_CONSTRAINT();
    Body *get_body_i_ptr();
    Body *get_body_j_ptr();

private:
    unsigned int Type;
    VECTOR(pi, 3);
    VECTOR(pj, 3);
    VECTOR(qi, 3);
    VECTOR(qj, 3);
    MATRIX(Cqi, 3, 6);
    MATRIX(Cqj, 3, 6);
    VECTOR(GAMMA, 3);
    VECTOR(CONSTRAINT, 3);
    MATRIX(TBI_i, 3, 3);
    MATRIX(TBI_j, 3, 3);
    VECTOR(Pi, 3);
    VECTOR(Pj, 3);
    VECTOR(Qi, 3);
    VECTOR(Qj, 3);
    VECTOR(wi, 3);
    VECTOR(wj, 3);
    VECTOR(Si, 3);
    VECTOR(Sj, 3);
    Body *body_i_ptr;
    Body *body_j_ptr;
};

#endif  //JOINT_HPP


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/math/geometry.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/poses/Lie/SO.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::poses::Lie;

// See .h for documentation

// ====== SE(3) ===========
SE<3>::type SE<3>::exp(const SE<3>::tangent_vector& x)
{
	return CPose3D(Lie::SO<3>::exp(x.tail<3>()), x.head<3>());
}

SE<3>::tangent_vector SE<3>::log(const SE<3>::type& P)
{
	tangent_vector v;
	const auto log_R = mrpt::poses::Lie::SO<3>::log(P.getRotationMatrix());
	v[0] = P.x();
	v[1] = P.y();
	v[2] = P.z();
	v[3] = log_R[0];
	v[4] = log_R[1];
	v[5] = log_R[2];
	return v;
}

SE<3>::manifold_vector SE<3>::asManifoldVector(const SE<3>::type& pose)
{
	manifold_vector v;
	pose.getAs12Vector(v);
	return v;
}

SE<3>::type SE<3>::fromManifoldVector(const SE<3>::manifold_vector& v)
{
	return type(v);
}

// See 10.3.1 in \cite blanco_se3_tutorial
SE<3>::tang2mat_jacob SE<3>::jacob_dexpe_de(const SE<3>::tangent_vector& x)
{
	// 12x6 Jacobian:
	tang2mat_jacob J = tang2mat_jacob::Zero();
	J.block<3, 3>(9, 0) = Eigen::Matrix3d::Identity();
	const auto w = SO<3>::tangent_vector(x.tail<3>());
	J.block<9, 3>(0, 3) = SO<3>::jacob_dexpe_de(w).asEigen();
	return J;
}

SE<3>::mat2tang_jacob SE<3>::jacob_dlogv_dv(const SE<3>::type& P)
{
	mrpt::math::CMatrixDouble6_12 J;
	J.setZero();
	const CMatrixDouble33& R = P.getRotationMatrix();
	J.block<3, 9>(3, 0) = SO<3>::jacob_dlogv_dv(R).asEigen();
	J(0, 9) = J(1, 10) = J(2, 11) = 1.0;
	return J;
}

// Section 10.3.3 in tech report
// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
SE<3>::tang2mat_jacob SE<3>::jacob_dexpeD_de(const CPose3D& D)
{
	mrpt::math::CMatrixDouble12_6 jacob;
	jacob.block<9, 3>(0, 0).setZero();
	jacob.block<3, 3>(9, 0).setIdentity();
	for (int i = 0; i < 3; i++)
	{
		auto trg_blc = jacob.block<3, 3>(3 * i, 3);
		mrpt::math::skew_symmetric3_neg(
			D.getRotationMatrix().blockCopy<3, 1>(0, i), trg_blc);
	}
	{
		auto trg_blc = jacob.block<3, 3>(9, 3);
		mrpt::math::skew_symmetric3_neg(D.m_coords, trg_blc);
	}
	return jacob;
}

// Section 10.3.4 in tech report
// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
SE<3>::tang2mat_jacob SE<3>::jacob_dDexpe_de(const CPose3D& D)
{
	mrpt::math::CMatrixDouble12_6 jacob;
	const auto& dRot = D.getRotationMatrix();
	jacob.setZero();
	jacob.block<3, 3>(9, 0) = dRot.asEigen();

	jacob.block<3, 1>(3, 5) = -dRot.col(0);
	jacob.block<3, 1>(6, 4) = dRot.col(0);

	jacob.block<3, 1>(0, 5) = dRot.col(1);
	jacob.block<3, 1>(6, 3) = -dRot.col(1);

	jacob.block<3, 1>(0, 4) = -dRot.col(2);
	jacob.block<3, 1>(3, 3) = dRot.col(2);
	return jacob;
}

// Eq. 10.3.7 in tech report
// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
SE<3>::tang2mat_jacob SE<3>::jacob_dAexpeD_de(
	const CPose3D& A, const CPose3D& D)
{
	const auto& Arot = A.getRotationMatrix();

	mrpt::math::CMatrixDouble12_6 jacob;
	jacob.block<9, 3>(0, 0).setZero();
	jacob.block<3, 3>(9, 0) = A.getRotationMatrix().asEigen();
	Eigen::Matrix<double, 3, 3> aux;
	for (int i = 0; i < 3; i++)
	{
		mrpt::math::skew_symmetric3_neg(
			D.getRotationMatrix().blockCopy<3, 1>(0, i), aux);
		jacob.block<3, 3>(3 * i, 3) = Arot.asEigen() * aux;
	}
	mrpt::math::skew_symmetric3_neg(D.m_coords, aux);
	jacob.block<3, 3>(9, 3) = Arot.asEigen() * aux;
	return jacob;
}

void SE<3>::jacob_dDinvP1invP2_de1e2(
	const CPose3D& Dinv, const CPose3D& P1, const CPose3D& P2,
	mrpt::optional_ref<matrix_TxT> df_de1,
	mrpt::optional_ref<matrix_TxT> df_de2)
{
	using namespace mrpt::math;

	// The rotation matrix of the overall error expression:
	const CPose3D P1inv = -P1;
	const CPose3D DinvP1invP2 = Dinv + P1inv + P2;

	// Common part: d_PseudoLn(T)_dT:
	// (6x12 matrix)
	const auto dLnT_dT = SE<3>::jacob_dlogv_dv(DinvP1invP2);

	// See section 10.3.10 of Tech. report:
	// "A tutorial on SE(3) transformation parameterizations and on-manifold
	// optimization"
	if (df_de1)
	{
		matrix_TxT& J1 = df_de1.value().get();

		const CMatrixFixed<double, 12, 12> J1a =
			SE<3>::jacob_dAB_dA(Dinv, P1inv + P2);
		const auto J1b = CMatrixDouble12_6(-SE<3>::jacob_dDexpe_de(Dinv));

		J1 = dLnT_dT.asEigen() * J1a.asEigen() * J1b.asEigen();
	}
	if (df_de2)
	{
		matrix_TxT& J2 = df_de2.value().get();
		const auto dAe_de = SE<3>::jacob_dDexpe_de(DinvP1invP2);
		J2 = dLnT_dT * dAe_de;
	}
}

SE<3>::matrix_MxM SE<3>::jacob_dAB_dA(
	const SE<3>::type& A, const SE<3>::type& B)
{
	using namespace mrpt::math;

	matrix_MxM J = matrix_MxM::Zero();
	// J_wrt_A = kron(B,eye(3));
	const auto B_HM =
		B.getHomogeneousMatrixVal<CMatrixDouble44>().transpose().eval();
	for (int c = 0; c < 4; c++)
		for (int r = 0; r < 4; r++)
			for (int q = 0; q < 3; q++) J(r * 3 + q, c * 3 + q) = B_HM(r, c);

	return J;
}

SE<3>::matrix_MxM SE<3>::jacob_dAB_dB(
	const SE<3>::type& A, const SE<3>::type& B)
{
	matrix_MxM J = matrix_MxM::Zero();
	// J_wrt_B = kron(eye(3),A_rot);
	const auto& AR = A.getRotationMatrix();
	for (int c = 0; c < 4; c++) J.block<3, 3>(c * 3, c * 3) = AR.asEigen();
	return J;
}

// See .h for documentation
// ====== SE(2) ===========
SE<2>::type SE<2>::exp(const SE<2>::tangent_vector& x)
{
	SE<2>::type P;
	P.x(x[0]);
	P.y(x[1]);
	P.phi(x[2]);
	return P;
}

SE<2>::tangent_vector SE<2>::log(const SE<2>::type& P)
{
	SE<2>::tangent_vector x;
	x[0] = P.x();
	x[1] = P.y();
	x[2] = mrpt::math::wrapToPi(P.phi());
	return x;
}

SE<2>::manifold_vector SE<2>::asManifoldVector(const SE<2>::type& pose)
{
	manifold_vector v;
	v[0] = pose.x();
	v[1] = pose.y();
	v[2] = mrpt::math::wrapToPi(pose.phi());
	return v;
}

SE<2>::type SE<2>::fromManifoldVector(const SE<2>::manifold_vector& v)
{
	return type(v[0], v[1], mrpt::math::wrapToPi(v[2]));
}

SE<2>::matrix_MxM SE<2>::jacob_dAB_dA(
	const SE<2>::type& A, const SE<2>::type& B)
{
	const auto bx = B.x(), by = B.y();
	const auto cphia = A.phi_cos(), sphia = A.phi_sin();

	matrix_MxM J = matrix_MxM::Identity();
	J(0, 2) = -bx * sphia - by * cphia;
	J(1, 2) = +bx * cphia - by * sphia;
	return J;
}

SE<2>::matrix_MxM SE<2>::jacob_dAB_dB(
	const SE<2>::type& A, const SE<2>::type& B)
{
	matrix_MxM J = matrix_MxM::Identity();
	const auto cphia = A.phi_cos(), sphia = A.phi_sin();
	J(0, 0) = cphia;
	J(0, 1) = -sphia;
	J(1, 0) = sphia;
	J(1, 1) = cphia;
	return J;
}

SE<2>::tang2mat_jacob SE<2>::jacob_dDexpe_de(const SE<2>::type& D)
{
	const auto c = D.phi_cos(), s = D.phi_sin();

	// clang-format off
	return SE<2>::tang2mat_jacob((Eigen::Matrix3d() <<
	        c, -s, 0,
	        s,  c, 0,
	        0,  0, 1
	        ).finished());
	// clang-format on
}

void SE<2>::jacob_dDinvP1invP2_de1e2(
	const CPose2D& Dinv, const CPose2D& P1, const CPose2D& P2,
	mrpt::optional_ref<matrix_TxT> df_de1,
	mrpt::optional_ref<matrix_TxT> df_de2)
{
	const CPose2D P1inv = -P1;
	const CPose2D P1invP2 = P1inv + P2;
	const CPose2D DinvP1invP2 = Dinv + P1invP2;

	if (df_de1)
	{
		auto& J1 = df_de1.value().get();
		J1 = jacob_dAB_dA(Dinv, P1invP2).asEigen() * (-jacob_dDexpe_de(Dinv));
	}

	if (df_de2)
	{
		auto& J2 = df_de2.value().get();
		J2 = SE<2>::jacob_dDexpe_de(DinvP1invP2);
	}
}


// Copyright ETHZ 2017
// autor: mathias, 2017, mathias.rothermel@geod.baug.ethz.ch
#include "OriGeom.h"
#include "Orientation.h"
#include <math.h>
#include <Eigen/Dense>

namespace OriGeom
{

	void computeRelative( const Orientation &identity, const Orientation &other, Orientation &out )
	{
	    	// Copy
	    	out=other;

		// First Cam (after transform this is identity)
		Eigen::Vector3d C0=identity.getC();
		Eigen::Matrix3d R0=identity.getR();
		Eigen::Vector3d t0=(-1.0)*R0*C0;

		// Second cam
		Eigen::Vector3d C1=other.getC();
		Eigen::Matrix3d R1=other.getR();
		Eigen::Matrix3d K1=other.getK();
		Eigen::Vector3d t1=(-1.0)*R1*C1;

		// New cam
		Eigen::Matrix3d R10=R1*R0.transpose();
		Eigen::Vector3d C10=R1*C0+t1;

		out.setK(K1);
		out.setR(R10);
		out.setC(C10);
	}
}


#include <boost/test/unit_test.hpp>
#include "functions/all_simplifications.hh"
#include "functions/complex.hh"
#include "functions/operators.hh"
#include "functions/streaming.hh"

BOOST_AUTO_TEST_CASE(complex_test) {
  using namespace manifolds;
  static_assert(ComplexOutputType<decltype(x)>::value == NeverComplex, "");
  BOOST_CHECK_EQUAL(Simplify(Real()(x)), x);
  BOOST_CHECK_EQUAL(Simplify(Imag()(x)), zero);
  BOOST_CHECK_EQUAL(Phase(), Arg());
  auto r_phase = Simplify(Phase()(x));
  BOOST_CHECK_EQUAL(r_phase(3), 0);
  BOOST_CHECK_EQUAL(r_phase(-3), -M_PI);
  BOOST_CHECK_EQUAL(Simplify(Real()(I)), zero);
  BOOST_CHECK_EQUAL(Simplify(Imag()(I)), IP<1>());
  BOOST_CHECK_EQUAL(Simplify(Norm()(I)), IP<1>());
  BOOST_CHECK_EQUAL(Simplify(Norm()(x)), x * x);
  BOOST_CHECK_EQUAL(Simplify(Conjugate()(I)), -I);
  BOOST_CHECK_EQUAL(Simplify(Conjugate()(x)), x);
  BOOST_CHECK_EQUAL(Simplify(I(x)), I);
  BOOST_CHECK_EQUAL(Simplify(IP<1, 1, 1, 1, 1, 1>()(I)), IP<1>() + I);
  BOOST_CHECK_EQUAL(Simplify(IP<0, 0, 1>()(I)), IP<-1>());
  BOOST_CHECK_EQUAL(Simplify(Real()(IP<1>() * I + x)), x);
  BOOST_CHECK_EQUAL(Simplify(Imag()(IP<1>() * I + x)), IP<1>());
  BOOST_CHECK_EQUAL(Simplify(Real()((I + Sin()(x)) * (I + Cos()(x)))),
                    (Sin() * Cos())(x) - IP<1>());
  BOOST_CHECK_EQUAL(Simplify(IP<1, 2, 3, 4, 5, 6, 7>()(Sign())),
                    (IP<16, 12>()(Sign())));
  BOOST_CHECK_EQUAL(Simplify(GetPolynomial(1, 2, 3, 4, 5, 6, 7)(Sign())),
                    GetPolynomial(16, 12)(Sign()));
}


#include <iostream>
#include <boost/multiprecision/gmp.hpp>
#include <string>

namespace mp = boost::multiprecision;

int main(int argc, char const *argv[])
{
    // We could just use (1 << 18) instead of tmpres, but let's point out one
    // pecularity with gmp and hence boost::multiprecision: they won't accept
    // a second mpz_int with pow(). Therefore, if we stick to multiprecision
    // pow we need to convert_to<uint64_t>().
    uint64_t tmpres = mp::pow(mp::mpz_int(4)
                            , mp::pow(mp::mpz_int(3)
                                    , 2).convert_to<uint64_t>()
                                      ).convert_to<uint64_t>();
    mp::mpz_int res = mp::pow(mp::mpz_int(5), tmpres);
    std::string s = res.str();
    std::cout << s.substr(0, 20)
              << "..."
              << s.substr(s.length() - 20, 20) << std::endl;
    return 0;
}