#include "visuality/view.h"
#include "details/io_details.h"

#include <gtest/gtest.h>
#include <boost/unordered_map.hpp>

#ifdef USE_TBB
#  include "tbb/task_scheduler_init.h"
#  include "tbb/task.h"
#  include "tbb/concurrent_hash_map.h"
#  include <tbb/scalable_allocator.h>
#endif

// C++98
#include <vector>
#include <ostream>
#include <string>

using namespace std;
#ifdef USE_TBB
using namespace tbb;
#endif

using view::operator<<;
using io_details::Item;
using io_details::get_test_items;
using io_details::get_dyn_items;

struct TaskId {
  TaskId(int _w_bound, int _idx) : w_bound(_w_bound), idx(_idx) {}
  TaskId() : w_bound(0), idx(0) {}
  int w_bound;
  int idx;
};

// Structure that defines hashing and comparison operations for user's type.
struct TBBHashCompare {
    static size_t hash( const TaskId& k ) {
        return boost::hash<int>()(k.idx) ^ (boost::hash<int>()(k.w_bound) << 1);//KeyHash()(k);
    }
    //! True if strings are equal
    static bool equal( const TaskId& lhs, const TaskId& rhs ) {
        return lhs.idx == rhs.idx && lhs.w_bound == rhs.w_bound;//KeyEqual()(x,y);
    }
};

#ifdef USE_TBB

// Аллокатор тоже не помогает
//  tbb::scalable_allocator<pair<TaskId, int> >
typedef concurrent_hash_map<TaskId, int, TBBHashCompare> TaskTable;

// Alse exist:
//   http://www.threadingbuildingblocks.org/docs/help/reference/containers_overview/concurrent_unordered_map_cls.htm

// скорости не добавляет, а наоборот медленнее работает
template <typename Store>
struct Future: public task {
    int* const r_;
    int x;
    int y;
    Store* const store_;
    const TaskId id_;
    int delta;

    Future( int* sum_, Store* store, const TaskId& id) : r_(sum_), store_(store), id_(id) {}
    task* execute() {

      x += delta;
      *r_ = std::max(x, y);

      bool connect = true;
      if (connect) {
        // DANGER: похоже некоторые задачи таки решаются не один раз - и это уменьшает быстродействие
        TaskTable::accessor a;
        if (store_->insert(a, id_)) {
          a->second = *r_;
        } else {
          // DANGER: промахов очень много
          //cout << "BAD: Vain solved\n";
        }
      }
      return NULL;
    }
};

template <typename Store>
class KnapsackTaskFuture : public task  {
  Store* const store_;
  const vector<Item>* items_;
  int* const r_;
  const TaskId id_;

  KnapsackTaskFuture(const KnapsackTaskFuture&);
  KnapsackTaskFuture& operator=(const KnapsackTaskFuture&);

public:
  KnapsackTaskFuture(const vector<Item>& items, Store& store, int& result, const TaskId& id)
    : store_(&store), items_(&items), r_(&result), id_(id)
  {}

  task* execute()
  {
    const bool connect = true;
    // Похоже так лихо не выйдет - задача обязуется выполнить вычисления а сумма фейковая
    // Если решена то вернуть результат
    // Распространить лок не удалось пока что.
    if (connect) {
      //TaskTable::accessor a;
      TaskTable::const_accessor a;
      if (store_->find(a, id_)) {
        *r_ = a->second;
        return NULL;
      }
    }

    // Некоторые задачи отбросили, но некоторые в процессе решения и будут готовы к моменту записи
    {
      // Work
      int n = id_.idx;
      int w_bound = id_.w_bound;

      // Base Case
      if (n == 0 || w_bound == 0) {
        *r_ = 0;  // можно сохранять, но на производительность сильно не влияет
        return NULL;
      }

      // Prepare
      int delta = 0;
      TaskId work_id(w_bound, n-1);
      if (!((*items_)[n-1].w > w_bound)) {
        work_id = TaskId(w_bound-(*items_)[n-1].w, n-1);
        delta = (*items_)[n-1].v;
      }
      TaskId old_task(w_bound, n-1);

      // Actions
      Future<TaskTable>& c = *new( allocate_continuation() ) Future<TaskTable>(r_, store_, id_);
      c.delta = delta;
      KnapsackTaskFuture& a = *new( c.allocate_child() ) KnapsackTaskFuture(*items_, *store_, c.x, work_id);
      KnapsackTaskFuture& b = *new( c.allocate_child() ) KnapsackTaskFuture(*items_, *store_, c.y, old_task);
      // Set ref_count to 'two children plus one for the wait".
      c.set_ref_count(2);
      // Start b running.
      spawn( b );
      //spawn( a );
      return &a;  // кажется чуть быстрее чем spawn(a), но все равно медленно
      // Start a running and wait for all children (a and b).
      //spawn_and_wait_for_all(a);

    }
    return NULL;
  }
};

// DANGER: алгоритм пробуксовывает - иногда решает уже решенные задачи
template <typename Store>
class KnapsackTaskSerial : public task  {
  Store* const store_;
  const vector<Item>* items_;
  int* const r_;
  const TaskId id_;

  KnapsackTaskSerial(const KnapsackTaskSerial&);
  KnapsackTaskSerial& operator=(const KnapsackTaskSerial&);

public:
  KnapsackTaskSerial(const vector<Item>& items, Store& store, int& result, const TaskId& id)
    : store_(&store), items_(&items), r_(&result), id_(id)
  {}

  task* execute()
  {
    const bool connect = true;
    // Похоже так лихо не выйдет - задача обязуется выполнить вычисления а сумма фейковая
    // Если решена то вернуть результат
    // Распространить лок не удалось пока что.
    if (connect) {
      //TaskTable::accessor a;
      TaskTable::const_accessor a;
      if (store_->find(a, id_)) {
        *r_ = a->second;
        return NULL;
      }
    }

    // Некоторые задачи отбросили, но некоторые в процессе решения и будут готовы к моменту записи
    {
      // Work
      int n = id_.idx;
      int w_bound = id_.w_bound;

      // Base Case
      if (n == 0 || w_bound == 0) {
        *r_ = 0;  // можно сохранять, но на производительность сильно не влияет
        return NULL;
      }

      // Prepare
      int delta = 0;
      TaskId work_id(w_bound, n-1);
      if (!((*items_)[n-1].w > w_bound)) {
        work_id = TaskId(w_bound-(*items_)[n-1].w, n-1);
        delta = (*items_)[n-1].v;
      }
      TaskId old_task(w_bound, n-1);

      // Actions
      int x = 0;
      int y = 0;
      KnapsackTaskSerial& a = *new( allocate_child() ) KnapsackTaskSerial(*items_, *store_, x, work_id);
      KnapsackTaskSerial& b = *new( allocate_child() ) KnapsackTaskSerial(*items_, *store_, y, old_task);
      // Set ref_count to 'two children plus one for the wait".
      set_ref_count(3);
      // Start b running.
      spawn( b );
      // Start a running and wait for all children (a and b).
      spawn_and_wait_for_all(a);  // BAD: NEED NO WAIT!

      x += delta;
      *r_ = std::max(x, y);
    }

    if (connect) {
      // DANGER: похоже некоторые задачи таки решаются не один раз - и это уменьшает быстродействие
      TaskTable::accessor a;
      if (store_->insert(a, id_)) {
        a->second = *r_;
      } else {
        // DANGER: промахов очень много
        //cout << "BAD: Vain solved\n";
      }
    }

    return NULL;
  }
};

TEST(W3, ParallelKnap) {
  TaskTable g_task_table;
  pair<int, vector<Item > > tmp =
    get_dyn_items("./input_data/knapsack_big.txt");
    //get_test_items("NoFile");
  vector<Item> items = tmp.second;
  int W = tmp.first;
  int count = items.size();
  int result = 0;

  TaskId root(W, count);
  //int store = 0;
  KnapsackTaskSerial<TaskTable>& a = *new(task::allocate_root())
      KnapsackTaskSerial<TaskTable>(items, g_task_table, result, root);
  task::spawn_root_and_wait(a);
  //KnapsackTaskSerial<int>(items, store, result, root).execute();

  printf("sum(v(i)) = %d \n", result);
  //assert(result == 8);
  assert(result == 4243395);
}

TEST(W3, ParallelKnapNoWait) {
  TaskTable g_task_table;
  pair<int, vector<Item > > tmp =
    get_dyn_items("./input_data/knapsack_big.txt");
    //get_test_items("NoFile");
  vector<Item> items = tmp.second;
  int W = tmp.first;
  int count = items.size();
  int result = 0;

  TaskId root(W, count);
  //int store = 0;
  KnapsackTaskFuture<TaskTable>& a = *new(task::allocate_root())
      KnapsackTaskFuture<TaskTable>(items, g_task_table, result, root);
  task::spawn_root_and_wait(a);
  //KnapsackTaskSerial<int>(items, store, result, root).execute();

  printf("sum(v(i)) = %d \n", result);
  //assert(result == 8);
  assert(result == 4243395);
}
#endif


//  Copyright 2015 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/detail/config.hpp>

#if BOOST_MP11_MSVC
# pragma warning( disable: 4503 ) // decorated name length exceeded
#endif

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>
#include <utility>

struct X1 {};

using boost::mp11::mp_bool;

template<class T> using is_even = mp_bool< T::value % 2 == 0 >;

int main()
{
    using boost::mp11::mp_list;
    using boost::mp11::mp_count_if;
    using boost::mp11::mp_size_t;

    {
        using L1 = mp_list<>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L1, std::is_const>, mp_size_t<0>>));

        using L2 = mp_list<X1, X1 const, X1*, X1 const, X1*, X1*>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_volatile>, mp_size_t<0>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_const>, mp_size_t<2>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_pointer>, mp_size_t<3>>));
    }

    {
        using L1 = std::tuple<>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L1, std::is_const>, mp_size_t<0>>));

        using L2 = std::tuple<X1, X1 const, X1*, X1 const, X1*, X1*>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_volatile>, mp_size_t<0>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_const>, mp_size_t<2>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_pointer>, mp_size_t<3>>));
    }

    {
        using L2 = std::pair<X1 const, X1*>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_volatile>, mp_size_t<0>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_const>, mp_size_t<1>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_count_if<L2, std::is_pointer>, mp_size_t<1>>));
    }

    {
        using boost::mp11::mp_iota_c;

        int const N = 1089;

        using L = mp_iota_c<N>;
        using R = mp_count_if<L, is_even>;

        BOOST_TEST_TRAIT_TRUE((std::is_same<R, mp_size_t<(N + 1) / 2>>));
    }

    return boost::report_errors();
}


#include <crab/common/types.hpp>
#include <crab/numbers/wrapint.hpp>
//#include <boost/functional/hash.hpp>

#include <climits>
#include <sstream>
#include <string>

namespace crab {

void wrapint::sanity_check_bitwidth() const {
  if (_width == 0) {
    CRAB_ERROR("no bitwidth found for a wrapint");
  }
  if (_width > 64) {
    CRAB_ERROR(_width, " is a too big bitwidth for a wrapint");
  }
}

void wrapint::sanity_check_bitwidths(const wrapint &other) const {
  if (_width != other._width) {
    CRAB_ERROR("two wrapint numbers with different bitwidths");
  }
}

void wrapint::compute_mod() {
  assert(_width <= 64);
  switch (_width) {
  case 8:
    _mod = mod_8;
    break;
  case 16:
    _mod = mod_16;
    break;
  case 32:
    _mod = mod_32;
    break;
  case 64:
    break;
  default:
    _mod = (uint64_t)1 << (uint64_t)_width;
  }
}

wrapint::wrapint(uint64_t n, bitwidth_t width, uint64_t mod)
    : _n(n), _width(width), _mod(mod) {}

wrapint::wrapint(uint64_t n, bitwidth_t width) : _n(n), _width(width), _mod(0) {

  sanity_check_bitwidth();
  assert(_width <= 64);
  compute_mod();

  if (_width < 64) {
    _n = _n % _mod;
  }
}

wrapint::wrapint(ikos::z_number n, bitwidth_t width)
    : _n(0), _width(width), _mod(0) {

  sanity_check_bitwidth();
  assert(_width <= 64);
  compute_mod();
  if (!n.fits_int64()) {
    CRAB_ERROR(n, " does not fit in an int64_t.");
  }
  int64_t x = static_cast<int64_t>(n);
  if (_width == 64) {
    _n = static_cast<uint64_t>(x);
  } else {
    _n = static_cast<uint64_t>(x) % _mod;
  }
}

wrapint::wrapint(ikos::q_number n, bitwidth_t width)
    : _n(0), _width(width), _mod(0) {

  sanity_check_bitwidth();
  assert(_width <= 64);
  compute_mod();
  ikos::z_number i = n.round_to_upper();
  if (!i.fits_int64()) {
    CRAB_ERROR(n, " does not fit in an int64_t.");
  }
  int64_t x = static_cast<int64_t>(i);
  if (_width == 64) {
    _n = static_cast<uint64_t>(x);
  } else {
    _n = static_cast<uint64_t>(x) % _mod;
  }
}

wrapint::wrapint(std::string s, bitwidth_t width)
    : _n(0), _width(width), _mod(0) {

  sanity_check_bitwidth();
  assert(_width <= 64);
  compute_mod();
  std::istringstream iss(s);
  iss >> _n;

  if (_width < 64) {
    _n = _n % _mod;
  }
}

wrapint::bitwidth_t wrapint::get_bitwidth() const { return _width; }

bool wrapint::fits_wrapint(ikos::z_number n, bitwidth_t width) {
  if (width > 64)
    return false;
  return n.fits_int64();
}

bool wrapint::fits_wrapint(ikos::q_number n, bitwidth_t width) {
  return fits_wrapint(n.round_to_upper(), width);
}

// return true iff most significant bit is 1.
bool wrapint::msb() const {
  return (_n & ((uint64_t)1 << (uint64_t)(_width - 1)));
}

// return 01111...1
wrapint wrapint::get_signed_max(bitwidth_t w) {
  return wrapint(((uint64_t)1 << (uint64_t)(w - 1)) - 1, w);
}

// return 1000....0
wrapint wrapint::get_signed_min(bitwidth_t w) {
  return wrapint((uint64_t)1 << (uint64_t)(w - 1), w);
}

// return 1111....1
wrapint wrapint::get_unsigned_max(bitwidth_t w) {
  switch (w) {
  case 8:
    return wrapint(mod_8 - 1, w);
  case 16:
    return wrapint(mod_16 - 1, w);
  case 32:
    return wrapint(mod_32 - 1, w);
  case 64:
    return wrapint(UINT64_MAX, w);
  default:
    return wrapint(((uint64_t)1 << (uint64_t)w) - 1, w);
  }
}

// return 0000....0
wrapint wrapint::get_unsigned_min(bitwidth_t w) { return wrapint(0, w); }

// return the wrapint as an unsigned number
std::string wrapint::get_unsigned_str() const {
  return get_unsigned_bignum().get_str();
}

// return the wrapint as a signed number
std::string wrapint::get_signed_str() const {
  return get_signed_bignum().get_str();
}

uint64_t wrapint::get_uint64_t() const { return _n; }

// return the wrapint as an unsigned big number
ikos::z_number wrapint::get_unsigned_bignum() const {
  // XXX: cannot use here ikos::z_number(_n) because it will cast _n
  // implicitly to a signed integer.
  return ikos::z_number::from_uint64(_n);
}

// return the wrapint as a signed big number
ikos::z_number wrapint::get_signed_bignum() const {
  if (msb()) {
    // get two's complement and negate
    wrapint r = *this ^ get_unsigned_max(get_bitwidth());
    return ikos::z_number(-(r.get_unsigned_bignum() + 1));
  } else {
    return get_unsigned_bignum();
  }
}

bool wrapint::is_zero() const { return _n == 0; }

wrapint wrapint::operator+(wrapint x) const {
  sanity_check_bitwidths(x);

  uint64_t r = (_width == 64 ? (_n + x._n) : (_n + x._n) % _mod);
  return wrapint(r, _width, _mod);
}

wrapint wrapint::operator*(wrapint x) const {
  sanity_check_bitwidths(x);

  uint64_t r = (_width == 64 ? (_n * x._n) : (_n * x._n) % _mod);
  return wrapint(r, _width, _mod);
}

wrapint wrapint::operator-(wrapint x) const {
  sanity_check_bitwidths(x);

  uint64_t r = (_width == 64 ? (_n - x._n) : (_n - x._n) % _mod);
  return wrapint(r, _width, _mod);
}

wrapint wrapint::operator-() const {
  uint64_t r = (_width == 64 ? -_n : -_n % _mod);
  return wrapint(r, _width, _mod);
}

// signed division
wrapint wrapint::operator/(wrapint x) const { return sdiv(x); }

// signed division: rounding towards 0
wrapint wrapint::sdiv(wrapint x) const {
  sanity_check_bitwidths(x);
  if (x.is_zero()) {
    CRAB_ERROR("wrapint: signed division by zero ", __LINE__);
  } else {
    ikos::z_number dividend = get_signed_bignum();
    ikos::z_number divisor = x.get_signed_bignum();
    ikos::z_number r = dividend / divisor;
    return wrapint(r, get_bitwidth());
  }
}

// unsigned division: rounding towards 0
wrapint wrapint::udiv(wrapint x) const {
  sanity_check_bitwidths(x);
  if (x.is_zero()) {
    CRAB_ERROR("wrapint: unsigned division by zero ", __LINE__);
  } else {
    uint64_t r = (_width == 64 ? _n / x._n : (_n / x._n) % _mod);
    return wrapint(r, _width, _mod);
  }
}

// signed remainder
wrapint wrapint::operator%(wrapint x) const { return srem(x); }

// signed rem: is the remainder of the signed division so rounding
// also towards 0.
wrapint wrapint::srem(wrapint x) const {
  sanity_check_bitwidths(x);
  if (x.is_zero()) {
    CRAB_ERROR("wrapint: signed division by zero ", __LINE__);
  } else {
    ikos::z_number dividend = get_signed_bignum();
    ikos::z_number divisor = x.get_signed_bignum();
    ikos::z_number r = dividend % divisor;
    return wrapint(r, get_bitwidth());
  }
}

// unsigned rem: is the remainder of unsigned division so rounding
// also towards 0.
wrapint wrapint::urem(wrapint x) const {
  sanity_check_bitwidths(x);
  if (x.is_zero()) {
    CRAB_ERROR("wrapint: unsigned division by zero ", __LINE__);
  } else {
    uint64_t r = (_width == 64 ? _n % x._n : (_n % x._n) % _mod);
    return wrapint(r, _width, _mod);
  }
}

wrapint &wrapint::operator+=(wrapint x) {
  sanity_check_bitwidths(x);
  _n += x._n;
  if (_width < 64)
    _n = _n % _mod;
  return *this;
}

wrapint &wrapint::operator*=(wrapint x) {
  sanity_check_bitwidths(x);
  _n *= x._n;
  if (_width < 64)
    _n = _n % _mod;
  return *this;
}

wrapint &wrapint::operator-=(wrapint x) {
  sanity_check_bitwidths(x);
  _n -= x._n;
  if (_width < 64)
    _n = _n % _mod;
  return *this;
}

wrapint &wrapint::operator--() {
  --(_n);
  if (_width < 64)
    _n = _n % _mod;
  return *this;
}

wrapint &wrapint::operator++() {
  ++(_n);
  if (_width < 64)
    _n = _n % _mod;
  return *this;
}

wrapint wrapint::operator++(int) {
  wrapint r(*this);
  ++(*this);
  return r;
}

wrapint wrapint::operator--(int) {
  wrapint r(*this);
  --(*this);
  return r;
}

bool wrapint::operator==(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n == x._n;
}

bool wrapint::operator!=(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n != x._n;
}

bool wrapint::operator<(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n < x._n;
}

bool wrapint::operator<=(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n <= x._n;
}

bool wrapint::operator>(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n > x._n;
}

bool wrapint::operator>=(wrapint x) const {
  sanity_check_bitwidths(x);
  return _n >= x._n;
}

wrapint wrapint::operator&(wrapint x) const {
  sanity_check_bitwidths(x);
  return wrapint(_n & x._n, _width, _mod);
}

wrapint wrapint::operator|(wrapint x) const {
  sanity_check_bitwidths(x);
  return wrapint(_n | x._n, _width, _mod);
}

wrapint wrapint::operator^(wrapint x) const {
  sanity_check_bitwidths(x);
  return wrapint(_n ^ x._n, _width, _mod);
}

wrapint wrapint::operator<<(wrapint x) const {
  sanity_check_bitwidths(x);

  uint64_t r = (_width == 64 ? (_n << x._n) : (_n << x._n) % _mod);
  return wrapint(r, _width, _mod);
}

// logical right shift: blanks filled by 0's
wrapint wrapint::lshr(wrapint x) const {
  sanity_check_bitwidths(x);
  return wrapint(_n >> x._n, _width, _mod);
}

// arithmetic right shift
wrapint wrapint::ashr(wrapint x) const {
  sanity_check_bitwidths(x);
  if (!msb()) {
    return wrapint(_n >> x._n, _width, _mod);
  } else {
    // fill blanks with 1's
    uint64_t all_ones =
        (_width < 64 ? ((uint64_t)1 << (uint64_t)_width) - 1 : UINT64_MAX);
    // 1110..0
    uint64_t only_upper_bits_ones = all_ones << (uint64_t)(_width - x._n);
    return wrapint(only_upper_bits_ones | (_n >> x._n), _width, _mod);
  }
}

wrapint wrapint::sext(bitwidth_t bits_to_add) const {
  bitwidth_t new_width = _width + bits_to_add;
  if (new_width > 64) {
    CRAB_ERROR("cannot signed extend: ", new_width,
               " is a too big bitwidth for a wrapint");
  }

  if (msb()) {
    // -- fill upper bits with ones
    // 111...1
    uint64_t all_ones =
        (new_width < 64 ? ((uint64_t)1 << (uint64_t)new_width) - 1
                        : UINT64_MAX);
    // 1110..0
    uint64_t only_upper_bits_ones = all_ones << (uint64_t)_width;
    return wrapint(_n | only_upper_bits_ones, new_width);
  } else {
    // -- fill upper bits with zeros
    return wrapint(_n, _width + bits_to_add);
  }
}

wrapint wrapint::zext(bitwidth_t bits_to_add) const {
  bitwidth_t new_width = _width + bits_to_add;
  if (new_width > 64) {
    CRAB_ERROR("cannot unsigned extend: ", new_width,
               " is a too big bitwidth for a wrapint");
  }
  return wrapint(_n, new_width);
}

wrapint wrapint::keep_lower(bitwidth_t bits_to_keep) const {
  if (bits_to_keep >= _width)
    return *this;
  return wrapint(_n & (((uint64_t)1 << (uint64_t)(bits_to_keep + 1)) - 1),
                 bits_to_keep);
}

void wrapint::write(crab::crab_os &o) const { o << get_unsigned_str(); }

// inline std::size_t hash_value(const wrapint& n) {
//   boost::hash<std::string> hasher;
//   return hasher(n.get_unsigned_str());
// }

} // end namespace crab


#include <vector>
#include <iostream>
#include <fstream>
#include <Eigen/Core>

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>

inline bool is_white_space(char c)
{
    return ((c) == ' ' || (c) == '\t');
};

inline bool is_valid_digit(char c)
{
    return ((c) >= '0' && (c) <= '9');
};

inline void skip_whitespace(char * &c)
{
    while (is_white_space(*c) ) c++;
}

inline void skip_until_whitespace(char * &c)
{
    while (*c && !is_white_space(*c)) c++;
}

inline int fast_atoi(char * &c)
{
    int val = 0;
    while(is_valid_digit(*c)) {
        val = val*10 + (*c - '0');
        c++;
    }
    return val;
}

// this function was adapted from Tom Van Baaks code here: http://leapsecond.com/tools/fast_atof.c
inline double fast_atod(char * &p)
{
    int frac;
    double sign, value, scale;

    // Get sign, if any.

    sign = 1.0;
    if (*p == '-') {
        sign = -1.0;
        p += 1;

    } else if (*p == '+') {
        p += 1;
    }

    // Get digits before decimal point or exponent, if any.

    for (value = 0.0; is_valid_digit(*p); p += 1) {
        value = value * 10.0 + (*p - '0');
    }

    // Get digits after decimal point, if any.

    if (*p == '.') {
        double pow10 = 10.0;
        p += 1;
        while (is_valid_digit(*p)) {
            value += (*p - '0') / pow10;
            pow10 *= 10.0;
            p += 1;
        }
    }

    // Handle exponent, if any.

    frac = 0;
    scale = 1.0;
    if ((*p == 'e') || (*p == 'E')) {
        unsigned int expon;

        // Get sign of exponent, if any.

        p += 1;
        if (*p == '-') {
            frac = 1;
            p += 1;

        } else if (*p == '+') {
            p += 1;
        }

        // Get digits of exponent, if any.

        for (expon = 0; is_valid_digit(*p); p += 1) {
            expon = expon * 10 + (*p - '0');
        }
        if (expon > 308) expon = 308;

        // Calculate scaling factor.

        while (expon >= 50) { scale *= 1E50; expon -= 50; }
        while (expon >=  8) { scale *= 1E8;  expon -=  8; }
        while (expon >   0) { scale *= 10.0; expon -=  1; }
    }

    // Return signed and scaled floating point result.

    return sign * (frac ? (value / scale) : (value * scale));
}

using RowMatX3d = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;
using RowMatXi = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

std::pair<RowMatX3d, RowMatXi>
loadOBJFast(std::string filename)
{
    std::ifstream inF(filename, std::fstream::in | std::fstream::binary);
    std::string line;

    std::vector<std::array<double, 3>> verts_vec;
    std::vector<std::array<int, 4>> quads;
    std::vector<std::array<int, 3>> tris;
    int faceDim = 0;

    while (std::getline(inF, line)) {
        char* c = &line[0];
        if (c[0] == 'v' && is_white_space(c[1])) {
            c += 2;
            skip_whitespace(c);
            // parse vertex coordinates
            double x = fast_atod(c);
            skip_whitespace(c);
            double y = fast_atod(c);
            skip_whitespace(c);
            double z = fast_atod(c);
            verts_vec.push_back({x, y, z});
            // TODO: check if there is a vertex color, read that
        }
        else if (c[0] == 'f' && is_white_space(c[1])) {
            c += 2;
            skip_whitespace(c);
            if (faceDim == 0) {
                // determine face dimension - quads or triangles?
                auto c_lookahead = c;
                for (; is_valid_digit(*c_lookahead); faceDim++) {
                    skip_until_whitespace(c_lookahead);
                    skip_whitespace(c_lookahead);
                }
            }
            if (faceDim == 3) {
                // parse triangle
                std::array<int, 3> tri;
                tri[0] = fast_atoi(c) - 1;
                skip_until_whitespace(c); skip_whitespace(c);
                tri[1] = fast_atoi(c) - 1;
                skip_until_whitespace(c); skip_whitespace(c);
                tri[2] = fast_atoi(c) - 1;
                if (tri[0] < 0 || tri[1] < 0 || tri[2] < 0) {
                    std::cerr << "negative face index found, ignoring" << std::endl;
                }
                else {
                    tris.push_back(tri);
                }
            } else {
                // parse quad
                std::array<int, 4> quad;
                quad[0] = fast_atoi(c) - 1;
                skip_until_whitespace(c); skip_whitespace(c);
                quad[1] = fast_atoi(c) - 1;
                skip_until_whitespace(c); skip_whitespace(c);
                quad[2] = fast_atoi(c) - 1;
                skip_until_whitespace(c); skip_whitespace(c);
                quad[3] = fast_atoi(c) - 1;
                if (quad[0] < 0 || quad[1] < 0 || quad[2] < 0 || quad[3] < 0) {
                    std::cerr << "negative face index found, ignoring";
                }
                else {
                    quads.push_back(quad);
                }
            }
        }
    }

    RowMatX3d verts = Eigen::Map<RowMatX3d>((double*)verts_vec.data(), verts_vec.size(), 3);
    RowMatXi faces;
    if (faceDim == 4) {
        faces = Eigen::Map<RowMatXi>((int*)quads.data(), quads.size(), 4);
    }
    else if (faceDim == 3) {
        faces = Eigen::Map<RowMatXi>((int*)tris.data(), tris.size(), 3);
    }
    return std::make_pair(verts, faces);
}

PYBIND11_MODULE(_fastobj_ext, m) {
    m.def("load_obj_fast", &loadOBJFast);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto sorted = sort_by(ordering(sizeof_), list(
        type<char[3]>,
        type<char[1]>,
        type<char[2]>,
        type<char[15]>
    ));
    BOOST_HANA_CONSTANT_ASSERT(sorted == list(
        type<char[1]>,
        type<char[2]>,
        type<char[3]>,
        type<char[15]>
    ));
    //! [main]
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP
#define CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP

#include <vector>

#include <boost/math/tools/polynomial.hpp>

#include <nil/crypto3/fft/domains/evaluation_domain.hpp>
#include <nil/crypto3/fft/domains/arithmetic_sequence_domain.hpp>
#include <nil/crypto3/fft/domains/basic_radix2_domain.hpp>
#include <nil/crypto3/fft/domains/extended_radix2_domain.hpp>
#include <nil/crypto3/fft/domains/geometric_sequence_domain.hpp>
#include <nil/crypto3/fft/domains/step_radix2_domain.hpp>

#include <nil/crypto3/fft/detail/field_utils.hpp>

namespace nil {
    namespace crypto3 {
        namespace fft {

            /*!
            @brief
             A convenience method for choosing an evaluation domain
             Returns an evaluation domain object in which the domain S has size
             |S| >= MinSize.
             The function get_evaluation_domain is chosen from different supported domains,
             depending on MinSize.
            */

            namespace detail {
                using namespace nil::crypto3::algebra;

                template<typename FieldType>
                bool is_basic_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));

                    return (m > 1) && (log_m <= fields::arithmetic_params<FieldType>::s) && (m == (1ul << log_m));
                }

                template<typename FieldType>
                bool is_extended_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));
                    const std::size_t small_m = m / 2;
                    const std::size_t log_small_m = static_cast<std::size_t>(std::ceil(std::log2(small_m)));

                    return (m > 1) && (log_m == fields::arithmetic_params<FieldType>::s + 1) &&
                           (small_m == (1ul << log_small_m)) &&
                           (log_small_m <= fields::arithmetic_params<FieldType>::s);
                }

                template<typename FieldType>
                bool is_step_radix2_domain(std::size_t m) {
                    const std::size_t log_m = static_cast<std::size_t>(std::ceil(std::log2(m)));
                    const std::size_t shift_log_m = (1ul << log_m);
                    const std::size_t log_shift_log_m = static_cast<std::size_t>(std::ceil(std::log2(shift_log_m)));
                    const std::size_t small_m = m - (1ul << (static_cast<std::size_t>(std::ceil(std::log2(m))) - 1));
                    const std::size_t log_small_m = static_cast<std::size_t>(std::ceil(std::log2(small_m)));

                    return (m > 1) && (small_m == (1ul << log_small_m)) && (shift_log_m == (1ul << log_shift_log_m)) &&
                           (log_shift_log_m <= fields::arithmetic_params<FieldType>::s);
                }

                template<typename FieldType>
                bool is_geometric_sequence_domain(std::size_t m) {
                    return (m > 1) &&
                           (typename FieldType::value_type(fields::arithmetic_params<FieldType>::geometric_generator) !=
                            FieldType::value_type::zero());
                }

                template<typename FieldType>
                bool is_arithmetic_sequence_domain(std::size_t m) {
                    return (m > 1) && (typename FieldType::value_type(
                                           fields::arithmetic_params<FieldType>::arithmetic_generator) !=
                                       FieldType::value_type::zero());
                }

            }    // namespace detail

            template<typename FieldType>
            std::shared_ptr<evaluation_domain<FieldType>> make_evaluation_domain(std::size_t m) {
                typedef std::shared_ptr<evaluation_domain<FieldType>> ret_type;

                const std::size_t big = 1ul << (std::size_t(std::ceil(std::log2(m))) - 1);
                const std::size_t rounded_small = (1ul << std::size_t(std::ceil(std::log2(m - big))));

                if (detail::is_basic_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new basic_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_extended_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new extended_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_step_radix2_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new step_radix2_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_basic_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new basic_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_extended_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new extended_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_step_radix2_domain<FieldType>(big + rounded_small)) {
                    ret_type result;
                    result.reset(new step_radix2_domain<FieldType>(big + rounded_small));
                    return result;
                }

                if (detail::is_geometric_sequence_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new geometric_sequence_domain<FieldType>(m));
                    return result;
                }

                if (detail::is_arithmetic_sequence_domain<FieldType>(m)) {
                    ret_type result;
                    result.reset(new arithmetic_sequence_domain<FieldType>(m));
                    return result;
                }

                return ret_type();
            }
        }    // namespace fft
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ALGEBRA_FFT_MAKE_EVALUATION_DOMAIN_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2003 Neil Firth
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/instruments/barrieroption.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/pricingengines/barrier/analyticbarrierengine.hpp>
#include <ql/exercise.hpp>
#include <boost/scoped_ptr.hpp>

namespace QuantLib {

    BarrierOption::BarrierOption(
        Barrier::Type barrierType,
        Real barrier,
        Real rebate,
        const ext::shared_ptr<StrikedTypePayoff>& payoff,
        const ext::shared_ptr<Exercise>& exercise)
    : OneAssetOption(payoff, exercise),
      barrierType_(barrierType), barrier_(barrier), rebate_(rebate) {}

    void BarrierOption::setupArguments(PricingEngine::arguments* args) const {

        OneAssetOption::setupArguments(args);

        auto* moreArgs = dynamic_cast<BarrierOption::arguments*>(args);
        QL_REQUIRE(moreArgs != nullptr, "wrong argument type");
        moreArgs->barrierType = barrierType_;
        moreArgs->barrier = barrier_;
        moreArgs->rebate = rebate_;
    }

    Volatility BarrierOption::impliedVolatility(
             Real targetValue,
             const ext::shared_ptr<GeneralizedBlackScholesProcess>& process,
             Real accuracy,
             Size maxEvaluations,
             Volatility minVol,
             Volatility maxVol) const {

        QL_REQUIRE(!isExpired(), "option expired");

        ext::shared_ptr<SimpleQuote> volQuote(new SimpleQuote);

        ext::shared_ptr<GeneralizedBlackScholesProcess> newProcess =
            detail::ImpliedVolatilityHelper::clone(process, volQuote);

        // engines are built-in for the time being
        boost::scoped_ptr<PricingEngine> engine;
        switch (exercise_->type()) {
          case Exercise::European:
            engine.reset(new AnalyticBarrierEngine(newProcess));
            break;
          case Exercise::American:
          case Exercise::Bermudan:
            QL_FAIL("engine not available for non-European barrier option");
            break;
          default:
            QL_FAIL("unknown exercise type");
        }

        return detail::ImpliedVolatilityHelper::calculate(*this,
                                                          *engine,
                                                          *volQuote,
                                                          targetValue,
                                                          accuracy,
                                                          maxEvaluations,
                                                          minVol, maxVol);
    }

    BarrierOption::arguments::arguments()
    : barrierType(Barrier::Type(-1)), barrier(Null<Real>()),
      rebate(Null<Real>()) {}

    void BarrierOption::arguments::validate() const {
        OneAssetOption::arguments::validate();

        switch (barrierType) {
          case Barrier::DownIn:
          case Barrier::UpIn:
          case Barrier::DownOut:
          case Barrier::UpOut:
            break;
          default:
            QL_FAIL("unknown type");
        }

        QL_REQUIRE(barrier != Null<Real>(), "no barrier given");
        QL_REQUIRE(rebate != Null<Real>(), "no rebate given");
    }

    bool BarrierOption::engine::triggered(Real underlying) const {
        switch (arguments_.barrierType) {
          case Barrier::DownIn:
          case Barrier::DownOut:
            return underlying < arguments_.barrier;
          case Barrier::UpIn:
          case Barrier::UpOut:
            return underlying > arguments_.barrier;
          default:
            QL_FAIL("unknown type");
        }
    }

}


#include "util/rectangle.hpp"
#include "util/typedefs.hpp"

#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(rectangle_test)

using namespace osrm;
using namespace osrm::util;

// Verify that the bearing-bounds checking function behaves as expected
BOOST_AUTO_TEST_CASE(get_min_dist_test)
{
    //           ^
    //           |
    //           +- 80
    //           |
    //           |
    //           +- 10
    //           |
    //--|-----|--+--|-----|-->
    // -100  -10 |  10    100
    //           +- -10
    //           |
    //           |
    //           +- -80
    //           |
    RectangleInt2D ne{
        FloatLongitude{10}, FloatLongitude{100}, FloatLatitude{10}, FloatLatitude{80}};
    RectangleInt2D nw{
        FloatLongitude{-100}, FloatLongitude{-10}, FloatLatitude{10}, FloatLatitude{80}};
    RectangleInt2D se{
        FloatLongitude{10}, FloatLongitude{100}, FloatLatitude{-80}, FloatLatitude{-10}};
    RectangleInt2D sw{
        FloatLongitude{-100}, FloatLongitude{-10}, FloatLatitude{-80}, FloatLatitude{-10}};

    Coordinate nw_sw{FloatLongitude{-100.1}, FloatLatitude{9.9}};
    Coordinate nw_se{FloatLongitude{-9.9}, FloatLatitude{9.9}};
    Coordinate nw_ne{FloatLongitude{-9.9}, FloatLatitude{80.1}};
    Coordinate nw_nw{FloatLongitude{-100.1}, FloatLatitude{80.1}};
    Coordinate nw_s{FloatLongitude{-55}, FloatLatitude{9.9}};
    Coordinate nw_e{FloatLongitude{-9.9}, FloatLatitude{45.0}};
    Coordinate nw_w{FloatLongitude{-100.1}, FloatLatitude{45.0}};
    Coordinate nw_n{FloatLongitude{-55}, FloatLatitude{80.1}};
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_sw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_se), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_ne), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_nw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_s), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_e), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_w), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        nw.GetMinSquaredDist(nw_n), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);

    Coordinate ne_sw{FloatLongitude{9.9}, FloatLatitude{9.9}};
    Coordinate ne_se{FloatLongitude{100.1}, FloatLatitude{9.9}};
    Coordinate ne_ne{FloatLongitude{100.1}, FloatLatitude{80.1}};
    Coordinate ne_nw{FloatLongitude{9.9}, FloatLatitude{80.1}};
    Coordinate ne_s{FloatLongitude{55}, FloatLatitude{9.9}};
    Coordinate ne_e{FloatLongitude{100.1}, FloatLatitude{45.0}};
    Coordinate ne_w{FloatLongitude{9.9}, FloatLatitude{45.0}};
    Coordinate ne_n{FloatLongitude{55}, FloatLatitude{80.1}};
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_sw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_se), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_ne), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_nw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_s), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_e), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_w), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        ne.GetMinSquaredDist(ne_n), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);

    Coordinate se_ne{FloatLongitude{100.1}, FloatLatitude{-9.9}};
    Coordinate se_nw{FloatLongitude{9.9}, FloatLatitude{-9.9}};
    Coordinate se_sw{FloatLongitude{9.9}, FloatLatitude{-80.1}};
    Coordinate se_se{FloatLongitude{100.1}, FloatLatitude{-80.1}};
    Coordinate se_n{FloatLongitude{55}, FloatLatitude{-9.9}};
    Coordinate se_w{FloatLongitude{9.9}, FloatLatitude{-45.0}};
    Coordinate se_e{FloatLongitude{100.1}, FloatLatitude{-45.0}};
    Coordinate se_s{FloatLongitude{55}, FloatLatitude{-80.1}};
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_sw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_se), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_ne), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_nw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_s), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_e), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_w), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        se.GetMinSquaredDist(se_n), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);

    Coordinate sw_ne{FloatLongitude{-9.9}, FloatLatitude{-9.9}};
    Coordinate sw_nw{FloatLongitude{-100.1}, FloatLatitude{-9.9}};
    Coordinate sw_sw{FloatLongitude{-100.1}, FloatLatitude{-80.1}};
    Coordinate sw_se{FloatLongitude{-9.9}, FloatLatitude{-80.1}};
    Coordinate sw_n{FloatLongitude{-55}, FloatLatitude{-9.9}};
    Coordinate sw_w{FloatLongitude{-100.1}, FloatLatitude{-45.0}};
    Coordinate sw_e{FloatLongitude{-9.9}, FloatLatitude{-45.0}};
    Coordinate sw_s{FloatLongitude{-55}, FloatLatitude{-80.1}};
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_sw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_se), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_ne), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_nw), 0.02 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_s), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_e), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_w), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
    BOOST_CHECK_CLOSE(
        sw.GetMinSquaredDist(sw_n), 0.01 * COORDINATE_PRECISION * COORDINATE_PRECISION, 0.1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <sys/time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

#include "ros/ros.h"
#include "bosch_arm.h"
#include <kdl/frames.hpp>
#include "motor_controller.h"
#include <bosch_arm_control/ArmState.h>
#include <bosch_arm_control/PointCmd.h>
#include <bosch_arm_control/Diagnostic.h>
#include "cc2.h"
#include <Eigen/Core>
#include<Eigen/Array>
#include <Eigen/LU>
#include <Eigen/QR>

/** This controller mainly focus on tuning motor 4 for high stiffness and damping.
  * It contains the LTISys tool, interface for filter tuning, motor sine signal generation,
  * position and torque reference management, switching between open loop and closed loop,
  * variant stiffness control.
  * related files: motor_controller.cpp,motor_controller.h,main_motor.cpp
 */

using namespace std;
void TrajectoryController::singlePtCmdCallBack(const bosch_arm_control::PointCmd& msg)
{
  MoveAbsAction* a=new MoveAbsAction(msg.coordinates,this,msg.duration);
  act_que.push_back(a);
}

TrajectoryController::TrajectoryController(BoschArm *ptr)
{
  rob=ptr;
  diagnostic_pub =  n.advertise<bosch_arm_control::Diagnostic> ("/diagnostics",100);
  sigle_point_cmd_sub = n.subscribe("/joint_point_cmd",3, &TrajectoryController::singlePtCmdCallBack, this);
  joint_state_pub =  n.advertise<bosch_arm_control::ArmState> ("/joint_states",100);
  js.position.resize(4);
  js.velocity.resize(4);
  js.effort.resize(4);
  Kqp[0]=20;
  Kqp[1]=20;
  Kqp[2]=10;
  Kqp[3]=20;

  Kqv[0]=0.01;
  Kqv[1]=0.01;
  Kqv[2]=0.01;
  Kqv[3]=0.01;

  joint_limit_low[0]=-1.7;
  joint_limit_high[0]=2.1;
  joint_limit_low[1]=-0.6;
  joint_limit_high[1]=0.55;
  joint_limit_low[2]=-2.5;
  joint_limit_high[2]=2.3;
  joint_limit_low[3]=-2.3;
  joint_limit_high[3]=2.4;

  qtmax[0]=constants::t_max/constants::r1;
  qtmax[1]=constants::t_max/constants::r2;
  qtmax[2]=constants::t_max/constants::r2;
  qtmax[3]=constants::t_max/constants::r2;

  Kmp[0]=0.8;
  Kmp[1]=0.8;
  Kmp[2]=0.8;
  Kmp[3]=0.8;

  Kmv[0]=0.006;
  Kmv[1]=0.006;
  Kmv[2]=0.006;
  Kmv[3]=0.006;

  mtmax[0]=constants::t_max;
  mtmax[1]=constants::t_max;
  mtmax[2]=constants::t_max;
  mtmax[3]=constants::t_max;

  qvtime[0]=3000;
  qvtime[1]=1500;
  qvtime[2]=800;
  qvtime[3]=800;

  mvtime[0]=3000;
  mvtime[1]=1500;
  mvtime[2]=800;
  mvtime[3]=800;

  qvstep[0]=0.01;
  qvstep[1]=0.01;
  qvstep[2]=0.01;
  qvstep[3]=0.01;

  mvstep[0]=0.01;
  mvstep[1]=0.01;
  mvstep[2]=0.01;
  mvstep[3]=0.01;

  chirp_step=1;

  do_qvstep=false;
  do_mvstep=false;
  do_chirp=false;
  do_msin=false;
  do_qsin=false;
  close_loop=false;
  ///resonance compensation y1=1/4,f1=8,y2=1/8,f2=40
//   filter_order=2;
//   double bb[]={4.134265039735645e-02,    -7.771105812573594e-02,     3.886211445065686e-02};
//   double aa[]={1.000000000000000e+00,    -1.972700933810727e+00,     9.751946405330041e-01};
///resonance compensation y1=1/2,f1=8,y2=1/8,f2=40, lead compensation f3=1, f4=10
//   filter_order=3;
//   double bb[]={3.971672532347112e-01,    -1.141227944523378e+00,     1.115209779749842e+00,-3.709990377268638e-01};
//   double aa[]={1.000000000000000e+00,    -2.887615836095562e+00,     2.780830044410648e+00,-8.930641575807744e-01};
//   a=new double[filter_order+1];
//   b=new double[filter_order+1];
//   for (int i=0;i<filter_order+1;i++)
//   {
//     a[i]=aa[i];
//     b[i]=bb[i];
//   }
//   for (int i=0;i<4;i++)
//   {
//     tx[i]=new double[filter_order];
//     ty[i]=new double[filter_order];
//   }
//   for (int j=0;j<4;j++)
//     for (int i=0;i<filter_order;i++)
//     {
//       tx[j][i]=tzero[j];
//       ty[j][i]=tzero[j];
//     }
  qtvstep=0;
  mtvstep=0;
  max_torque=0.05;
  clear_ref();

  for(int i=0;i<4;i++)
    filter[i]=new LTISys(1);

  flt_delay=new LTISys(1);

  fpara[0]=25;
  fpara[1]=0.2;
  fpara[2]=55;
  fpara[3]=0.2;
  fpara[4]=50;
  fpara[5]=0.3;
  Kmp[3]=6;

  updateFilter();
//   double w1=25*2*M_PI;
//   double w2=55*2*M_PI;
//   double zeta1=1.0/4;
//   double zeta2=1.0/8;
//   double rescpa[]={1,2*zeta1/w1,1/w1/w1};
//   double rescpb[]={1,2*zeta2/w2,1/w2/w2};
//   LTISys rescp(rescpb,2,rescpa,2);
//   LTISys rescpd=rescp.tustin(0.001);

//   double w1=4*2*M_PI;
//   double w2=50*2*M_PI;
//   double sb[]={1,1/w1};
//   double sa[]={1,1/w2};
//   LTISys sys(sb,1,sa,1);
//   sys=sys.tustin(0.001);
//   for(int i=0;i<4;i++)
//     filter[i]=new LTISys(sys);
//   cout<<sys.b[1]<<','<<sys.b[0]<<endl;
//   cout<<sys.a[1]<<','<<sys.a[0]<<endl;
  //cout<<rescpd<<endl;
//  LTISys *ptrpp=&rescpd;

}

// void TrajectoryController::updateFilter()
// {
//   double w1=fpara[0]*2*M_PI;
//   double zeta1=fpara[1];
//   double w2=fpara[2]*2*M_PI;
//   double zeta2=fpara[3];
//   double w3=fpara[4]*fpara[5]*2*M_PI;
//   double w4=fpara[4]/fpara[5]*2*M_PI;
//   double sa1[]={1,2*zeta1/w1,1/w1/w1};
//   double sb1[]={1,2*zeta2/w2,1/w2/w2};
//   double wcgain=sqrt((1+1/fpara[5]/fpara[5])/(1+fpara[5]*fpara[5]));
//   //cout<<' '<<wcgain<<' '<<endl;
//   double sa2[]={1,1/w4};
//   double sb2[]={1/wcgain,1/w3/wcgain};
//   double td=2.3e-3;
//   double ts=1e-3;
//   double sb3[]={1,td/2};
//   double sa3[]={1,-td/2};
//   LTISys sys3(sb3,1,sa3,1);
//   LTISys sys_delay=sys3.tustin(ts);
//   flt_delay=new LTISys(sys_delay);
//   flt_delay->initialize(m[3]);
//
//   LTISys sys1(sb1,2,sa1,2);
//   LTISys sys2(sb2,1,sa2,1);
// //   double t1[]={1};
// //   LTISys sys3(t1,0,sa2,1);
//   LTISys sys=sys1*sys2;
//   sys=sys.tustin(ts);
//   if(filter[3]->order_b>0)
//     sys.initialize(filter[3]->x[0]);
//   LTISys *tmp=filter[3];
//
//   filter[3]=new LTISys(sys);
//
//   delete tmp;
//   //cout<<sys<<endl;
// }

void TrajectoryController::updateFilter()
{
  double wc=fpara[4]*2*M_PI;
  double ts=1.0/constants::cycle_hz;
  double alpha=1.0/fpara[5];
  double wleadl=wc/alpha;
  double wleadh=wc*alpha;

  double wlagh=wc/10;
  double wlagl=wlagh/alpha;

  double sleadb[]={1,1/wleadl};
  double sleada[]={1,1/wleadh};
  double slagb[]={1,1/wlagh};
  double slaga[]={1,1/wlagl};
  double glag= sqrt((wc/wlagh*wc/wlagh+1)/(wc/wlagl*wc/wlagl+1));
  double glead=sqrt((wc/wleadl*wc/wleadl+1)/(wc/wleadh*wc/wleadh+1));
  LTISys lead(sleadb,1,sleada,1,1/glead);
  LTISys lag(slagb,1,slaga,1,1/glag);
  LTISys leadlag(lead*lag);

  //resonance compensation
  double w1=fpara[0]*2*M_PI;
  double zeta1=fpara[1];
  double w2=fpara[2]*2*M_PI;
  double zeta2=fpara[3];
  double sa1[]={1,2*zeta1/w1,1/w1/w1};
  double sb1[]={1,2*zeta2/w2,1/w2/w2};
  LTISys reso(sb1,2,sa1,2);
  LTISys overall(reso*leadlag);

  LTISys *tmp=filter[3];
  filter[3]=new LTISys(overall.tustin(ts));
  if(tmp->order_b>0)
    filter[3]->initialize(tmp->x[0]);
  delete tmp;

  //delay compensation for input.
  double td=2.3e-3;
  double sdelayb[]={1,td};
  double sdelaya[]={1,td/10};
  LTISys delay(sdelayb,1,sdelaya,1);
  tmp=flt_delay;
  flt_delay=new LTISys(delay.tustin(ts));
  if(tmp->order_b>0)
    flt_delay->initialize(tmp->x[0]);
  cout<<*flt_delay;
  delete tmp;

}

void TrajectoryController::set_position_ref()
{
  for (int i=0;i<4;i++)
  {
    mzero[i]=m[i];
    qzero[i]=q[i];

  }
}
void TrajectoryController::set_gc()
{
  for (int i=0;i<4;i++)
  {
    tzero[i]=mt[i];
  }
}
void TrajectoryController::clear_ref()
{
  for (int i=0;i<4;i++)
  {
    mzero[i]=0;
    qzero[i]=0;
    tzero[i]=0;
  }
}

//calculate the frames given the joint values.
void TrajectoryController::updateKinematics()
{
  frm_cur[0]=frm_home[0];
  frm0[0]=frm_home[0];
  for (int i=1;i<=4;i++)
  {
    frm_cur[i].p=frm_home[i].p;
    frm_cur[i].M=frm_home[i].M*Rotation::RotZ(q[i-1]);
    frm0[i]=frm0[i-1]*frm_cur[i];
  }
  frm_cur[5]=frm_home[5];
  frm0[5]=frm0[4]*frm_cur[5];
  frmn[4]=frm_cur[5];
  Vector z(0,0,1);

  //jacobian[3](joint 4) = z4*p54
  for (int i=3;i>=0;i--)
  {
    frmn[i]=frm_cur[i+1]*frmn[i+1];
    jacobian[i]=z*frmn[i+1].p;
    jacobian[i]=frm0[i+1].M*jacobian[i];
  }
}

void TrajectoryController::start()
{
  rob->initialize();
  //L0,L3 are not available until rob is initialized.
  frm_home[0]=Frame::Identity();
  Rotation r(0,-1,0,
             0,0,-1,
             1,0,0);
  frm_home[1].M=r;
  frm_home[1].p=Vector(0,0,rob->L0);
  frm_home[2].M=r;
  frm_home[3].M=r;
  frm_home[4].M=r;
  frm_home[4].p=Vector(-rob->L5,0,rob->L3);
  frm_home[5].p=Vector(rob->L4,0,0);

  rob->motor2JointPosition(rob->q,q);

  for (int i=0;i<4;i++)
  {
    qv[i]=0;
    dqv[i]=0;
    dq[i]=0;
    qt[i]=0;
    qd[i]=q[i];
    qvd[i]=qv[i];

    m[i]=rob->q[i];
    mv[i]=0;
    md[i]=m[i];
    mvd[i]=mv[i];
    dm[i]=0;
    dmv[i]=0;
    mt[i]=0;
  }
  for (int i=0;i<4;i++)
    gc[i]=Vector(constants::gravc[i*2],constants::gravc[i*2+1],0);
  //state=CALIBRATION;
  //state=LOADTRACE;
  state=SERVO;
  //ctr_mode=GRAVITY;
  ctr_mode=NOCONTROL;
  //ctr_mode=PDWITHGC;
  if (state==CALIBRATION)
  {
    genCalibTraj2();
    cur_act=act_que.front();
    cur_act->initialize();
    act_que.pop_front();
  }
  else if (state==LOADTRACE)
  {

    vector<double> des(4,0);
//     for(int i=0;i<4;i++)
//       des[i]=q[i];
//     des[1]=joint_limit_low[1];
//     des[3]=joint_limit_low[3]+0.1;
//     act_que.push_back(new MoveAbsAction(des,this));
//     des[1]=joint_limit_high[1];
    act_que.push_back(new MoveAbsAction(des,this));
    cur_act=act_que.front();
    cur_act->initialize();
    act_que.pop_front();

    //state=SERVO;

  }
}

void TrajectoryController::genCalibTraj2()
{
  vector<double> des(4,0);
//  int t=8000;
  //move the the zero position.
  act_que.push_back(new MoveAbsAction(des,this));
  des[0]=-1.5;
  des[1]=-0.5;
  des[2]=-2.2;
  des[3]=-1.7;
  cali1_act=new MoveAbsAction(des,this);
  calib1=new Calibration("calib1");
  cali1_act->calib=calib1;
  act_que.push_back(cali1_act);
  des[0]=0.7;
  des[1]=0;
  des[2]=0;
  des[3]=0;
  act_que.push_back(new MoveAbsAction(des,this));
  des[0]=-0.8;
  des[1]=-0.5;
  des[2]=-2.2;
  des[3]=-1.7;
  cali2_act=new MoveAbsAction(des,this);
  calib2=new Calibration("calib2");
  cali2_act->calib=calib2;
  act_que.push_back(cali2_act);

}

void TrajectoryController::calcGravityCompensation()
{
  for (int j=0;j<calib1->len_b;j++)
    if (calib1->count[j]>0&&calib2->count[j]>0)
    {
      calib1->b[j]-=calib2->b[j];
      calib1->count[j]=1;
    }

  for (int i=0;i<8;i++)
  {
    for (int j=0;j<calib1->len_b;j++)
    {
      if (calib1->count[j]>0)
      {
        calib1->A[i][j]-=calib2->A[i][j];
      }
    }
  }

  Eigen::MatrixXf AA(8,8);
  for (int i=0;i<8;i++)
  {
    for (int j=0;j<8;j++)
    {
      double s=0;
      for (int k=0;k<calib1->len_b;k++)
        if (calib1->count[k]>0)
          s+=calib1->A[i][k]*calib1->A[j][k];
      AA(i,j)=s/calib1->len_b;
    }
  }
  //cout<<AA;
  Eigen::VectorXf Ab(8);
  for (int i=0;i<8;i++)
  {
    double s=0;
    for (int j=0;j<calib1->len_b;j++)
      if (calib1->count[j]>0)
        s+=calib1->A[i][j]*calib1->b[j];
    Ab(i)=-s/calib1->len_b;
  }
  Eigen::MatrixXf M=AA.inverse()*Ab;
  //cout<<M<<endl;
  for (int i=0;i<4;i++)
  {
    gc[i]=Vector(M(i*2,0),M(i*2+1,0),0);
  }
  for (int i=0;i<4;i++)
  {
    for (int j=0;j<3;j++)
      cout<<gc[i][j]<<',';
    cout<<endl;
  }
}

// void TrajectoryController::vstep(int joint, double step)
// {
//   vector<double> des(4,0);
//   for (int i=0;i<4;i++)
//     des[i]=qd[i];
//   des[joint]+=step;
//   cur_act=new MoveAbsAction(des,this);
//   cur_act->initialize();
//
// }

void TrajectoryController::qstep(int joint, double step)
{

//   for (int i=0;i<4;i++)
//     qd[i]=q[i];
  qd[joint]+=step;

}

void TrajectoryController::mstep(int joint, double step)
{

//   for (int i=0;i<4;i++)
//     md[i]=m[i];
  md[joint]+=step;

}

void TrajectoryController::update()
{
  rob->update();
  for (int i=0;i<4;i++)
  {
    m[i]=rob->q[i];
    mv[i]=rob->v[i];
  }
  clock_gettime(CLOCK_REALTIME, &ts);
  //save the last joint position
//   for (int i=0;i<4;i++)
//     ql[i]=q[i];
  rob->motor2JointPosition(rob->q,q);
  rob->motor2JointVelocity(rob->v,qv);

  if (state==CALIBRATION)
  {
    //update desired position
    if (cur_act->finished)
    {
      if (cur_act->calib==NULL)
        delete cur_act;

      if (act_que.size()==0)
      {
        calcGravityCompensation();
        state=SERVO;
        return;
      }
      cur_act=act_que.front();
      //cout<<cur_act->rel[3]<<endl;
      cur_act->initialize();
      act_que.pop_front();
    }
    cur_act->update();
    for (int i=0;i<4;i++)
    {
      qd[i]=cur_act->cmd.coordinates[i];
      qvd[i]=cur_act->cmd.velocity[i];

    }
    //update torque command
    qd_Kq_gc_m2jT();

    logging();
  }
  else if (state==LOADTRACE)
  {
    if (cur_act->finished)
    {
      if (cur_act->calib==NULL)
        delete cur_act;

      if (act_que.size()==0)
      {
        //calcGravityCompensation();
        state=SERVO;
        return;
      }

      cur_act=act_que.front();
      //cout<<cur_act->rel[3]<<endl;
      cur_act->initialize();
      act_que.pop_front();

    }
    cur_act->update();
    for (int i=0;i<4;i++)
    {
      qd[i]=cur_act->cmd.coordinates[i];
      qvd[i]=cur_act->cmd.velocity[i];

    }
    //update torque command
    qd_Kq_gc_m2jT();
    //PDControl();
    logging();
  }
  else if (state==SERVO)
  {
    if (cur_act!=NULL)
    {
      if (cur_act->finished)
      {
        if (cur_act->calib==NULL)
        {
          delete cur_act;
          cur_act=NULL;
        }

        if (act_que.size()!=0)
        {
          cur_act=act_que.front();
          cur_act->initialize();
          act_que.pop_front();
        }
        else
          cur_act=NULL;

      }
      else
      {
        cur_act->update();
        for (int i=0;i<4;i++)
        {
          qd[i]=cur_act->cmd.coordinates[i];
          qvd[i]=cur_act->cmd.velocity[i];

        }
      }
    }
    else if (act_que.size()!=0)
    {
      cur_act=act_que.front();
      cur_act->initialize();
      act_que.pop_front();
    }
    else if (do_qvstep)
      do_qVStep();
    else if (do_mvstep)
      do_mVStep();
    else if (do_qsin)
      do_qSin();
//     else if (do_msin)
//       do_mSin();
    else if (do_chirp)
      do_motorSine();
      //do_ChirpSine();
      //do_stretch();

    if (ctr_mode==QD_KQ_M2JT)
      qd_Kq_m2jT();
    else if (ctr_mode==QD_KQ_GC_M2JT)
      qd_Kq_gc_m2jT();
    else if (ctr_mode==QD_J2M_KM)
      qd_j2m_Km();
    else if (ctr_mode==GC_M2JT)
      gc_m2jT();
    else if (ctr_mode==MD_KM)
      md_VarKm();
    else if (ctr_mode==NOCONTROL)
    {
      for (int i=0;i<4;i++)
        rob->torque[i]=tzero[i];
    }
    else if (ctr_mode==SYSID)
    {
      for (int i=0;i<4;i++)
        rob->torque[i]=mt[i];
    }

    logging();
  }
}

void TrajectoryController::do_stretch()
{
  for (int i=0;i<4;i++)
    mt[i]=tzero[i];

  if (mtvstep<mvtime[jid])
  {
    mt[jid]-=mtvstep*0.15/5000-0.02;
    mtvstep++;
    if (mtvstep%100==0)
      cout<<mt[jid]<<endl;
  }
}

void TrajectoryController::do_ChirpSine()
{

  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=chirp_step;
  for (int i=0;i<4;i++)
  {
    mt[i]=tzero[i];
    md[i]=mzero[i];
  }
  mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
  mtvstep++;
//     if(mtvstep%1000==0)
//       cout<<freq<<"Hz"<<endl;

}

void TrajectoryController::do_motorSine()
{
  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=chirp_step;
  for (int i=0;i<4;i++)
  {
    mt[i]=tzero[i];
    md[i]=mzero[i];
    dmv[i]=-mv[i];
  }

  md[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);

//   if(mtvstep%500<250)
//     md[jid]+=mvstep[jid];
//   else
//     md[jid]-=mvstep[jid];
//   if(close_loop)
//   {
//     dm[jid]=md[jid]-m[jid];
//     dq[jid]=filter[jid]->filter(dm[jid]);
//     double dmt=Kmp[jid]*dq[jid];
//     if(fabs(dmt)<0.06)
//       mt[jid]+=Kmp[jid]*dq[jid];
//     else if(dmt>=0.06)
//       mt[jid]+=0.06;
//     else
//       mt[jid]-=0.06;
//   }
//   if(close_loop)
//   {
//     dm[jid]=md[jid]-m[jid];
//     double dmt=Kmp[jid]*dm[jid];
//     if(dmt>0.06)
//       dm[jid]=0.06/Kmp[jid];
//     else if(dmt<-0.06)
//       dm[jid]=-0.06/Kmp[jid];
//
//     dq[jid]=filter[jid]->filter(dm[jid]);
//     mt[jid]+=Kmp[jid]*dq[jid];
//   }
  //Kmp[3]=2;
  if(mtvstep==0)
    flt_delay->initialize(m[jid]);
  if(close_loop)
  {
    qd[jid]=flt_delay->filter(m[jid]);
    //double tmp=dm[i];
    dm[jid]=md[jid]-qd[jid];
    //dmv[i]=0-mv[i];
    dq[jid]=filter[jid]->filter(dm[jid]);

    double r=Kmp[jid]*dq[jid]/max_torque;
    if (fabs(r)<=1)
      mt[jid]+=Kmp[jid]*dq[jid];
//     else if(fabs(r)<=20)
//       mt[jid]+=Kmp[jid]*dm[jid]/20+Kmv[jid]*dmv[jid];
    else if (r>1)
      mt[jid]=max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];
    else
      mt[jid]=-max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];

    //mt[jid]+=Kmp[jid]*dq[jid];
    //cout<<m[jid]<<','<<qd[jid]<<','<<dm[jid]<<','<<dq[jid]<<','<<mt[jid]<<endl;
  }
  else
  {

    dm[jid]=md[jid]-mzero[jid];
    //dq[jid]=filter[jid]->filter(dm[jid]);
    //qd[jid]=flt_delay->filter(m[jid]);
    dq[jid]=dm[jid];
    mt[jid]+=Kmp[jid]*dq[jid];
  }

  //mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
  mtvstep++;
}

// void TrajectoryController::do_mSin()
// {
//   double t=double(mtvstep)/double(constants::cycle_hz);
//   double freq=chirp_step;
//   for (int i=0;i<4;i++)
//   {
//    md[i]=mzero[i];
//    mt[i]=tzero[i];
//  }
//   md[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);

  //double tmp=dm[jid];

  //dmv[jid]=(dm[jid]-tmp)/rob->dt;
  //dq[jid]=filter[jid]->filter(dm[jid]);
  //dq[jid]=dm[jid];
  //mt[jid]+=Kmp[jid]*dm[jid];
  //mt[jid]+=dm[jid];

//   mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
//   mtvstep++;
//   if (mtvstep==2*mvtime[jid])
//         mtvstep=0;
//   double tmp=mzero[jid]+mvstep[jid]*sin(double(mtvstep)/(double)mvtime[jid]*M_PI);
//   mvd[jid]=tmp-md[jid];
//   md[jid]=tmp;
//   mtvstep++;
//}

void TrajectoryController::do_qSin()
{

  if (qtvstep==2*qvtime[jid])
    qtvstep=0;
  double tmp=qzero[jid]+qvstep[jid]*sin(double(qtvstep)/double(qvtime[jid])*M_PI);
  qvd[jid]=tmp-qd[jid];
  qd[jid]=tmp;
  qtvstep++;

}

void TrajectoryController::do_Chirp()
{

//   updateKinematics();
//   Vector g(0,0,-1);
//   //double tq[4];
//   for (int i=0;i<4;i++)
//   {
//     Vector Zi=frm0[i+1].M.UnitZ();
//     qt[i]=0;
//     for (int j=i;j<4;j++)
//     {
//       Rotation R0j=frm0[j+1].M.Inverse();
//       Vector tmp=R0j*(g*Zi);
//       qt[i]-=dot(gc[j],tmp);
//     }
//   }
//
//   for (int i=0;i<4;i++)
//   {
//     mt[i]=0;
//     for (int j=0;j<4;j++)
//     {
//       mt[i]+=rob->m2j[4*j+i]*qt[j];
//
//     }
//
//   }
//   if (mtvstep==0)
//     for (int j=0;j<4;j++)
//       for (int i=0;i<filter_order;i++)
//       {
//         tx[j][i]=tzero[j];
//         ty[j][i]=tzero[j];
//       }
  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=exp(chirp_step*t*5);
  for (int i=0;i<4;i++)
    mt[i]=tzero[i];
  if (mtvstep<mvtime[jid])
  {
    mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
    mtvstep++;
    if (mtvstep%1000==0)
      cout<<freq<<"Hz"<<endl;
  }
//   for(int i=0;i<4;i++)
//     qt[i]=mt[i];
//   double tmp[4];
//   for(int i=2;i<=2;i++)
//   {
//     tmp[i]=b[0]*mt[i];
//     for(int j=0;j<filter_order;j++)
//       tmp[i]+=b[j+1]*tx[i][j];
//     for(int j=0;j<filter_order;j++)
//       tmp[i]-=a[j+1]*ty[i][j];
//     for(int j=filter_order-1;j>0;j--)
//       tx[i][j]=tx[i][j-1];
//     tx[i][0]=mt[i];
//     for(int j=filter_order-1;j>0;j--)
//       ty[i][j]=ty[i][j-1];
//     ty[i][0]=tmp[i];
//     //q[i]=tmp[i];
//     mt[i]=ty[i][0];
//   }

//   if(mtvstep>mvtime[jid])
//     do_chirp=false;
}

void TrajectoryController::do_qVStep()
{
  if (qtvstep==2*qvtime[jid])
    qtvstep=0;
  if (qtvstep<qvtime[jid]/2)
  {
    qd[jid]+=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=qvstep[jid]*2/qvtime[jid];
  }
  else if (qtvstep<qvtime[jid]/2+qvtime[jid])
  {
    qd[jid]-=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=-qvstep[jid]*2/qvtime[jid];
  }
  else
  {
    qd[jid]+=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=qvstep[jid]*2/qvtime[jid];
  }
  qtvstep++;
}

void TrajectoryController::do_mVStep()
{
  if (mtvstep==2*mvtime[jid])
    mtvstep=0;
  if (mtvstep<mvtime[jid]/2)
  {
    md[jid]+=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=mvstep[jid]*2/mvtime[jid];
  }
  else if (mtvstep<mvtime[jid]/2+mvtime[jid])
  {
    md[jid]-=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=-mvstep[jid]*2/mvtime[jid];
  }
  else
  {
    md[jid]+=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=mvstep[jid]*2/mvtime[jid];
  }
  mtvstep++;
}

void TrajectoryController::gc_m2jT()
{

  updateKinematics();
  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    qt[i]=0;
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      qt[i]-=dot(gc[j],tmp);
    }
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();

}

void TrajectoryController::qd_Kq_gc_m2jT()
{
  updateKinematics();
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    dqv[i]=qvd[i]-qv[i];
    qt[i]=Kqp[i]*dq[i]+Kqv[i]*dqv[i];
  }

  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      qt[i]-=dot(gc[j],tmp);
    }
  }
  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::md_VarKm()
{
  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    double r=Kmp[i]*dm[i]/max_torque;
    if (fabs(r)<=1)
      mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
    else if (r>1)
      mt[i]=max_torque+Kmv[i]/(1+log(fabs(r)))*dmv[i];
    else
      mt[i]=-max_torque+Kmv[i]/(1+log(fabs(r)))*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
}

void TrajectoryController::md_Km_VarKm_Comp()
{

  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    if (fabs(Kmp[i]*dm[i])<=max_torque)
      mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
    else if (Kmp[i]*dm[i]>max_torque)
      mt[i]=max_torque+Kmv[i]*dmv[i];
    else
      mt[i]=-max_torque+Kmv[i]*dmv[i];
  }

  for (int i=2;i<=2;i++)
  {
    if (fabs(Kmp[i]*dm[i])<=max_torque)
      mt[i]=Kmp[i]*dm[i];
    else if (Kmp[i]*dm[i]>max_torque)
      mt[i]=max_torque;
    else
      mt[i]=-max_torque;
  }

//   double tmp[4];
//   for (int i=2;i<=2;i++)
//   {
//     tmp[i]=b[0]*mt[i];
//     for (int j=0;j<filter_order;j++)
//       tmp[i]+=b[j+1]*tx[i][j];
//     for (int j=0;j<filter_order;j++)
//       tmp[i]-=a[j+1]*ty[i][j];
//     for (int j=filter_order-1;j>0;j--)
//       tx[i][j]=tx[i][j-1];
//     tx[i][0]=mt[i];
//     for (int j=filter_order-1;j>0;j--)
//       ty[i][j]=ty[i][j-1];
//     ty[i][0]=tmp[i];
//     //q[i]=tmp[i];
//     mt[i]=ty[i][0];
//   }

  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];

}

void TrajectoryController::md_Km()
{
  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::qd_j2m_Km()
{
  for (int i=0;i<4;i++)
  {
    dq[i]=qd[i]-q[i];
    dqv[i]=qvd[i]-qv[i];
    //dv[i]=-v[i];
  }

  //transform errors into motor space

  for (int i=0;i<4;i++)
  {
    dm[i]=0;
    dmv[i]=0;
    for (int j=0;j<4;j++)
    {
      dm[i]+=dq[j]*rob->j2m[4*i+j];
      dmv[i]+=dqv[j]*rob->j2m[4*i+j];
    }
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::qd_Kq_m2jT()
{
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    qt[i]=Kqp[i]*dq[i]-Kqv[i]*(qv[i]-qvd[i]);
  }
  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::logging()
{
  std::string log;
  std::stringstream out;
  for (int i=0;i<4;i++)
    out<<m[i]-mzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<md[i]-mzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<mv[i]<<",";
  for (int i=0;i<4;i++)
    out<<mt[i]-tzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<dm[i]<<",";
  for (int i=0;i<4;i++)
    out<<q[i]-qzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<qd[i]-mzero[i]<<",";
  for (int i=0;i<3;i++)
    out<<qv[i]<<",";
  out<<rob->dt<<',';
  for (int i=0;i<4;i++)
    out<<qt[i]<<",";
  for (int i=0;i<4;i++)
    out<<dq[i]<<",";

  out << ts.tv_sec << ','<< ts.tv_nsec;
  log = out.str();
  diag.data=log;
  diag.header.stamp.sec=ts.tv_sec;
  diag.header.stamp.nsec=ts.tv_nsec;
  diagnostic_pub.publish(diag);
  //vector<double> cur_pos_act(q,q+4);
  js.header.stamp.sec=ts.tv_sec;
  js.header.stamp.nsec=ts.tv_nsec;
  //js.position=cur_pos_act;

  for (int i=0;i<4;i++)
  {
    js.position[i]=q[i];
    js.velocity[i]=qv[i];
    js.effort[i]=qt[i];
  }
  joint_state_pub.publish(js);
}

// void TrajectoryController::logging()
// {
//   std::string log;
//   std::stringstream out;
//   for (int i=0;i<4;i++)
//     out<<q[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<qd[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<v[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<f[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<dq[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<rob->q[i]<<',';
//   for (int i=0;i<4;i++)
//     out<<rob->v[i]<<',';
//   for (int i=0;i<4;i++)
//     out<<rob->torque[i]<<',';
//   out << ts.tv_sec << ','<< ts.tv_nsec;
//   log = out.str();
//   diag.data=log;
//   diag.header.stamp.sec=ts.tv_sec;
//   diag.header.stamp.nsec=ts.tv_nsec;
//   diagnostic_pub.publish(diag);
//   //vector<double> cur_pos_act(q,q+4);
//   js.header.stamp.sec=ts.tv_sec;
//   js.header.stamp.nsec=ts.tv_nsec;
//   //js.position=cur_pos_act;
//
//   for (int i=0;i<4;i++)
//   {
//     js.position[i]=q[i];
//     js.velocity[i]=v[i];
//     js.effort[i]=f[i];
//   }
//   joint_state_pub.publish(js);
// }
void TrajectoryController::stop()
{
  rob->close();
}

MoveAbsAction::MoveAbsAction(std::vector<double> despos,TrajectoryController* ptr,int tt)
{
  for (int i=0;i<4;i++)
    des[i]=despos[i];
  //std::cout<<rel[3]<<"---"<<std::endl;
  t_end=tt;
  //calibration=false;
  //name="";
  ctr=ptr;
  calib=NULL;
}

MoveAbsAction::MoveAbsAction(std::vector<double> despos,TrajectoryController* ptr)
{
  for (int i=0;i<4;i++)
    des[i]=despos[i];
  //std::cout<<rel[3]<<"---"<<std::endl;
  t_end=-1;
  //calibration=false;
  //name="";
  ctr=ptr;
  calib=NULL;
}

void MoveAbsAction::initialize()
{
  finished=false;
  t=0;
  cmd.coordinates.resize(4);
  cmd.velocity.resize(4);
  cmd.acceleration.resize(4);
  cmd.header.frame_id="joint_space";
  double *pos=ctr->qd;
  for (int i=0;i<4;i++)
  {
    init_cmd_pos[i]=pos[i];
    rel[i]=des[i]-pos[i];
    cmd.coordinates[i]=pos[i];
    cmd.velocity[i]=0;
    cmd.acceleration[i]=0;
  }
  if (t_end<0)
  {
    double relabs=0;
    int imax;
    for (int i=0;i<4;i++)
      if (fabs(rel[i])>relabs)
      {
        relabs=fabs(rel[i]);
        imax=i;
      }
    t_end=floor(relabs/M_PI*20*constants::cycle_hz);
    if (calib!=NULL)
      t_end*=2;
  }
  if (t_end<2*constants::cycle_hz/10)
  {
    cout<<"duration less than 200!\n"<<endl;
    t_end=2*constants::cycle_hz/10;
  }
  t_dec=t_end-constants::cycle_hz/100;
  t_acc=constants::cycle_hz/10;
  t_lin=t_dec-t_acc;
  for (int i=0;i<4;i++)
  {
    double v_max=rel[i]/(t_lin);
    acc[i]=v_max/t_acc;
  }
  if (calib!=NULL)
    calib->initialize(ctr,this);
}

void MoveAbsAction::update()
{
  t++;
  //std::cout<<t<<std::endl;
  if (t>t_end)
  {
    finished=true;
    if (calib!=NULL)
      calib->finish();
    return;
  }

  for (int i=0;i<4;i++)
  {
    if (t<=t_acc)
    {
      cmd.velocity[i]+=acc[i];
      cmd.coordinates[i]+=cmd.velocity[i];
      cmd.acceleration[i]=acc[i];
    }
    else if (t<=t_lin)
    {
      cmd.velocity[i]=t_acc*acc[i];
      cmd.coordinates[i]+=cmd.velocity[i];
      cmd.acceleration[i]=0;
    }
    else if (t<=t_dec)
    {
      cmd.velocity[i]-=acc[i];
      cmd.coordinates[i]+=cmd.velocity[i];
      cmd.acceleration[i]=-acc[i];
    }
    else if (t<=t_end)
    {
      cmd.coordinates[i]=des[i];
      cmd.velocity[i]=0;
      cmd.acceleration[i]=0;
    }

  }
  if (calib!=NULL)
    calib->update();
}

int Calibration::q2bin(double *q)
{
  double dist=0;
  for (int i=0;i<4;i++)
    dist+=(q[i]-traj->init_cmd_pos[i])*(q[i]-traj->init_cmd_pos[i]);
  return round(sqrt(dist)/0.01)+10;
}

void Calibration::bin2q(int bin,double* q)
{
  for (int i=0;i<4;i++)
    q[i]=traj->init_cmd_pos[i]+bin_step[i]*(bin-10);
  return;
}

void Calibration::initialize(TrajectoryController* cptr,MoveAbsAction* mptr)
{
  //the parameter to be calibrated has size 8

//put the b into its bins. b has size 4
//the b in the same bin has the same left side coefficients.

//there are n non-empty bins, so there are n*4 equations.
//the coefficients can be pre-computed.

  //compute the number of bins.
  ctr=cptr;
  traj=mptr;
  double dist=0;
  for (int i=0;i<4;i++)
  {
    dist+=traj->rel[i]*traj->rel[i];
  }
  int n=round(sqrt(dist)/0.01);
  n_bin=n+20;
  len_b=n_bin*4;
  b=new double[len_b];
  count=new int[len_b];
  cout<<"initialing"<<endl;
  for (int i=0;i<len_b;i++)
  {
    b[i]=0;
    count[i]=0;
  }
  for (int i=0;i<4;i++)
    bin_step[i]=mptr->rel[i]/n;

  for (int i=0;i<8;i++)
  {
    A[i]=new double[len_b];
  }
  double q[4];
  Frame frm0[6];
  Frame frm_cur;
  Vector g(0,0,-1);
  Vector Zi;
  for (int j=0;j<n_bin;j++)
  {
    bin2q(j,q);
    //Ti=sum>i Mj dot (g*Zi)
    frm0[0]=cptr->frm_home[0];
    for (int i=1;i<=4;i++)
    {
      frm_cur.p=cptr->frm_home[i].p;
      frm_cur.M=cptr->frm_home[i].M*Rotation::RotZ(q[i-1]);
      frm0[i]=frm0[i-1]*frm_cur;
    }

    //each bin contains 4 equations for 4 joints
    //each eqation contains 8 coefficients for 4 joints.
    //first equation:
    //A[0-2]=R02*(g*Z1),A[3-5]=R03*(g*Z1), A[6-8]=R04*(g*Z1)
    //second
    //A[0-2]=0,...
    for (int i=0;i<4;i++)
    {
      Zi=frm0[i+1].M.UnitZ();
      int n_eq=j*4+i;
      for (int k=0;k<i;k++)
      {
        for (int c=0;c<2;c++)
          A[k*2+c][n_eq]=0;
      }
      for (int k=i;k<4;k++)
      {
        Rotation R0k=frm0[k+1].M.Inverse();
        Vector tmp=R0k*(g*Zi);
        for (int c=0;c<2;c++)
          A[k*2+c][n_eq]=tmp[c];
      }
    }

  }

}

void Calibration::loadTrace()
{
//   string line;
//   string str=name.append(".txt");
//   ifstream myfile(str.c_str());
//   for (int i=0;i<4;i++)
//   {
//     f[i]=new double[629];
//     count[i]=new double[629];
//   }
//   if (myfile.is_open())
//   {
//     int j=0;
//     cout<<"load "<<str<<endl;
//     char buf[200];
//     while (myfile.good())
//     {
//       getline(myfile,line);
//       char* pch;
//       sprintf(buf,line.c_str());
//       pch=strtok(buf,",");
//       int i=0;
//       while (pch!=NULL)
//       {
//         f[i][j]=atof(pch);
//         if (f[i][j]!=0)
//           count[i][j]=1;
//         else
//           count[i][j]=0;
//         //cout<<f[i][j]<<",";
//         pch=strtok(NULL,",");
//         i++;
//       }
//       //cout<<endl;
//       j++;
//     }
//     myfile.close();
//   }
//
//   else cout << "Unable to open file";

}

void Calibration::update()
{
  int bin=q2bin(ctr->q);
  for (int i=0;i<4;i++)
  {
    count[bin*4+i]++;
    b[bin*4+i]+=ctr->qt[i];
  }

}

void Calibration::finish()
{
  for (int j=0;j<4;j++)
  {
    for (int i=0;i<n_bin;i++)
    {
      int id=i*4+j;
      if (count[id]==0)
        continue;
      b[id]=b[id]/count[id];
    }
  }
  ofstream out;
  string str=name;
  str.append(".txt");
  out.open(str.c_str());
  for (int i=0;i<len_b;i++)
  {
    for (int j=0;j<8;j++)
      out<<A[j][i]<<',';
    out<<b[i]<<endl;
  }
  out.close();
  //calculate M (A'A)^-1 (A'b)

}

LTISys::LTISys(double c)
{
  a=new double[1];
  b=new double[1];
  x=new double[0];
  y=new double[0];
  order_a=0;
  order_b=0;
  a[0]=1;
  b[0]=c;
}

// LTISys::LTISys(const double* tb, int ob)
// {
//   int ib=ob;
//   while (ib>=0&&tb[ib]==0)
//     ib--;
//
//   if (ib==-1)
//   {
//     a=new double[1];
//     b=new double[1];
//     x=new double[0];
//     y=new double[0];
//     order_a=0;
//     order_b=0;
//     a[0]=1;
//     b[0]=0;
//   }
//   else
//   {
//     order_b=ib;
//     order_a=0;
//     b=new double[ib+1];
//     a=new double[1];
//     x=new double[ib];
//     y=new double[0];
//     for (int i=0;i<=ib;i++)
//       b[i]=tb[i];
//     for(int i=0;i<ib;i++)
//       x[i]=0;
//     a[0]=1;
//   }
// }

LTISys::LTISys(const double* tb, int ob)
{
  int ib=ob;
  while (ib>=0&&tb[ib]==0)
    ib--;
  //numerator is zero
  if (ib==-1)
  {
    order_b=0;
    b=new double[1];
    x=0;
    b[0]=0;
  }
  else
  {
    order_b=ib;
    b=new double[ib+1];
    x=new double[ib];
    for (int i=0;i<=ib;i++)
      b[i]=tb[i];
    for(int i=0;i<ib;i++)
      x[i]=0;
  }
  order_a=0;
  a=new double[1];
  y=new double[0];
  a[0]=1;
}

LTISys::LTISys(const double* tb, int ob, const double* ta, int oa, double gain)
{

  int ia=oa;
  while (ia>=0&&ta[ia]==0)
    ia--;
  //denumerator is zero
  if (ia==-1)
  {
    order_a=0;
    a=new double[1];
    y=0;
    a[0]=0;
  }
  else
  {
    order_a=ia;
    a=new double[ia+1];
    y=new double[ia];
    for (int i=0;i<=ia;i++)
      a[i]=ta[i];
    for(int i=0;i<ia;i++)
      y[i]=0;
  }

  int ib=ob;
  while (ib>=0&&tb[ib]==0)
    ib--;
  //numerator is zero
  if (ib==-1)
  {
    order_b=0;
    b=new double[1];
    x=0;
    b[0]=0;
  }
  else
  {
    order_b=ib;
    b=new double[ib+1];
    x=new double[ib];
    for (int i=0;i<=ib;i++)
      b[i]=tb[i];
    for(int i=0;i<ib;i++)
      x[i]=0;
  }
  for(int i=0;i<=ib;i++)
    b[i]*=gain;
}

LTISys::LTISys(const LTISys &rhs)
{
  order_a=rhs.order_a;
  order_b=rhs.order_b;
  a=new double[order_a+1];
  b=new double[order_b+1];
  x=new double[order_b];
  y=new double[order_a];
  for (int i=0;i<=order_a;i++)
    a[i]=rhs.a[i];
  for (int i=0;i<=order_b;i++)
    b[i]=rhs.b[i];
  for(int i=0;i<order_a;i++)
    y[i]=0;
  for(int i=0;i<order_b;i++)
    x[i]=0;
}

LTISys & LTISys::operator=(const LTISys &rhs)
{
  if (this==&rhs)
    return *this;

  order_a=rhs.order_a;
  order_b=rhs.order_b;
  delete[] a;
  delete[] b;
  delete[] x;
  delete[] y;
  a=new double[order_a+1];
  b=new double[order_b+1];

  if(order_a==0)
    y=0;
  else
    y=new double[order_a];

  if(order_b==0)
    x=0;
  else
    x=new double[order_b];

  for (int i=0;i<=order_a;i++)
    a[i]=rhs.a[i];
  for (int i=0;i<=order_b;i++)
    b[i]=rhs.b[i];
  for(int i=0;i<order_a;i++)
    y[i]=rhs.y[i];
  for(int i=0;i<order_b;i++)
    x[i]=rhs.x[i];
  return *this;
}

const LTISys LTISys::num() const
{
  return LTISys(b,order_b);
}

const LTISys LTISys::den() const
{
  return LTISys(a,order_a);
}

const LTISys LTISys::operator*(const LTISys &other) const
{
  int oa=order_a+other.order_a;
  int ob=order_b+other.order_b;
  double *ta=new double[oa+1];
  double *tb=new double[ob+1];
  for (int i=0;i<oa+1;i++)
  {
    ta[i]=0;
    for (int j=0;j<=i&&j<=order_a;j++)
    {
      if ((i-j)<=other.order_a)
        ta[i]+=a[j]*other.a[i-j];
    }
    //cout<<result.a[i]<<endl;
  }
  for (int i=0;i<ob+1;i++)
  {
    tb[i]=0;
    for (int j=0;j<=i&&j<=order_b;j++)
    {
      if ((i-j)<=other.order_b)
        tb[i]+=b[j]*other.b[i-j];
    }
  }
  LTISys result(tb,ob,ta,oa);
  delete[] ta;
  delete[] tb;
  return result;
}

// const LTISys LTISys::operator*(const LTISys &other) const
// {
//   LTISys result(1);
//   result.order_a=order_a+other.order_a;
//   //cout<<order_a<<' '<<other.order_a<<' '<<result.order_a<<endl;
//   result.order_b=order_b+other.order_b;
//   delete[] result.a;
//   delete[] result.b;
//   result.a=new double[result.order_a+1];
//   result.b=new double[result.order_b+1];
//   for (int i=0;i<result.order_a+1;i++)
//   {
//     result.a[i]=0;
//     for (int j=0;j<=i&&j<=order_a;j++)
//     {
//       if ((i-j)<=other.order_a)
//         result.a[i]+=a[j]*other.a[i-j];
//     }
//     //cout<<result.a[i]<<endl;
//   }
//   for (int i=0;i<result.order_b+1;i++)
//   {
//     result.b[i]=0;
//     for (int j=0;j<=i&&j<=order_b;j++)
//     {
//       if ((i-j)<=other.order_b)
//         result.b[i]+=b[j]*other.b[i-j];
//     }
//   }
//   return result;
// }

const LTISys LTISys::inv() const
{
  return LTISys(a,order_a,b,order_b);
}

const LTISys LTISys::operator/(const LTISys &other) const
{
  LTISys result=(*this)*other.inv();
  return result;
}

const LTISys LTISys::operator+(const LTISys &other) const
{
  LTISys ra=this->den()*other.den();
  LTISys rb1=this->num()*other.den();
  LTISys rb2=this->den()*other.num();

  int oa,ob;
  oa=ra.order_b;
  ob=(rb1.order_b>rb2.order_b)?rb1.order_b:rb2.order_b;

  double *ta=new double[oa+1];
  double *tb=new double[ob+1];

  for (int i=0;i<ob+1;i++)
  {
    tb[i]=0;
    if(i<=rb1.order_b)
      tb[i]+=rb1.b[i];
    if(i<=rb2.order_b)
      tb[i]+=rb2.b[i];
  }
  for (int i=0;i<oa+1;i++)
    ta[i]=ra.b[i];

  LTISys result(tb,ob,ta,oa);
  delete[] ta;
  delete[] tb;
  return result;
}

const LTISys LTISys::operator-() const
{
  LTISys result=*this;
  for (int i=0;i<order_b;i++)
    result.b[i]=-result.b[i];
  return result;
}

const LTISys LTISys::operator-(const LTISys &other) const
{
  return (*this)+(-other);
}

ostream& operator<<(ostream &os,const LTISys &obj)
  {
    for(int i=obj.order_b;i>=0;i--)
      os<<obj.b[i]<<',';
    os<<endl;
    for(int i=obj.order_a;i>=0;i--)
      os<<obj.a[i]<<',';
    return os;
  }
const LTISys LTISys::tustin(double ts) const
{
  double ssa[]={ts/2,ts/2};
  double ssb[]={-1,1};
  LTISys s(ssb,1,ssa,1);
  LTISys sa(ssa,1);
  LTISys sb(ssb,1);

  LTISys ra=LTISys(a[order_a]);
  for (int i=order_a-1;i>=0;i--)
    ra=s*ra+LTISys(a[i]);
  LTISys rb=LTISys(b[order_b]);
  for (int i=order_b-1;i>=0;i--)
    rb=s*rb+LTISys(b[i]);

  int n=order_a-order_b;
  if (n>0)
    for (int i=0;i<n;i++)
      rb=rb*sa;
  else
    for (int i=0;i<-n;i++)
      ra=ra*sa;
  LTISys r=rb.num()/ra.num();
  //LTISys *dbgr=&r;
  double c=r.a[r.order_a];
  for(int i=0;i<=r.order_a;i++)
    r.a[i]/=c;
  for(int i=0;i<=r.order_b;i++)
    r.b[i]/=c;
  return r;
}

void LTISys::initialize(double x0)
{
  for(int i=0;i<order_b;i++)
    x[i]=x0;
  double sa=0;
  double sb=0;
  for(int i=0;i<=order_a;i++)
    sa+=a[i];
  for(int i=0;i<=order_b;i++)
    sb+=b[i];
  double dc_gain=sb/sa;
  for(int i=0;i<order_a;i++)
    y[i]=x0*dc_gain;
}

double LTISys::filter(double xn)
{
  double yn=xn*b[order_b];
  //cout<<xn<<','<<b[1]<<',';
  for(int i=order_b-1;i>=0;i--)
    yn+=x[order_b-1-i]*b[i];
  //cout<<x[0]<<','<<b[0]<<',';
  for(int i=order_a-1;i>=0;i--)
    yn-=y[order_b-1-i]*a[i];
  yn/=a[order_b];
  //cout<<y[0]<<','<<a[0]<<','<<yn<<endl;
  for(int i=order_b-1;i>0;i--)
    x[i]=x[i-1];
  x[0]=xn;
  for(int i=order_a-1;i>0;i--)
    y[i]=y[i-1];
  y[0]=yn;

  return yn;
}


// Comment if using older version of Pololu IMUv9.
#include "vector.h"
#include "MinIMU9_API.h"
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <system_error>
#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include "ros/ros.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"

#define LOOP_RATE 10

namespace opts = boost::program_options;

// TODO: print warning if accelerometer magnitude is not close to 1 when starting up

// An Euler angle could take 8 chars: -234.678, but usually we only need 6.
float field_width = 6;
#define FLOAT_FORMAT std::fixed << std::setprecision(3) << std::setw(field_width)

std::ostream & operator << (std::ostream & os, const vector & vector)
{
    return os << FLOAT_FORMAT << vector(0) << ' '
              << FLOAT_FORMAT << vector(1) << ' '
              << FLOAT_FORMAT << vector(2);
}

std::ostream & operator << (std::ostream & os, const matrix & matrix)
{
    return os << (vector)matrix.row(0) << ' '
              << (vector)matrix.row(1) << ' '
              << (vector)matrix.row(2);
}

std::ostream & operator << (std::ostream & os, const quaternion & quat)
{
    return os << FLOAT_FORMAT << quat.w() << ' '
              << FLOAT_FORMAT << quat.x() << ' '
              << FLOAT_FORMAT << quat.y() << ' '
              << FLOAT_FORMAT << quat.z();
}

typedef void rotation_output_function(quaternion& rotation);

void output_quaternion(quaternion & rotation)
{
    std::cout << rotation;
}

void output_matrix(quaternion & rotation)
{
    std::cout << rotation.toRotationMatrix();
}

void output_euler(quaternion & rotation)
{
    std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) * (180 / M_PI));
}

int millis()
{
/*    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;*/
    return ros::Time::now().toSec();
}
/*
void streamRawValues(IMU& imu)
{
    imu.enable();
    while(1)
    {
        imu.read();
        printf("%7d %7d %7d  %7d %7d %7d  %7d %7d %7d\n",
               imu.raw_m[0], imu.raw_m[1], imu.raw_m[2],
               imu.raw_a[0], imu.raw_a[1], imu.raw_a[2],
               imu.raw_g[0], imu.raw_g[1], imu.raw_g[2]
        );
        usleep(20*1000);
    }
}
*/

//! Uses the acceleration and magnetic field readings from the compass
// to get a noisy estimate of the current rotation matrix.
// This function is where we define the coordinate system we are using
// for the ground coords:  North, East, Down.
matrix rotationFromCompass(const vector& acceleration, const vector& magnetic_field)
{
    vector down = -acceleration;     // usually true
    vector east = down.cross(magnetic_field); // actually it's magnetic east
    vector north = east.cross(down);

    east.normalize();
    north.normalize();
    down.normalize();

    matrix r;
    r.row(0) = north;
    r.row(1) = east;
    r.row(2) = down;
    return r;
}

typedef void fuse_function(quaternion& rotation, float dt, const vector& angular_velocity,
                  const vector& acceleration, const vector& magnetic_field);

void fuse_compass_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    // Implicit conversion of rotation matrix to quaternion.
    rotation = rotationFromCompass(acceleration, magnetic_field);
}

// Uses the given angular velocity and time interval to calculate
// a rotation and applies that rotation to the given quaternion.
// w is angular velocity in radians per second.
// dt is the time.
void rotate(quaternion& rotation, const vector& w, float dt)
{
    // Multiply by first order approximation of the
    // quaternion representing this rotation.
    rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2);
    rotation.normalize();
}

void fuse_gyro_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    rotate(rotation, angular_velocity, dt);
}

void fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    vector correction = vector(0, 0, 0);

    if (abs(acceleration.norm() - 1) <= 0.3)
    {
        // The magnetidude of acceleration is close to 1 g, so
        // it might be pointing up and we can do drift correction.

        const float correction_strength = 1;

        matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
        matrix rotationMatrix = rotation.toRotationMatrix();

        correction = (
            rotationCompass.row(0).cross(rotationMatrix.row(0)) +
            rotationCompass.row(1).cross(rotationMatrix.row(1)) +
            rotationCompass.row(2).cross(rotationMatrix.row(2))
          ) * correction_strength;

    }

    rotate(rotation, angular_velocity + correction, dt);
}

void ahrs(std::string bag_path, fuse_function * fuse, rotation_output_function * output)
{
    rosbag::Bag bag;
    double start; // truncate 64-bit return value
    bag.open(bag_path.c_str(), rosbag::bagmode::Read);
    std::vector<container> gyro_readings_for_offset;
    std::cout<< "OPENED" ;

    std::vector<std::string> topics;
    topics.push_back(std::string("/dn1/imu_chatter"));

    rosbag::View view(bag, rosbag::TopicQuery(topics));
    int message_index = 0;
    foreach(rosbag::MessageInstance const msg, view)
    {
      sensor_msgs::Imu::ConstPtr reading = msg.instantiate<sensor_msgs::Imu>();

      if (reading != NULL)
      {
        if (message_index == 0)
          start = reading->header.stamp.toSec();
        else if (message_index == 32)
          break;
        message_index++;
        container r;
        r.reading[0] = reading->angular_velocity.x;
        r.reading[1] = reading->angular_velocity.y;
        r.reading[2] = reading->angular_velocity.z;
        gyro_readings_for_offset.push_back(r);
      }
    }
    bag.close();

    loadCalibration();
    measureOffsets(gyro_readings_for_offset);

    // The quaternion that can convert a vector in body coordinates
    // to ground coordinates when it its changed to a matrix.
    quaternion rotation = quaternion::Identity();

    bag.open(bag_path.c_str(), rosbag::bagmode::Read);
    rosbag::View view2(bag, rosbag::TopicQuery(topics));

    foreach(rosbag::MessageInstance const msg, view2)
    {
        sensor_msgs::Imu::ConstPtr reading = msg.instantiate<sensor_msgs::Imu>();
        if (reading != NULL)
        {
            double last_start = start;
            start = reading->header.stamp.toSec();
            float dt = (start-last_start);
            if (dt < 0){ throw std::runtime_error("Time went backwards."); }
            container g;
            g.reading[0] = reading->angular_velocity.x;
            g.reading[1] = reading->angular_velocity.y;
            g.reading[2] = reading->angular_velocity.z;

            vector angular_velocity = readGyro(g);

            container a;
            a.reading[0] = reading->linear_acceleration.x;
            a.reading[1] = reading->linear_acceleration.y;
            a.reading[2] = reading->linear_acceleration.z;
            vector acceleration = readAcc(a);

            container m;
            a.reading[0] = reading->orientation.x;
            a.reading[1] = reading->orientation.y;
            a.reading[2] = reading->orientation.z;
            vector magnetic_field = readMag(m);

            fuse(rotation, dt, angular_velocity, acceleration, magnetic_field);

            output(rotation);
            std::cout << "  " << acceleration << "  " << magnetic_field << std::endl << std::flush;

            // Ensure that each iteration of the loop takes at least 20 ms.
            /*while(millis() - start < 20)
            {
                usleep(1000);
            }*/
            usleep(1000);
        }
    }
}

int main(int argc, char *argv[])
{
    try
    {
        // Define what all the command-line parameters are.
        std::string mode, output_mode, i2cDevice, bag_path;
        opts::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "produce help message")
            ("version,v", "print version number")
            ("i2c-bus,b", opts::value<std::string>(&i2cDevice)->default_value("/dev/i2c-0"),
             "i2c-bus the IMU is connected to")
            ("mode", opts::value<std::string>(&mode)->default_value("normal"),
             "specifies what algorithm to use.\n"
             "normal: Fuse compass and gyro.\n"
             "gyro-only:  Use only gyro (drifts).\n"
             "compass-only:  Use only compass (noisy).\n"
             "raw: Just print raw values from sensors.")
            ("output", opts::value<std::string>(&output_mode)->default_value("matrix"),
             "specifies how to output the orientation.\n"
             "matrix: Direction Cosine Matrix.\n"
             "quaternion: Quaternion.\n"
             "euler: Euler angles (yaw, pitch, roll).\n")
            ("bag", opts::value<std::string>(&bag_path)->default_value("test.bag"),
             "specifies the bag file from where to read.\n")
            ;
        opts::variables_map options;
        opts::store(opts::command_line_parser(argc, argv).options(desc).run(), options);
        opts::notify(options);

        if(options.count("help"))
        {
            std::cout << desc << std::endl;
            std::cout << "For more information, run: man minimu9-ahrs" << std::endl;
            return 0;
        }

        if (options.count("version"))
        {
            //std::cout << VERSION << std::endl;
            return 0;
        }

        //MinIMU9 imu(i2cDevice.c_str());

        rotation_output_function * output;

        // Figure out the output mode.
        if (output_mode == "matrix")
        {
            output = &output_matrix;
        }
        else if (output_mode == "quaternion")
        {
            output = &output_quaternion;
        }
        else if (output_mode == "euler")
        {
            field_width += 2;  // See comment above for field_width.
            output = &output_euler;
        }
        else
        {
            std::cerr << "Unknown output mode '" << output_mode << "'" << std::endl;
            return 1;
        }

        ros::init(argc, argv, "minimu9_ahrs");

        // Figure out the basic operating mode and start running.
        if (mode == "raw")
        {
            //streamRawValues(imu);
        }
        else if (mode == "gyro-only")
        {
            ahrs(bag_path, &fuse_gyro_only, output);
        }
        else if (mode == "compass-only")
        {
            ahrs(bag_path, &fuse_compass_only, output);
        }
        else if (mode == "normal")
        {
            ahrs(bag_path, &fuse_default, output);
        }
        else
        {
            std::cerr << "Unknown mode '" << mode << "'" << std::endl;
            return 1;
        }
        return 0;
    }
    catch(const std::system_error & error)
    {
        std::string what = error.what();
        const std::error_code & code = error.code();
        std::cerr << "Error: " << what << "  " << code.message() << " (" << code << ")" << std::endl;
        return 2;
    }
    catch(const opts::multiple_occurrences & error)
    {
        std::cerr << "Error: " << error.what() << " of " << error.get_option_name() << " option." << std::endl;
        return 1;
    }
    catch(const std::exception & error)
    {
        std::cerr << "Error: " << error.what() << std::endl;
        return 9;
    }
}


#include "multiprecision_reflection.h"

#include <bond/core/bond.h>
#include <bond/stream/output_buffer.h>

#include <boost/multiprecision/miller_rabin.hpp>

using namespace examples::multiprecision;

int main()
{
    using namespace boost::random;

    mt11213b gen1(clock());
    mt19937 gen2(clock());
    independent_bits_engine<mt11213b, 512, uint512> gen512(gen1);
    independent_bits_engine<mt11213b, 1024, uint1024> gen1024(gen1);

    Example obj, obj2;

    // Generate some large random primes
    // Note that the literature recommends 25 trials of Miller-Rabin for a good
    // likelihood that a number is prime but this is just a silly little
    // example and we want it to run relatively fast so doing just one.
    while (!miller_rabin_test(obj.public_key, 1, gen2))
        obj.public_key = gen512();

    while (!miller_rabin_test(obj.private_key, 1, gen2))
        obj.private_key = gen1024();

    // Arbitrary precision math
    obj.product = obj.public_key * obj.private_key;

    // Serialize the object
    bond::OutputBuffer output;
    bond::CompactBinaryWriter<bond::OutputBuffer> writer(output);
    Serialize(obj, writer);

    // De-serialize the object
    bond::CompactBinaryReader<bond::InputBuffer> reader(output.GetBuffer());
    Deserialize(reader, obj2);

    BOOST_ASSERT(obj2 == obj);

    return 0;
}