#ifndef _TREEGRAPH_HPP
#define _TREEGRAPH_HPP  1
#include <iostream>
#include <queue>
#include <map>
#include <boost/bimap.hpp>
#include <boost/utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/visitors.hpp>
#include "ztree.hpp"
#include "ptree.hpp"

#define VERBOSE   0
using namespace std;
using namespace boost;
using namespace tree;

struct max_value {
  unsigned int max;
  max_value( ) : max( 0 ) { }
  void operator() ( unsigned int x ) { if ( x > max ) max = x; }
};

// la classe T indica il tipo di albero, la classe E il tipo di etichetta da usare per gli
// archi e per la numerazione dei vertici
template <class T = ztree, typename E = unsigned int>
class treegraph {
  // definisco un grafo che utilizza un vector per le liste di adiacenza e
  // un vector per la lista dei nodi
  typedef adjacency_list<vecS, vecS, undirectedS> graph_type;
  // definisco il tipo dell'arco
  typedef pair<E, E> edge_type;
  // definisco il tipo dell'array usato per il calcolo delle distanze
  typedef graph_traits<graph_type>::vertices_size_type dist_type;

private:
  E n;
  std::queue<T> q;
  bimap<T, E> m;
  graph_type g;
  dist_type* dist;

public:
  treegraph ( E num ) : n( num ), m( ), dist( NULL ) {
    build( );
    dist = new dist_type [num_vertices( g )];
  }

  ~treegraph ( ) {
    if ( dist ) delete[] dist;
  }

  E num_trees ( ) {
    return num_vertices( g );
  }

  // costruzione del grafo
  void build ( ) {
    // prendo un albero casuale come albero 0.. che generalita' ragazzi.. ^^
    T zero( n );
    q.push( zero );
    set<edge_type> edges;
    m.insert( typename bimaps::bimap<T,E>::value_type( zero, m.size() ) );

    while ( !q.empty() ) {
      T t = q.front();
      q.pop();

      E tindex = m.left.find( t )->second;

      // genero tutti i possibili alberi a partire da quello selezionato
      for ( typename T::node_type i = 1; i <= n; i++ ) {
        T s = t;
        // per non perdere di generalita' utilizzo l'operatore ^, implementato
        // sia in ptree che in ztree
        s ^ i;

        if ( s == t ) continue;

        // se non c'e' lo aggiungo
        if ( m.left.find( s ) == m.left.end() ) {
          m.insert( typename bimaps::bimap<T,E>::value_type( s, m.size() ) );
          q.push( s );
        }

        // prendo l'indice dell'albero s
        E index = m.left.find( s )->second;

        // aggiungo l'arco fra t e s
        if ( VERBOSE ) cout << "Adding edge " << index << ", " << tindex << endl;

        if ( index <= tindex )
          edges.insert( edge_type( index, tindex ) );
        else   edges.insert( edge_type( tindex, index ) );
      }
    }

    g = graph_type( edges.begin(), edges.end(), m.size() );

    //if ( VERBOSE )
    cout << "Rotation graph generated. There are " << m.size() << " different trees." << endl;
  }

  void full_search ( ) {
    cout << "Search path lenghts on all trees: 0%\r";
    cout.flush( );

    E max = 0;
    int perc = 0;

    for ( E i = 0; i < num_vertices( g ); i++ ) {
      std::fill_n( dist, num_vertices( g ), 0 );

      breadth_first_search( g, vertex( i, g ),
        visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );

      max_value result = for_each( dist, dist + num_vertices( g ), max_value() );
      if ( VERBOSE )
        cout << "Max lenght for paths starting from tree " << i << " is " << result.max << ".\n";

      if ( result.max > max ) max = result.max;

      // stampa una percentuale per avere un idea del punto a cui sono...
      if ( int( i * 100.0f / num_vertices( g ) ) >= int( perc + 1 ) ) {
        perc++;
        cout << "Search path lenghts on all trees: " << perc << "%\r";
        cout.flush( );
      }
    }

    cout << "\nMax lenght for all paths between trees of " << n << " nodes is " << max << ".\n";
  }

  void full_minmax ( ) {
    cout << "Calculating minmax lenghts.\n";

    double min = 100 * n;

    for ( E i = 0; i < num_vertices( g ); i++ ) {
      std::fill_n(dist, num_vertices( g ), 0 );

      breadth_first_search( g, vertex( i, g ),
        visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );

      max_value result = for_each( dist, dist + num_vertices( g ), max_value() );
      if ( result.max <= min ) {
        min = result.max;
        cout << "Found tree distant " << min << " from any other tree.\n";
      }
    }
  }

  void get_paths ( T a, T b ) {
    if ( a.size() != b.size() || a.size() != n ) return;

    E start = m.project_right( m.left.find( a ) )->first;
    E end = m.project_right( m.left.find( b ) )->first;

    std::fill_n(dist, num_vertices( g ), 0 );
    breadth_first_search( g, vertex( start, g ), visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );
    E total = dist[end];

    for ( E i = 0; i < num_vertices( g ); i++ ) {
      if ( dist[i] != 1 ) continue;

      graph_traits<graph_type>::vertices_size_type tmp [ num_vertices( g ) ];
      std::fill_n(tmp, num_vertices( g ), 0 );
      breadth_first_search( g, vertex( i, g ), visitor( make_bfs_visitor( record_distances( tmp, on_tree_edge() ) ) ) );

      if ( tmp[end] == total - 1 ) {
        printf( "Found one tree with distance %d\n", total - 1 );
        //cout << ptree<E>( m.project_left( m.right.find( i ) )->first ) << endl;
      }
    }
  }

  void random_average ( E num ) {
    cout << "Calculating average lenghts starting from " << num << " random trees.\n";

    E random_vertex;
    double average = 0.0;

    for ( E i = 0; i < num; i++ ) {
      std::fill_n(dist, num_vertices( g ), 0 );
      random_vertex = random() % num_vertices( g );

      breadth_first_search( g, vertex( random_vertex, g ),
        visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );

      double sum = 0.0;
      for ( E j = 0; j < num_vertices( g ); j++ ) sum += dist[j];
      sum /= num_vertices( g ) - 1;

      average += sum;
    }

    cout << "Average is " << average / num << ".\n";
  }

  // calcolo della distanza ottima fra a e b
  E distance ( const T a, const T b ) {
    if ( a.vertices() != b.vertices() || a.vertices() != n ) return -1;

    E j = m.project_right( m.left.find( a ) )->first;
    E i = m.project_right( m.left.find( b ) )->first;

    std::fill_n(dist, num_vertices( g ), 0 );
    breadth_first_search( g, vertex( i, g ), visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );

    return dist[j];
  }

  void random_max_path_search ( E num ) {
    cout << "Search path lenghts starting from " << num << " random trees.\n";

    E random_vertex;
    E max = 0;

    for ( E i = 0; i < num; i++ ) {
      std::fill_n(dist, num_vertices( g ), 0 );
      random_vertex = random() % num_vertices( g );

      breadth_first_search( g, vertex( random_vertex, g ),
        visitor( make_bfs_visitor( record_distances( dist, on_tree_edge() ) ) ) );

      max_value result = for_each( dist, dist + num_vertices( g ), max_value() );
      if ( result.max > max ) {
        max = result.max;
        cout << "Found path(s) of lenght " << max << ".\n";
      }

      for ( E i = 0; i < num_vertices( g ); i++ ) {
        if ( dist[i] == max ) {

          if ( VERBOSE ) {
            ptree<typename T::node_type> first( m.project_left( m.right.find( i ) )->first );
            ptree<typename T::node_type> second( m.project_left( m.right.find( random_vertex ) )->first );

            //cout << first.to_string_with( second ) << endl << endl;
            //cout << first << endl;
            //cout << second << endl;
          }
        }
      }
    }
  }
};

#endif


#ifndef BLUB_MATH_VECTOR2TEMPLATE_HPP
#define BLUB_MATH_VECTOR2TEMPLATE_HPP

#include "blub/core/globals.hpp"
#include "blub/serialization/access.hpp"
#include "blub/serialization/nameValuePair.hpp"

#include <boost/functional/hash.hpp>

namespace blub
{

template <typename valueType, valueType valueDefault>
class vector2Template
{
public:
    typedef vector2Template<valueType, valueDefault> t_thisClass;

    vector2Template()
        : x(valueDefault)
        , y(valueDefault)
    {
        ;
    }

    vector2Template(const valueType& x_, const valueType& y_)
        : x(x_)
        , y(y_)
    {
        ;
    }

    vector2Template(const valueType& val)
        : x(val)
        , y(val)
    {
        ;
    }

    vector2Template(const t_thisClass& copy)
        : x(copy.x)
        , y(copy.y)
    {
        ;
    }

    bool operator == (const t_thisClass& other) const
    {
        return other.x == x && other.y == y;
    }

    bool operator <= (const t_thisClass& other) const
    {
        return x <= other.x && y <= other.y;
    }

    bool operator < (const t_thisClass& other) const
    {
        return x < other.x && y < other.y;
    }

    bool operator >= (const t_thisClass& other) const
    {
        return x >= other.x && y >= other.y;
    }

    bool operator > (const t_thisClass& other) const
    {
        return x > other.x && y > other.y;
    }

    t_thisClass operator * (const t_thisClass& other) const
    {
        return t_thisClass(other.x*x, other.y*y);
    }

    t_thisClass operator * (const int32& other) const
    {
        return t_thisClass(other*x, other*y);
    }

    t_thisClass operator + (const t_thisClass& other) const
    {
        return t_thisClass(other.x+x, other.y+y);
    }

    t_thisClass operator - (const t_thisClass& other) const
    {
        return t_thisClass(x-other.x, y-other.y);
    }

    t_thisClass operator - (void) const
    {
        return t_thisClass(-x, -y);
    }

    t_thisClass operator / (const t_thisClass& other) const
    {
        return t_thisClass(x/other.x, y/other.y);
    }

    t_thisClass operator / (const valueType& other) const
    {
        return t_thisClass(x/other, y/other);
    }

    valueType calculateArea() const
    {
        return x*y;
    }

protected:
    BLUB_SERIALIZATION_ACCESS
    template<typename Archive>
    void serialize(Archive & readWrite, const unsigned int version)
    {
        (void)version;

        readWrite & BLUB_SERIALIZATION_NAMEVALUEPAIR(x);
        readWrite & BLUB_SERIALIZATION_NAMEVALUEPAIR(y);
    }

public:
    valueType x;
    valueType y;

};

template <typename valueType, valueType valueDefault>
std::ostream& operator<< (std::ostream& ostr, const vector2Template<valueType, valueDefault>& toCast)
{
    return ostr << "(" << toCast.x << "," << toCast.y << ")";
}

template <typename valueType, valueType valueDefault>
std::size_t hash_value(const vector2Template<valueType, valueDefault>& value)
{
    std::size_t result(value.x);
    boost::hash_combine(result, value.y);

    return result;
}

}

#endif // BLUB_MATH_VECTOR2TEMPLATE_HPP


#include <boost/test/unit_test.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

using boost::format;
using namespace crave;

template <typename T>
bool check_unique(crv_vector<T>& v) {
  for (unsigned i = 0; i < v.size(); i++)
    for (unsigned j = 0; j < i; j++)
      if (v[i] == v[j]) return false;
  return true;
}

BOOST_FIXTURE_TEST_SUITE(Vector_Constraint_t, Context_Fixture)

struct Item : public crv_sequence_item {
  Item(crv_object_name) {}

  crv_vector<unsigned int> v;
  crv_constraint constraint = {30 <= v().size() && v().size() <= 50,
                               foreach (v(), v()[_i] == v()[_i - 1] + v()[_i - 2]),
                               foreach (v(), if_then(_i <= 1, v()[_i] == _i))};
};

BOOST_AUTO_TEST_CASE(fibo_test) {
  Item it{"item"};
  BOOST_REQUIRE(it.randomize());
  BOOST_REQUIRE_LE(30, it.v.size());
  BOOST_REQUIRE_LE(it.v.size(), 50);
  BOOST_REQUIRE_EQUAL(it.v[0], 0);
  BOOST_REQUIRE_EQUAL(it.v[1], 1);
  for (unsigned i = 2; i < it.v.size(); i++) BOOST_REQUIRE_EQUAL(it.v[i], it.v[i - 1] + it.v[i - 2]);
}

struct Item1 : public crv_sequence_item {
  Item1(crv_object_name) {}

  crv_vector<unsigned int> v;
};

BOOST_AUTO_TEST_CASE(free_vector_test) {
  Item1 it{"item"};
  BOOST_REQUIRE(it.randomize());
  BOOST_REQUIRE(it.v.size() > 0);
  for (unsigned i = 0; i < it.v.size(); i++) std::cout << it.v[i] << " ";
  std::cout << std::endl;
}

struct Item2 : public crv_sequence_item {
  Item2(crv_object_name) {}

  crv_vector<unsigned int> v;
  crv_constraint constraint = {foreach (v(), 100 <= v()[_i] && v()[_i] <= 200)};
};

BOOST_AUTO_TEST_CASE(default_size_test) {
  Item2 it{"item"};
  BOOST_REQUIRE(it.randomize());
  BOOST_REQUIRE(5 <= it.v.size() && it.v.size() <= 10);
  for (unsigned i = 0; i < it.v.size(); i++) {
    std::cout << it.v[i] << " ";
    BOOST_REQUIRE(100 <= it.v[i] && it.v[i] <= 200);
  }
  std::cout << std::endl;
}

struct Item3 : public crv_sequence_item {
  Item3(crv_object_name) {}

  crv_vector<int> v;
  crv_constraint constraint = {(v().size() == 100), (foreach (v(), v()[_i] < 100)), (foreach (v(), v()[_i] >= 0)),
                               (unique(v()))};
};

BOOST_AUTO_TEST_CASE(unique_test_1) {
  Item3 it{"item"};
  BOOST_REQUIRE(it.randomize());
  BOOST_REQUIRE(it.v.size() == 100);
  for (unsigned i = 0; i < it.v.size(); i++) {
    BOOST_REQUIRE(0 <= it.v[i] && it.v[i] < 100);
    for (unsigned j = 0; j < i; j++) BOOST_REQUIRE(it.v[i] != it.v[j]);
  }
}

struct s_unique_test_2 : crv_sequence_item {
  s_unique_test_2(crv_object_name) {}
  crv_vector<unsigned int> v;
  crv_constraint con = {v().size() == 7, foreach (v(), v()[_i] < 6)};
  crv_constraint con2 = {unique(v())};
};

BOOST_AUTO_TEST_CASE(unique_test_2) {
  s_unique_test_2 item("item");
  BOOST_REQUIRE(!item.randomize());
  item.con2.deactivate();
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(item.v.size() == 7);
  for (unsigned i = 0; i < item.v.size(); i++) {
    std::cout << item.v[i] << " ";
    BOOST_REQUIRE(item.v[i] < 6);
  }
  std::cout << std::endl;
}

struct s_soft_unique_test : crv_sequence_item {
  s_soft_unique_test(crv_object_name) {}
  crv_vector<unsigned int> v;
  crv_constraint con = {v().size() == 7, foreach (v(), v()[_i] < 6)};
  crv_soft_constraint con2 = {unique(v())};
};

BOOST_AUTO_TEST_CASE(soft_unique_test) {
  s_soft_unique_test item("item");
  BOOST_REQUIRE(item.randomize());
}

struct Item4 : public crv_sequence_item {
  Item4(crv_object_name) {}

  crv_vector<unsigned int> v;
  crv_constraint constraint = {(v().size() == 10)};
  crv_constraint c1 = {(foreach (v(), v()[_i] <= 100))};
  crv_constraint c2 = {(foreach (v(), v()[_i] > 100))};
};

BOOST_AUTO_TEST_CASE(constraint_management_test) {
  Item4 it{"item"};

  BOOST_REQUIRE(!it.randomize());
  it.c1.deactivate();
  BOOST_REQUIRE(it.randomize());
  for (unsigned i = 0; i < it.v.size(); i++) BOOST_REQUIRE_GT(it.v[i], 100);

  it.c1.activate();
  it.c2.deactivate();
  BOOST_REQUIRE(it.randomize());
  for (unsigned i = 0; i < it.v.size(); i++) BOOST_REQUIRE(it.v[i] <= 100);

  it.c2.activate();
  BOOST_REQUIRE(!it.randomize());
}

struct Item5 : public crv_sequence_item {
  Item5(crv_object_name) {}

  crv_vector<unsigned int> v;
  crv_constraint constraint = {(v().size() == 50), (foreach (v(), if_then(_i < 25, v()[_i] == _i))),
                               (foreach (v(), if_then(_i == 25, v()[_i] == 0))),
                               (foreach (v(), if_then(_i > 25, v()[_i] + _i == 200)))};
};

BOOST_AUTO_TEST_CASE(index_constraint_test) {
  Item5 it{"item"};
  BOOST_REQUIRE(it.randomize());
  BOOST_REQUIRE(it.v.size() == 50);
  for (unsigned i = 0; i < it.v.size(); i++) {
    if (i < 25)
      BOOST_REQUIRE(it.v[i] == i);
    else if (i > 25)
      BOOST_REQUIRE(it.v[i] + i == 200);
    else
      BOOST_REQUIRE(it.v[i] == 0);
  }
}

struct s_soft_vec_constraint : public crv_sequence_item {
  s_soft_vec_constraint(crv_object_name) {}
  crv_constraint con;
  crv_soft_constraint con2;
};

BOOST_AUTO_TEST_CASE(soft_vec_constraint) {
  crv_vector<unsigned int> v;
  s_soft_vec_constraint item1("item");
  s_soft_vec_constraint item2("item");

  item1.con &= {v().size() == 10, foreach (v(), v()[_i] >= v()[_i - 1])};
  item1.con2 &= {foreach (v(), v()[_i] < v()[_i - 1])};
  item2.con &= {v().size() == 4, foreach (v(), v()[_i] >= v()[_i - 1]), foreach (v(), v()[_i] <= 1000)};
  item2.con2 &= {foreach (v(), v()[_i] <= v()[_i - 1]), foreach (v(), if_then(_i == 0, v()[_i] % 13 == 3))};
  BOOST_REQUIRE(item1.randomize());
  BOOST_REQUIRE(item2.randomize());

  for (int j = 0; j < 10; j++) {
    BOOST_REQUIRE_EQUAL(v.size(), 4);
    std::cout << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << std::endl;
    BOOST_REQUIRE_EQUAL(v[0], v[1]);
    BOOST_REQUIRE_EQUAL(v[1], v[2]);
    BOOST_REQUIRE_EQUAL(v[2], v[3]);
    BOOST_REQUIRE_EQUAL(v[0] % 13, 3);
    if (!item2.randomize()) break;
  }
}

struct s_mixed_bv_width_1 : public crv_sequence_item {
  s_mixed_bv_width_1(crv_object_name) {}
  crv_vector<signed char> a;
  crv_constraint con = {a().size() == 28, foreach (a(), a()[_i] < (short)-100), unique(a())};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_1) {
  s_mixed_bv_width_1 item("item");
  BOOST_REQUIRE(item.randomize());
  for (unsigned i = 0; i < item.a.size(); i++) {
    BOOST_REQUIRE_LT(item.a[i], -100);
    std::cout << " " << (int)item.a[i];
  }
  std::cout << std::endl;
  BOOST_REQUIRE(check_unique(item.a));
}

struct s_mixed_bv_width_2 : public crv_sequence_item {
  s_mixed_bv_width_2(crv_object_name) {}
  crv_vector<short> a;
  crv_constraint con = {a().size() == 19, foreach (a(), a()[_i] < 10), foreach (a(), a()[_i] > -10), unique(a())};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_2) {
  s_mixed_bv_width_2 item("item");
  BOOST_REQUIRE(item.randomize());
  for (unsigned i = 0; i < item.a.size(); i++) BOOST_REQUIRE(-10 < item.a[i] && item.a[i] < 10);
  BOOST_REQUIRE(check_unique(item.a));
}

struct s_mixed_bv_width_3 : public crv_sequence_item {
  s_mixed_bv_width_3(crv_object_name) {}
  crv_vector<int> a;
  crv_constraint con = {a().size() == 19, foreach (a(), a()[_i] < (signed char)10), foreach (a(), a()[_i] > (short)-10),
                        unique(a())};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_3) {
  s_mixed_bv_width_3 item("item");
  BOOST_REQUIRE(item.randomize());
  for (unsigned i = 0; i < item.a.size(); i++) BOOST_REQUIRE(-10 < item.a[i] && item.a[i] < 10);
  BOOST_REQUIRE(check_unique(item.a));
}

struct s_mixed_bv_width_4 : public crv_sequence_item {
  s_mixed_bv_width_4(crv_object_name) {}
  crv_vector<short> a;
  crv_constraint con = {a().size() == 10, foreach (a(), -3 <= a()[_i] && a()[_i] <= 3), foreach (a(), a()[_i] != 0),
                        foreach (a(), a()[_i] * a()[_i - 1] % 6 == 0), foreach (a(), _i != 0 || a()[_i] % 2 == 0)};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_4) {
  s_mixed_bv_width_4 item("item");
  BOOST_REQUIRE(item.randomize());
  for (unsigned i = 0; i < item.a.size(); i++) {
    BOOST_REQUIRE(-3 <= item.a[i] && item.a[i] <= 3);
    BOOST_REQUIRE(item.a[i] != 0);
    if (i == 0)
      BOOST_REQUIRE(item.a[i] % 2 == 0);
    else
      BOOST_REQUIRE(item.a[i] * item.a[i - 1] % 6 == 0);
  }
}

struct s_bool_rand_vec : public crv_sequence_item {
  s_bool_rand_vec(crv_object_name) {}
  crv_vector<bool> a;
  crv_constraint con = {a().size() == 10, foreach (a(), a()[_i] != a()[_i - 1])};
};

BOOST_AUTO_TEST_CASE(bool_rand_vec) {
  s_bool_rand_vec item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE(item.a.size() == 10);
  for (unsigned i = 1; i < item.a.size(); i++) BOOST_REQUIRE(item.a[i] != item.a[i - 1]);
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/module.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/return_internal_reference.hpp>

#include <cctbx/cumulative_intensity_distribution.h>

namespace cctbx { namespace boost_python { namespace statistics_ext {

  template <typename FloatType>
  struct cumulative_intensity_wrapper
  {
    typedef cumulative_intensity<FloatType> wt;

    static void wrap() {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;

      class_<wt>("cumulative_intensity_core", no_init)
        .def(init<af::const_ref<FloatType> const &,
                  af::const_ref<FloatType> const &,
                  af::const_ref<FloatType> const &,
                  af::const_ref<FloatType> const &,
                  af::shared<miller::index<> > const &>(
              (arg("f_sq"),
               arg("d_spacings"),
               arg("mean_f_sq"),
               arg("bin_d_max"),
               arg("indices"))))
        .def("x", &wt::x, rbv)
        .def("y", &wt::y, rbv)
        ;
    }
  };

  void wrap_cumulative_intensity() {
    cumulative_intensity_wrapper<double>::wrap();
  }

  void init_module()
  {
    using namespace boost::python;

    wrap_cumulative_intensity();
  }
}}} // cctbx::boostpython::statistics_ext

BOOST_PYTHON_MODULE(cctbx_statistics_ext)
{
  cctbx::boost_python::statistics_ext::init_module();
}


/* transfer_function.cc
   Jeremy Barnes, 4 November 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Transfer function implementation.
*/

#include "transfer_function.h"
#include <cmath>
#include "jml/db/persistent.h"
#include "jml/boosting/registry.h"
#include "jml/utils/smart_ptr_utils.h"

#include <boost/static_assert.hpp>

using namespace ML::DB;
using namespace std;

namespace ML {

/*****************************************************************************/
/* RANGE_TYPE                                                                */
/*****************************************************************************/

/*****************************************************************************/
/* RANGE                                                                     */
/*****************************************************************************/

/*****************************************************************************/
/* TRANSFER_FUNCTION                                                         */
/*****************************************************************************/

void
Transfer_Function::
poly_serialize(DB::Store_Writer & store) const
{
    Registry<Transfer_Function>::singleton().serialize(store, this);
}

std::shared_ptr<Transfer_Function>
Transfer_Function::
poly_reconstitute(DB::Store_Reader & store)
{
    return Registry<Transfer_Function>::singleton().reconstitute(store);
}

distribution<float>
Transfer_Function::
transfer(const distribution<float> & activation) const
{
    int no = activation.size();
    distribution<float> output(no);
    transfer(&activation[0], &output[0], no);
    return output;
}

distribution<double>
Transfer_Function::
transfer(const distribution<double> & activation) const
{
    int no = activation.size();
    distribution<double> output(no);
    transfer(&activation[0], &output[0], no);
    return output;
}

distribution<float>
Transfer_Function::
derivative(const distribution<float> & outputs) const
{
    int no = outputs.size();
    distribution<float> result(no);
    derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<double>
Transfer_Function::
derivative(const distribution<double> & outputs) const
{
    int no = outputs.size();
    distribution<double> result(no);
    derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<float>
Transfer_Function::
second_derivative(const distribution<float> & outputs) const
{
    int no = outputs.size();
    distribution<float> result(no);
    second_derivative(&outputs[0], &result[0], no);
    return result;
}

distribution<double>
Transfer_Function::
second_derivative(const distribution<double> & outputs) const
{
    int no = outputs.size();
    distribution<double> result(no);
    second_derivative(&outputs[0], &result[0], no);
    return result;
}

/*****************************************************************************/
/* STANDARD_TRANSFER_FUNCTION                                                */
/*****************************************************************************/

Standard_Transfer_Function::
Standard_Transfer_Function(Transfer_Function_Type transfer_function)
    : transfer_function(transfer_function)
{
}

std::string
Standard_Transfer_Function::
print() const
{
    return ML::print(transfer_function);
}

Range
Standard_Transfer_Function::
range() const
{
    static Range ranges[5] = {
        { -INFINITY, INFINITY, 0.0, false, false, RT_PM_INF },  /* TF_LOGSIG */
        { -1.0,      1.0,      0.0, true,  true,  RT_PM_ONE },  /* TF_TANH */
        { -1.7159,   1.7159,   0.0, true,  true,  RT_OTHER  },  /* TF_TANHS */
        { -INFINITY, INFINITY, 0.0, false, false, RT_PM_INF }, /* TF_IDENTITY */
        { 0.0,       1.0,      0.5, true,  true,  RT_PROB   }  /* TF_SOFTMAX */};

    BOOST_STATIC_ASSERT(TF_LOGSIG   == 0);
    BOOST_STATIC_ASSERT(TF_TANH     == 1);
    BOOST_STATIC_ASSERT(TF_TANHS    == 2);
    BOOST_STATIC_ASSERT(TF_IDENTITY == 3);
    BOOST_STATIC_ASSERT(TF_SOFTMAX  == 4);

    if (transfer_function <= TF_SOFTMAX)
        return ranges[transfer_function];

    throw Exception("Standard_Transfer_Function::range(): non-standard");
}

std::pair<float, float>
Standard_Transfer_Function::
targets(float maximum) const
{
    switch (transfer_function) {
    case TF_TANH:
    case TF_IDENTITY: return std::make_pair(-maximum, maximum);
    case TF_TANHS: return make_pair(-1.0, 1.0);
    case TF_SOFTMAX:
    case TF_LOGSIG: return std::make_pair(0.0f, maximum);
    default:
        throw Exception("Layer::targets(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
serialize(DB::Store_Writer & store) const
{
    store << (char)0 // version
          << transfer_function;
}

void
Standard_Transfer_Function::
reconstitute(DB::Store_Reader & store)
{
    char version;
    store >> version;
    if (version != 0)
        throw Exception("Standard_Transfer_Function::reconstitute(): "
                        "unknown version");
    store >> transfer_function;
}

std::string
Standard_Transfer_Function::
class_id() const
{
    return "Standard";
}

bool
Standard_Transfer_Function::
equal(const Transfer_Function & other) const
{
    const Standard_Transfer_Function * other_cast
        = dynamic_cast<const Standard_Transfer_Function *>(&other);
    if (!other_cast)
        return false;  // not a standard transfer function...

    return transfer_function == other_cast->transfer_function;
}

template<typename FloatIn>
void
Standard_Transfer_Function::
transfer(const FloatIn * activation, FloatIn * outputs, int nvals,
         Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {
    case TF_IDENTITY:
        std::copy(activation, activation + nvals, outputs);
        return;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i) {
            // See https://bugzilla.redhat.com/show_bug.cgi?id=521190
            // for why we use double version of exp
            outputs[i] = 1.0 / (1.0 + exp((double)-activation[i]));
        }
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] = tanh(activation[i]);
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] = 1.7159 * tanh(0.66666666666666666666 * activation[i]);
        break;

    case TF_SOFTMAX: {
        double total = 0.0;

        for (unsigned i = 0;  i < nvals;  ++i) {
            // See https://bugzilla.redhat.com/show_bug.cgi?id=521190
            // for why we use double version of exp
            total += (outputs[i] = exp((double)activation[i]));
        }

        double factor = 1.0 / total;

        for (unsigned i = 0;  i < nvals;  ++i)
            outputs[i] *= factor;

        break;
    }

    default:
        throw Exception("Standard_Transfer_Function::transfer(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
transfer(const float * activation, float * outputs, size_t n) const
{
    transfer(activation, outputs, n, transfer_function);
}

void
Standard_Transfer_Function::
transfer(const double * activation, double * outputs, size_t n) const
{
    transfer(activation, outputs, n, transfer_function);
}

template<typename FloatIn>
void
Standard_Transfer_Function::
derivative(const FloatIn * outputs, FloatIn * deriv, int nvals,
           Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {

    case TF_IDENTITY:
        std::fill(deriv, deriv + nvals, 1.0);
        break;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = outputs[i] * (1.0 - outputs[i]);
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.0 - (outputs[i] * outputs[i]);
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.7159 * 0.6666666666666
                * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_SOFTMAX:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.0 / outputs[i];
        break;

    default:
        throw Exception("Standard_Transfer_Function::transfer(): invalid transfer_function");
    }
}

void
Standard_Transfer_Function::
derivative(const float * outputs, float * derivatives, size_t n) const
{
    derivative(outputs, derivatives, n, transfer_function);
}

void
Standard_Transfer_Function::
derivative(const double * outputs, double * derivatives, size_t n) const
{
    derivative(outputs, derivatives, n, transfer_function);
}

template<typename FloatIn>
void
Standard_Transfer_Function::
second_derivative(const FloatIn * outputs, FloatIn * deriv, int nvals,
                  Transfer_Function_Type transfer_function)
{
    switch (transfer_function) {

    case TF_IDENTITY:
        std::fill(deriv, deriv + nvals, 0.0);
        break;

    case TF_TANH:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = -2.0 * outputs[i] * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_TANHS:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = 1.7159 * 0.6666666666666 * 0.6666666666666
                * -2.0 * outputs[i] * (1.0 - (outputs[i] * outputs[i]));
        break;

    case TF_LOGSIG:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = outputs[i] * (1 - outputs[i]) * (1 - 2 * outputs[i]);
        break;

#if 0
    case TF_SOFTMAX:
        for (unsigned i = 0;  i < nvals;  ++i)
            deriv[i] = ...;
        break;
#endif

    default:
        throw Exception("Standard_Transfer_Function::transfer(): second derivative not implemented "
                        "for this transfer_function "
                        + ML::print(transfer_function));
    }
}

void
Standard_Transfer_Function::
second_derivative(const float * outputs, float * second_derivatives,
                  size_t n) const
{
    second_derivative(outputs, second_derivatives, n, transfer_function);
}

void
Standard_Transfer_Function::
second_derivative(const double * outputs, double * second_derivatives,
                  size_t n) const
{
    second_derivative(outputs, second_derivatives, n, transfer_function);
}

namespace {

Register_Factory<Transfer_Function, Standard_Transfer_Function>
    STF_REGISTER("Standard");

} // file scope

/*****************************************************************************/
/* FACTORY                                                                   */
/*****************************************************************************/

std::shared_ptr<Transfer_Function>
create_transfer_function(const Transfer_Function_Type & function)
{
    return make_sp(new Standard_Transfer_Function(function));
}

std::shared_ptr<Transfer_Function>
create_transfer_function(const std::string & name)
{
    throw Exception("create_transfer_function(name): not implemented");
}

} // namespace ML


#include <ros/ros.h>
#include <fstream>

#include "recon_surface/NormalsFromPointCloud2.h"

#include <iostream>
#include "recon_surface/surface_recon.hpp"
#include "recon_surface/pset.hpp"
#include "recon_surface/hole_filling.hpp"
#include "recon_surface/rm_artifacts.hpp"
#include "recon_surface/parser.hpp"
#include "recon_surface/texture.hpp"

#include <dynamic_reconfigure/server.h>
#include <recon_surface/ReconSurfaceConfig.h>

#include <map>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>

#include <iostream>
#include <iomanip>
#include <ctime>
#include <CGAL/Vector_3.h>

#include <CGAL/Kernel_traits.h>
#include <CGAL/boost/graph/properties.h>

#include <boost/cstdint.hpp>
#include <boost/graph/graph_traits.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <iostream>
#include <chrono>
#include <visualization_msgs/Marker.h>

double gridm = 0.0;
double holemaxsize = 0.0;
string output_folder = "";
double sample = 0.0;
double trim = 0.0;
int texturesize = 0;

string generate_point_cloud_normals(const sensor_msgs::PointCloud2 msg, const geometry_msgs::Point src, string output) {
    std::vector<Point> points;
    std::vector<Vector> normals;
    std::vector<Color> colors;
    std::list<PointVectorPair> estimated_pwn;
    std::vector<Point_3> orig_points;
    std::vector<unsigned int> indices;
    auto src_point = Point_3(src.x, src.y, src.z);

    FT average_spacing = 0.;

    auto start_total_time = chrono::high_resolution_clock::now();

    auto start = chrono::high_resolution_clock::now();
    Pset pointset(points, normals, colors, sample); // init my point cloud
    pointset.read_pointCloud2(msg);

    for (size_t i = 0; i < points.size(); i++) //initialize structure for Kdtree
    {
        orig_points.push_back(Point_3(points[i][0], points[i][1], points[i][2]));
        indices.push_back(i);
    }
    auto stop = chrono::high_resolution_clock::now();

    ROS_INFO_STREAM("Time initialize pointcloud: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    // Initialize a spatial kd-tree for further search on original set of points
    start = chrono::high_resolution_clock::now();
    Tree tree(
            boost::make_zip_iterator(boost::make_tuple(orig_points.begin(), indices.begin())),
            boost::make_zip_iterator(boost::make_tuple(orig_points.end(), indices.end()))
    );

    pointset.write_ply(output + "_full_pcd_rgb.ply"); //write full rgb .ply
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to generate full rgb *.ply file: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    start = chrono::high_resolution_clock::now();
    average_spacing = CGAL::compute_average_spacing<CGAL::Sequential_tag>(orig_points.begin(), orig_points.end(), 6);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to compute_average_spacing: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    double as_ratio = gridm * log(points.size() / 10e4);

    if (as_ratio < 0.6)
        as_ratio = 0.6;

    double cell_size = average_spacing * as_ratio;

    ROS_INFO_STREAM("Estimated weight for grid size w.r.t. avg. point distance: " << as_ratio);

    start = chrono::high_resolution_clock::now();
    pointset.sample_points_cgal(cell_size, 1);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to sample_points_cgal: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    ROS_INFO_STREAM(
            "Sampled point set size: " << points.size() << " | Normals: " << normals.size() << " | Original ply size: "
                                       << colors.size());

    start = chrono::high_resolution_clock::now();
    if (normals.size() == 0){
        //ROS_INFO_STREAM("Estimating normals");
        estimate_normals(points, src_point, estimated_pwn);
    }
    else{
        //ROS_INFO_STREAM("Registering normals normals");
        estimated_pwn = register_normals(points, grab_normals(orig_points, normals));
    }

    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to estimate normals: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    ROS_INFO_STREAM("Points sizes:" << points.size() << " Estimated PWN size:" << estimated_pwn.size());

    start = chrono::high_resolution_clock::now();
    string output_ply_filepath = output + "_simplified_pcd_normals.ply";
    write_ply_wnormals(output_ply_filepath, estimated_pwn, tree, colors);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to write point cloud .ply file: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

//    start = chrono::high_resolution_clock::now();
//    trim_mesh(reconstruct_surface(estimated_pwn, output), tree, trim * (double) average_spacing, output);
//    stop = chrono::high_resolution_clock::now();
//    ROS_INFO_STREAM("Time trim mesh: "
//                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
//                            << " seconds");
//
//    start = chrono::high_resolution_clock::now();
//    fill_hole(output + "temp3.off", holemaxsize * (double) average_spacing, output);
//    stop = chrono::high_resolution_clock::now();
//    ROS_INFO_STREAM("Time fill holes: "
//                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
//                            << " seconds");
//
////    start = chrono::high_resolution_clock::now();
////    texture t(tree, colors, output + "_textured.obj", texturesize);
////    t.build_png();
////    t.save_obj();
////    stop = chrono::high_resolution_clock::now();
////    ROS_INFO_STREAM("Time generate *.obj file: "
////                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
////                            << " seconds");
//
////    ROS_INFO_STREAM("Writing STL");
//
//    std::ifstream input_off3(output + "temp3.off");
//    string output_stl_filepath = output + "_mesh.stl";
//    string output_stl_filepath_inverted = output + "_inverted_mesh.stl";
//    Mesh mesh_obj;
//
//
//    stop = chrono::high_resolution_clock::now();
//    ROS_INFO_STREAM("Time total reconstruction: "
//                            << float(chrono::duration_cast<chrono::microseconds>(stop - start_total_time).count() / 1000000.0)
//                            << " seconds");

//    if (!input_off3 || !(input_off3 >> mesh_obj)) {
//        ROS_ERROR_STREAM("Not a valid off file (generate_mesh): " << output_stl_filepath);
//        return "";
//    } else {
//        start = chrono::high_resolution_clock::now();
//
//        writeSTLfromMesh(mesh_obj, output_stl_filepath, false, true);
//        publishSTLMarker(marker_pub_normal, output_stl_filepath, msg.header.frame_id);
//
//        writeSTLfromMesh(mesh_obj, output_stl_filepath_inverted, true, true);
//        publishSTLMarker(marker_pub_inverted, output_stl_filepath_inverted, msg.header.frame_id);
//
//        ROS_INFO_STREAM("STL files written to:" << output_stl_filepath << " and " << output_stl_filepath_inverted);
//        stop = chrono::high_resolution_clock::now();
//        ROS_INFO_STREAM("Time generate *.stl file: "
//                                << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
//                                << " seconds");
//    }

    return output_ply_filepath;
}

void dynReconfigureCallback(recon_surface::ReconSurfaceConfig &config, uint32_t level) {
    if (gridm != config.gridm) {
        gridm = config.gridm;
        ROS_INFO("Reconfigure gridm: %lf", gridm);
    }

    if (holemaxsize != config.holemaxsize) {
        holemaxsize = config.holemaxsize;
        ROS_INFO("Reconfigure holemaxsize: %lf", holemaxsize);
    }

    if (output_folder != config.output_folder) {
        output_folder = config.output_folder;
        ROS_INFO("Reconfigure output_folder: %s", output_folder.c_str());
    }

    if (sample != config.sample) {
        sample = config.sample;
        ROS_INFO("Reconfigure sample: %lf", sample);
    }

    if (texturesize != config.texturesize) {
        texturesize = config.texturesize;
        ROS_INFO("Reconfigure texturesize: %lf", texturesize);
    }

    if (trim != config.trim) {
        trim = config.trim;
        ROS_INFO("Reconfigure trim: %lf", trim);
    }
}

bool pointNormalsFromPointCloudCallback(recon_surface::NormalsFromPointCloud2::Request &req,
                                   recon_surface::NormalsFromPointCloud2::Response &res) {
    ROS_INFO("pointNormalsFromPointCloudCallback called");

    try {
        const sensor_msgs::PointCloud2 ptcloud = req.input;
        const geometry_msgs::Point src = req.src;

        time_t rawtime;
        struct tm *timeinfo;
        char buffer[80];

        time(&rawtime);
        timeinfo = localtime(&rawtime);

        strftime(buffer, sizeof(buffer), "%d-%m-%Y_%H:%M:%S", timeinfo);
        std::string str(buffer);

        string output_file = output_folder;
        char endch = output_file.back();
        if ('/' != endch) {
            output_file += "/";
        }
        output_file += buffer;

        ROS_INFO_STREAM("Output file:" << output_file);

        string output_path = generate_point_cloud_normals(ptcloud, src, output_file);

        res.success = true;
        res.path = output_path;
    } catch (std::runtime_error &e) {
        ROS_ERROR("genMeshFromPointCloudCallback exception: %s", e.what());
        res.success = false;
        return false;
    }

    return true;
}

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "p_normal_service_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_priv("~");

    ROS_INFO("Start p_normal_service node");

    std::string version(std::to_string(CGAL_VERSION_NR));
    ROS_INFO_STREAM("Using CGAL lib version " << version.substr(1, 2) << "." << version.substr(3, 2));

    // dynamic reconfigure server
    dynamic_reconfigure::Server<recon_surface::ReconSurfaceConfig> server;
    dynamic_reconfigure::Server<recon_surface::ReconSurfaceConfig>::CallbackType f;

    f = boost::bind(&dynReconfigureCallback, _1, _2);
    server.setCallback(f);

    // publishers
    ros::ServiceServer service = nh.advertiseService("/pnormal_from_pointclouds", pointNormalsFromPointCloudCallback);

    ROS_INFO("Spinning p_normal_service node");

    ros::spin();

    ROS_INFO("p_normal_service node stopped");

    return 0;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_LOG10_HPP
#define PYTHONIC_INCLUDE_NUMPY_LOG10_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/log10.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME log10
#define NUMPY_NARY_FUNC_SYM boost::simd::log10
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <cnoid/SimpleController>
#include <cnoid/SharedJoystick>
#include <cnoid/EigenUtil>
#include <boost/format.hpp>

using namespace std;
using namespace cnoid;
using boost::format;

class Jaco2Controller : public SimpleController
{
    Body* body;
    double dt;

    Link::ActuationMode mainActuationMode;

    struct JointInfo {
        Link* joint;
        double q_ref;
        double q_old;
        double kp;
        double kd;
    };

    vector<JointInfo> jointInfos;

    double fingerJointTargetAngle;

    struct JointSpec {
        string name;
        double kp_torque;
        double kd_torque;
        double kp_velocity;
    };

    enum {
        SHOULDER,
        ARM,
        FOREARM,
        WRIST1,
        WRIST2,
        HAND,
        FINGER1,
        FINGER2,
        FINGER3,
        //FINGER1_TIP,
        //FINGER2_TIP,
        //FINGER3_TIP,
        NUM_JOINTS
    };

    SharedJoystickPtr joystick;
    int targetMode;

public:
    virtual bool initialize(SimpleControllerIO* io) override;
    bool initializeJoints(SimpleControllerIO* io, vector<JointSpec>& specs, const string& prefix);
    virtual bool control() override;
    void updateTargetJointAngles();
    void setTargetJointAngle(int jointID, Joystick::AxisID stickID, double k);
    void setFingerTargetJointAngles();
    void clampTargetJointAngle(int jointId);
    void setTargetJointAngle(int jointID, Joystick::ButtonID button1, Joystick::ButtonID button2, double k);

    void controlJointsWithPosition();
    void controlJointsWithVelocity();
    void controlJointsWithTorque();
};

bool Jaco2Controller::initialize(SimpleControllerIO* io)
{
    body = io->body();
    dt = io->timeStep();

    mainActuationMode = Link::JOINT_TORQUE;
    string prefix;

    for(auto& option : io->options()){
        if(option == "velocity"){
            mainActuationMode = Link::JOINT_VELOCITY;
            io->os() << "velocity mode" << endl;
        } else if(option == "position"){
            mainActuationMode = Link::JOINT_ANGLE;
            io->os() << "position mode" << endl;
        } else if(option == "torque"){
            mainActuationMode = Link::JOINT_TORQUE;
            io->os() << "torque mode" << endl;
        } else {
            prefix = option;
            io->os() << "prefix: " << prefix << endl;
        }
    }

    jointInfos.clear();

    const double P_GAIN_VELOCITY = 0.5;

    vector<JointSpec> specs(NUM_JOINTS);

    if(io->timeStep() < 0.002){
        //                                     P      D      P (vel)
        specs[SHOULDER    ] = { "SHOULDER",  1000.0, 100,  P_GAIN_VELOCITY };
        specs[ARM         ] = { "ARM",       1000.0, 100,  P_GAIN_VELOCITY };
        specs[FOREARM     ] = { "FOREARM",   600.0,   60,  P_GAIN_VELOCITY };
        specs[WRIST1      ] = { "WRIST1",    300.0,   30,  P_GAIN_VELOCITY };
        specs[WRIST2      ] = { "WRIST2",    300.0,   30,  P_GAIN_VELOCITY };
        specs[HAND        ] = { "HAND",      250.0,   25,  P_GAIN_VELOCITY };
        specs[FINGER1     ] = { "FINGER1",     30,     3,  P_GAIN_VELOCITY };
        specs[FINGER2     ] = { "FINGER2",     30,     3,  P_GAIN_VELOCITY };
        specs[FINGER3     ] = { "FINGER3",     30,     3,  P_GAIN_VELOCITY };
        //specs[FINGER1_TIP ] = { "FINGER1_TIP", 20,     2,  P_GAIN_VELOCITY };
        //specs[FINGER2_TIP ] = { "FINGER2_TIP", 20,     2,  P_GAIN_VELOCITY };
        //specs[FINGER3_TIP ] = { "FINGER3_TIP", 20,     2,  P_GAIN_VELOCITY };
    } else {
        //                                     P      D      P (vel)
        specs[SHOULDER    ] = { "SHOULDER",   400.0, 30.0,  P_GAIN_VELOCITY };
        specs[ARM         ] = { "ARM",        400.0, 30.0,  P_GAIN_VELOCITY };
        specs[FOREARM     ] = { "FOREARM",    150.0, 15.0,  P_GAIN_VELOCITY };
        specs[WRIST1      ] = { "WRIST1",      60.0,  5.0,  P_GAIN_VELOCITY };
        specs[WRIST2      ] = { "WRIST2",      60.0,  5.0,  P_GAIN_VELOCITY };
        specs[HAND        ] = { "HAND",        60.0,  5.0,  P_GAIN_VELOCITY };
        specs[FINGER1     ] = { "FINGER1",      5.0,  0.5,  P_GAIN_VELOCITY };
        specs[FINGER2     ] = { "FINGER2",      5.0,  0.5,  P_GAIN_VELOCITY };
        specs[FINGER3     ] = { "FINGER3",      5.0,  0.5,  P_GAIN_VELOCITY };
        //specs[FINGER1_TIP ] = { "FINGER1_TIP", 3.0,  0.3,  P_GAIN_VELOCITY };
        //specs[FINGER2_TIP ] = { "FINGER2_TIP", 3.0,  0.3,  P_GAIN_VELOCITY };
        //specs[FINGER3_TIP ] = { "FINGER3_TIP", 3.0,  0.3,  P_GAIN_VELOCITY };
    }

    if(!initializeJoints(io, specs, prefix)){
        return false;
    }

    joystick = io->getOrCreateSharedObject<SharedJoystick>("joystick");
    targetMode = joystick->addMode();

    return true;
}

bool Jaco2Controller::initializeJoints(SimpleControllerIO* io, vector<JointSpec>& specs, const string& prefix)
{
    for(auto& spec : specs){
        JointInfo info;
        string name = prefix + spec.name;
        auto joint = body->link(name);
        if(!joint){
            io->os() << format("%1% of %2% is not found") % name % body->name() << endl;
            return false;
        } else {
            joint->setActuationMode(mainActuationMode);
            io->enableIO(joint);

            info.joint = joint;
            info.q_ref = info.q_old = joint->q();

            if(mainActuationMode == Link::JOINT_VELOCITY){
                info.kp = spec.kp_velocity;
            } else if(mainActuationMode == Link::JOINT_TORQUE){
                info.kp = spec.kp_torque;
                info.kd = spec.kd_torque;
            }
        }
        jointInfos.push_back(info);
    }

    fingerJointTargetAngle = jointInfos[FINGER1].joint->q();

    return true;
}

bool Jaco2Controller::control()
{
    joystick->updateState(targetMode);

    updateTargetJointAngles();

    switch(mainActuationMode){
    case Link::JOINT_TORQUE:
        controlJointsWithTorque();
        break;
    case Link::JOINT_VELOCITY:
        controlJointsWithVelocity();
        break;
    case Link::JOINT_ANGLE:
        controlJointsWithPosition();
        break;
    default:
        break;
    }

    return true;
}

void Jaco2Controller::updateTargetJointAngles()
{
    static const double K = 0.8;

    setTargetJointAngle(SHOULDER, Joystick::L_STICK_H_AXIS, -K);
    setTargetJointAngle(ARM,      Joystick::L_STICK_V_AXIS, -K);
    setTargetJointAngle(FOREARM,  Joystick::R_STICK_V_AXIS,  K);
    setTargetJointAngle(WRIST1,   Joystick::R_STICK_H_AXIS, -K);

    setTargetJointAngle(WRIST2, Joystick::B_BUTTON, Joystick::X_BUTTON, K);
    setTargetJointAngle(HAND,   Joystick::R_BUTTON, Joystick::L_BUTTON, 1.2 * K);

    setFingerTargetJointAngles();
}

void Jaco2Controller::setTargetJointAngle(int jointID, Joystick::AxisID stickID, double k)
{
    jointInfos[jointID].q_ref += dt * k * joystick->getPosition(targetMode, stickID, 0.1);
    clampTargetJointAngle(jointID);
}

void Jaco2Controller::setTargetJointAngle(int jointID, Joystick::ButtonID button1, Joystick::ButtonID button2, double k)
{
    if(joystick->getButtonState(targetMode, button1)){
        jointInfos[jointID].q_ref -= dt * k;
    }
    if(joystick->getButtonState(targetMode, button2)){
        jointInfos[jointID].q_ref += dt * k;
    }
    clampTargetJointAngle(jointID);
}

void Jaco2Controller::setFingerTargetJointAngles()
{
    double lt = joystick->getPosition(targetMode, Joystick::L_TRIGGER_AXIS);
    if(lt > 0.1){
        fingerJointTargetAngle -= dt * 0.4 * lt;
    }
    double rt = joystick->getPosition(targetMode, Joystick::R_TRIGGER_AXIS);
    if(rt > 0.1){
        fingerJointTargetAngle += dt * 0.4 * rt;
    }
    double q_max = -100.0;
    double q_min = 100.0;

    for(auto& id : vector<int>{ FINGER1, FINGER2, FINGER3 }){
        auto& info = jointInfos[id];
        info.q_ref = fingerJointTargetAngle;
        clampTargetJointAngle(id);
        q_max = std::max(info.q_ref, q_max);
        q_min = std::min(info.q_ref, q_min);
    }
    if(fingerJointTargetAngle > q_max){
        fingerJointTargetAngle = q_max;
    } else if(fingerJointTargetAngle < q_min){
        fingerJointTargetAngle = q_min;
    }
}

void Jaco2Controller::clampTargetJointAngle(int jointID)
{
    auto& info = jointInfos[jointID];
    auto joint = info.joint;
    auto& q_ref = info.q_ref;
    static const double maxerror = radian(3.0);
    double q_current = joint->q();
    double q_lower = std::max(q_current - maxerror, joint->q_lower());
    double q_upper = std::min(q_current + maxerror, joint->q_upper());
    if(q_ref < q_lower){
        q_ref = q_lower;
    } else if(q_ref > q_upper){
        q_ref = q_upper;
    }
}

void Jaco2Controller::controlJointsWithPosition()
{
    for(auto& info : jointInfos){
        info.joint->q_target() = info.q_ref;
    }
}

void Jaco2Controller::controlJointsWithVelocity()
{
    for(auto& info : jointInfos){
        auto joint = info.joint;
        double q = joint->q();
        joint->dq_target() = info.kp * (info.q_ref - q) / dt;
    }
}

void Jaco2Controller::controlJointsWithTorque()
{
    for(auto& info : jointInfos){
        auto joint = info.joint;
        double q = joint->q();
        double dq = (q - info.q_old) / dt;
        joint->u() = info.kp * (info.q_ref - q) + info.kd * (0.0 - dq);
        info.q_old = q;
    }
}

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(Jaco2Controller)


/* Copyright (C) 2012,2013 IBM Corp.
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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 GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
#include "matrix.h"
#include <NTL/ZZ.h>
#include <NTL/lzz_pXFactoring.h>
#include <cassert>
#include <cstdio>

template<class type>
class SingleBlockMatrix : public  PlaintextBlockMatrixInterface<type> {
public:
  PA_INJECT(type)

private:
  const EncryptedArray& ea;

  Mat<R> data;

public:
  SingleBlockMatrix(const EncryptedArray& _ea, const vector<ZZX>& vec) : ea(_ea) {
    long d = ea.getDegree();

    RBak bak; bak.save(); ea.getContext().alMod.restoreContext();

    data.SetDims(d, d);
    for (long i = 0; i < d; i++)
      for (long j = 0; j < d; j++)
         conv(data[i][j], coeff(vec[i], j));
  }

  virtual const EncryptedArray& getEA() const {
    return ea;
  }

  virtual bool get(Mat<R>& out, long i, long j) const {
    assert(i >= 0 && i < ea.size());
    assert(j >= 0 && j < ea.size());
    if (i != j) return true;
    out = data;
    return false;
  }

};

PlaintextBlockMatrixBaseInterface *buildSingleBlockMatrix(const EncryptedArray& ea,
                                                          const vector<ZZX>& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new SingleBlockMatrix<PA_GF2>(ea, vec);
    }

    case PA_zz_p_tag: {
      return new SingleBlockMatrix<PA_zz_p>(ea, vec);
    }

    default: return 0;
  }
}

template<class type>
class MultiBlockMatrix : public  PlaintextBlockMatrixInterface<type> {
public:
  PA_INJECT(type)

private:
  const EncryptedArray& ea;

  Vec< Mat<R> > data;

public:
  MultiBlockMatrix(const EncryptedArray& _ea, const vector< vector<ZZX> >& vec) : ea(_ea) {
    long n = ea.size();
    long d = ea.getDegree();

    RBak bak; bak.save(); ea.getContext().alMod.restoreContext();

    data.SetLength(n);
    for (long k = 0; k < n; k++) {
      data[k].SetDims(d, d);
      for (long i = 0; i < d; i++)
        for (long j = 0; j < d; j++)
           conv(data[k][i][j], coeff(vec[k][i], j));
    }
  }

  virtual const EncryptedArray& getEA() const {
    return ea;
  }

  virtual bool get(Mat<R>& out, long i, long j) const {
    assert(i >= 0 && i < ea.size());
    assert(j >= 0 && j < ea.size());
    if (i != j) return true;
    out = data[i];
    return false;
  }
};

PlaintextBlockMatrixBaseInterface *buildMultiBlockMatrix(const EncryptedArray& ea,
                                                          const vector< vector<ZZX> >& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new MultiBlockMatrix<PA_GF2>(ea, vec);
    }

    case PA_zz_p_tag: {
      return new MultiBlockMatrix<PA_zz_p>(ea, vec);
    }

    default: return 0;
  }
}

void  TestIt(long m, long p, long r, long d)
{
  cout << "\n\n******** TestIt" << (isDryRun()? "(dry run):" : ":")
       << " m=" << m
       << ", p=" << p
       << ", r=" << r
       << ", d=" << d
       << endl;

  FHEcontext context(m, p, r);
  buildModChain(context, /*L=*/10, /*c=*/2);
  context.zMStar.printout();
  cout << endl;

  cout << "generating keys and key-switching matrices... " << std::flush;
  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(/*w=*/64);// A Hamming-weight-w secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need
  cout << "done\n";

  ZZX G;
  if (d == 0) {
    G = context.alMod.getFactorsOverZZ()[0];
    d = deg(G);
  }
  else
    G = makeIrredPoly(p, d);
  cout << "G = " << G << "\n";
  cout << "computing masks and tables for rotation... " << std::flush;
  EncryptedArray ea(context, G);
  cout << "done\n";

  long nslots = ea.size();

  PlaintextArray p0(ea);
  PlaintextArray pp0(ea);

  Ctxt c0(publicKey), cc0(publicKey);

  cout << "\nTest #1: Apply the same linear transformation to all slots\n";
  {
  vector<ZZX> LM(d); // LM selects even coefficients
  for (long j = 0; j < d; j++)
    if (j % 2 == 0) LM[j] = ZZX(j, 1);

  // "building" the linearized-polynomial coefficients
  vector<ZZX> C;
  ea.buildLinPolyCoeffs(C, LM);

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPoly1(ea, c0, C);
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildSingleBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  cout << "\nTest #2: Apply different transformations to the different slots\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] rotates the coefficients in the i'th slot by (i % d)
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      long jj = (i+j) % d;
      LM[i][j] = ZZX(jj, 1);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPolyMany(ea, c0, C); // apply the linearized polynomials
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildMultiBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  cout << "\nTest #3: Testing low-level (cached) implementation\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] adds coefficients (i % d) and (i+1 % d) in the i'th slot
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      if ( j == (i % d) || j == ((i+1)%d) )
	LM[i][j] = conv<ZZX>(1L);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  // "encoding" the linearized-polynomial coefficients
  vector<ZZX> encodedC(d);
  for (long j = 0; j < d; j++) {
    vector<ZZX> v(nslots);
    for (long i = 0; i < nslots; i++) v[i] = C[i][j];
    ea.encode(encodedC[j], v);
  }

  p0.random();
  ea.encrypt(c0, publicKey, p0);
  ea.encrypt(cc0, publicKey, p0);

  applyLinPolyLL(c0, encodedC, ea.getDegree()); // apply linearized polynomials
  ea.decrypt(c0, secretKey, p0);

  shared_ptr<PlaintextBlockMatrixBaseInterface> mat(buildMultiBlockMatrix(ea, LM));
  free_mat_mul(ea, cc0, *mat);
  ea.decrypt(cc0, secretKey, pp0);

  if (pp0.equals(p0))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }
}

int main(int argc, char *argv[])
{
  ArgMapping amap;

  bool dry = false;
  amap.arg("dry", dry, "dry=1 for a dry-run");

  long m=91;
  amap.arg("m", m, "use specified value as modulus");

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long d=0;
  amap.arg("d", d, "degree of the field extension");
  amap.note("d == 0 => factors[0] defines extension");

  amap.parse(argc, argv);

  long repeat = 2;
  setTimersOn();
  setDryRun(dry);
  for (long repeat_cnt = 0; repeat_cnt < repeat; repeat_cnt++) {
    TestIt(m, p, r, d);
  }

}


#pragma once

#include <algorithm>
#include <cstddef>
#include <functional>
#include <memory>
#include <string>
#include <vector>

#include <boost/functional/hash.hpp>

namespace pyqubo {
  enum class expression_type {
    add_operator,
    mul_operator,
    binary_variable,
    spin_variable,
    place_holder_variable,
    sub_hamiltonian,
    constraint,
    with_penalty,
    user_defined_expression,
    numeric_literal
  };

  class expression {
  public:
    virtual ~expression() {
      ;
    }

    virtual pyqubo::expression_type expression_type() const noexcept = 0;

    virtual std::string to_string() const noexcept = 0;

    virtual std::size_t hash() const noexcept = 0;

    virtual bool equals(const std::shared_ptr<const expression>& other) const noexcept {
      return expression_type() == other->expression_type();
    };

    friend std::hash<expression>;
  };
}

namespace std {
  template <>
  struct hash<pyqubo::expression> {
    auto operator()(const pyqubo::expression& expression) const noexcept {
      return expression.hash();
    }
  };
}

namespace pyqubo {
  class add_operator final : public expression {
    std::vector<std::shared_ptr<const expression>> _children;

  public:
    add_operator(const std::shared_ptr<const expression>& lhs, const std::shared_ptr<const expression>& rhs) noexcept : _children{lhs, rhs} {
      ;
    }

    const auto& children() const noexcept {
      return _children;
    }

    auto add_child(const std::shared_ptr<const expression>& expression) noexcept {
      _children.emplace_back(expression);
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::add_operator;
    }

    std::string to_string() const noexcept override {
      return "(" +
             std::accumulate(std::begin(_children), std::end(_children), std::string(), [](const auto& acc, const auto& child) {
               return acc + (std::size(acc) > 0 ? " + " : "") + child->to_string();
             }) +
             ")";
    }

    std::size_t hash() const noexcept override {
      auto result = static_cast<std::size_t>(0);

      boost::hash_combine(result, "+");

      for (const auto& child : _children) {
        boost::hash_combine(result, std::hash<expression>()(*child));
      }

      return result;
    }

    bool equals(const std::shared_ptr<const expression>& other) const noexcept override {
      if (!expression::equals(other)) {
        return false;
      }

      const auto& other_add_operator = std::static_pointer_cast<const add_operator>(other);

      if (std::size(_children) != std::size(other_add_operator->_children)) {
        return false;
      }

      for (auto i = 0; i < static_cast<int>(std::size(_children)); ++i) {
        if (!_children[i]->equals(other_add_operator->_children[i])) {
          return false;
        }
      }

      return true;
    }
  };

  class mul_operator final : public expression {
    std::shared_ptr<const expression> _lhs;
    std::shared_ptr<const expression> _rhs;

  public:
    mul_operator(const std::shared_ptr<const expression>& lhs, const std::shared_ptr<const expression>& rhs) noexcept : _lhs(lhs), _rhs(rhs) {
      ;
    }

    const auto& lhs() const noexcept {
      return _lhs;
    }

    const auto& rhs() const noexcept {
      return _rhs;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::mul_operator;
    }

    std::string to_string() const noexcept override {
      return "(" + lhs()->to_string() + " * " + rhs()->to_string() + ")";
    }

    std::size_t hash() const noexcept override {
      auto result = static_cast<std::size_t>(0);

      boost::hash_combine(result, "*");
      boost::hash_combine(result, std::hash<expression>()(*_lhs));
      boost::hash_combine(result, std::hash<expression>()(*_rhs));

      return result;
    }

    bool equals(const std::shared_ptr<const expression>& other) const noexcept override {
      return expression::equals(other) && _lhs->equals(std::static_pointer_cast<const mul_operator>(other)->_lhs) && _rhs->equals(std::static_pointer_cast<const mul_operator>(other)->_rhs);
    }
  };

  class variable : public expression {
    std::string _name;

  protected:
    variable(const std::string& name) noexcept : _name(name) {
      ;
    }

  public:
    const auto& name() const noexcept {
      return _name;
    }

    std::size_t hash() const noexcept override {
      return std::hash<std::string>()(_name);
    }

    bool equals(const std::shared_ptr<const expression>& other) const noexcept override {
      return expression::equals(other) && _name == std::static_pointer_cast<const variable>(other)->_name;
    }
  };

  class binary_variable final : public variable {
  public:
    binary_variable(const std::string& name) noexcept : variable(name) {
      ;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::binary_variable;
    }

    std::string to_string() const noexcept override {
      return "Binary('" + name() + "')";
    }

    std::size_t hash() const noexcept override {
      auto result = variable::hash();

      boost::hash_combine(result, "binary_variable");

      return result;
    }
  };

  class spin_variable final : public variable {
  public:
    spin_variable(const std::string& name) noexcept : variable(name) {
      ;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::spin_variable;
    }

    std::string to_string() const noexcept override {
      return "Spin('" + name() + "')";
    }

    std::size_t hash() const noexcept override {
      auto result = variable::hash();

      boost::hash_combine(result, "spin_variable");

      return result;
    }
  };

  class placeholder_variable final : public variable {
  public:
    placeholder_variable(const std::string& name) noexcept : variable(name) {
      ;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::place_holder_variable;
    }

    std::string to_string() const noexcept override {
      return "Placeholder('" + name() + "')";
    }

    std::size_t hash() const noexcept override {
      auto result = variable::hash();

      boost::hash_combine(result, "placeholder_variable");

      return result;
    }
  };

  class sub_hamiltonian : public variable {
    std::shared_ptr<const expression> _expression;

  public:
    sub_hamiltonian(const std::shared_ptr<const pyqubo::expression>& expression, const std::string& name) noexcept : variable(name), _expression(expression) {
      ;
    }

    const auto& expression() const noexcept {
      return _expression;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::sub_hamiltonian;
    }

    std::string to_string() const noexcept override {
      return "SubH(" + _expression->to_string() + ", '" + name() + "')";
    }

    std::size_t hash() const noexcept override {
      auto result = variable::hash();

      boost::hash_combine(result, "sub_hamiltonian");
      boost::hash_combine(result, std::hash<pyqubo::expression>()(*_expression));

      return result;
    }

    bool equals(const std::shared_ptr<const pyqubo::expression>& other) const noexcept override {
      return variable::equals(other) && _expression->equals(std::static_pointer_cast<const sub_hamiltonian>(other)->_expression);
    }
  };

  class constraint final : public sub_hamiltonian {
    std::function<bool(double)> _condition;

  public:
    constraint(
        const std::shared_ptr<const pyqubo::expression>& expression, const std::string& name, const std::function<bool(double)>& condition) noexcept : sub_hamiltonian(expression, name), _condition(condition) {
      ;
    }

    const auto& condition() const noexcept {
      return _condition;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::constraint;
    }

    std::string to_string() const noexcept override {
      return "Constraint(" + expression()->to_string() + ", '" + name() + "')"; // conditionは文字列化できない……。
    }

    std::size_t hash() const noexcept override {
      auto result = variable::hash();

      boost::hash_combine(result, "constraint");

      return result;
    }
  };

  class with_penalty : public sub_hamiltonian {
    std::shared_ptr<const pyqubo::expression> _penalty;

  public:
    with_penalty(const std::shared_ptr<const pyqubo::expression>& expression, const std::shared_ptr<const pyqubo::expression>& penalty, const std::string& name) noexcept : sub_hamiltonian(expression, name), _penalty(penalty) {
      ;
    }

    const auto& penalty() const noexcept {
      return _penalty;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::with_penalty;
    }

    std::string to_string() const noexcept override {
      return "WithPenalty(" + expression()->to_string() + ", " + _penalty->to_string() + ", '" + name() + "')";
    }

    std::size_t hash() const noexcept override {
      auto result = sub_hamiltonian::hash();

      boost::hash_combine(result, "with_penalty");
      boost::hash_combine(result, std::hash<pyqubo::expression>()(*_penalty));

      return result;
    }

    bool equals(const std::shared_ptr<const pyqubo::expression>& other) const noexcept override {
      return sub_hamiltonian::equals(other) && _penalty->equals(std::static_pointer_cast<const with_penalty>(other)->_penalty);
    }
  };

  class user_defined_expression : public expression {
    std::shared_ptr<const expression> _expression;

  public:
    user_defined_expression(const std::shared_ptr<const pyqubo::expression>& expression) noexcept : _expression(expression) {
      ;
    }

    auto expression() const noexcept {
      return _expression;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::user_defined_expression;
    }

    std::string to_string() const noexcept override {
      return _expression->to_string();
    }

    std::size_t hash() const noexcept override {
      return std::hash<pyqubo::expression>()(*_expression);
    }

    bool equals(const std::shared_ptr<const pyqubo::expression>& other) const noexcept override {
      return expression::equals(other) && _expression->equals(std::static_pointer_cast<const user_defined_expression>(other)->_expression);
    }
  };

  class numeric_literal final : public expression {
    double _value;

  public:
    numeric_literal(double value) noexcept : _value(value) {
      ;
    }

    auto value() const noexcept {
      return _value;
    }

    pyqubo::expression_type expression_type() const noexcept override {
      return expression_type::numeric_literal;
    }

    std::string to_string() const noexcept override {
      return std::to_string(_value);
    }

    std::size_t hash() const noexcept override {
      return std::hash<double>()(_value);
    }

    bool equals(const std::shared_ptr<const expression>& other) const noexcept override {
      return expression::equals(other) && _value == std::static_pointer_cast<const numeric_literal>(other)->_value;
    }
  };

  inline std::shared_ptr<const expression> operator+(const std::shared_ptr<const expression>& lhs, const std::shared_ptr<const expression>& rhs) noexcept {
    if (lhs->expression_type() == expression_type::numeric_literal && rhs->expression_type() == expression_type::numeric_literal) {
      return std::make_shared<numeric_literal>(std::static_pointer_cast<const numeric_literal>(lhs)->value() + std::static_pointer_cast<const numeric_literal>(rhs)->value());
    }

    if (lhs->expression_type() == expression_type::numeric_literal && std::static_pointer_cast<const numeric_literal>(lhs)->value() == 0) {
      return rhs;
    }

    if (rhs->expression_type() == expression_type::numeric_literal && std::static_pointer_cast<const numeric_literal>(rhs)->value() == 0) {
      return lhs;
    }

    return std::make_shared<const add_operator>(lhs, rhs);
  }

  inline std::shared_ptr<const expression> operator*(const std::shared_ptr<const expression>& lhs, const std::shared_ptr<const expression>& rhs) noexcept {
    if (lhs->expression_type() == expression_type::numeric_literal && rhs->expression_type() == expression_type::numeric_literal) {
      return std::make_shared<numeric_literal>(std::static_pointer_cast<const numeric_literal>(lhs)->value() * std::static_pointer_cast<const numeric_literal>(rhs)->value());
    }

    if (lhs->expression_type() == expression_type::numeric_literal && std::static_pointer_cast<const numeric_literal>(lhs)->value() == 1) {
      return rhs;
    }

    if (rhs->expression_type() == expression_type::numeric_literal && std::static_pointer_cast<const numeric_literal>(rhs)->value() == 1) {
      return lhs;
    }

    return std::make_shared<const mul_operator>(lhs, rhs);
  }

  template <typename Result, typename Functor>
  Result visit(Functor& functor, const std::shared_ptr<const expression>& expression) noexcept {
    switch (expression->expression_type()) {
    case expression_type::add_operator:
      return functor(std::static_pointer_cast<const add_operator>(expression));

    case expression_type::mul_operator:
      return functor(std::static_pointer_cast<const mul_operator>(expression));

    case expression_type::binary_variable:
      return functor(std::static_pointer_cast<const binary_variable>(expression));

    case expression_type::spin_variable:
      return functor(std::static_pointer_cast<const spin_variable>(expression));

    case expression_type::place_holder_variable:
      return functor(std::static_pointer_cast<const placeholder_variable>(expression));

    case expression_type::sub_hamiltonian:
      return functor(std::static_pointer_cast<const sub_hamiltonian>(expression));

    case expression_type::constraint:
      return functor(std::static_pointer_cast<const constraint>(expression));

    case expression_type::with_penalty:
      return functor(std::static_pointer_cast<const with_penalty>(expression));

    case expression_type::user_defined_expression:
      return functor(std::static_pointer_cast<const user_defined_expression>(expression));

    case expression_type::numeric_literal:
      return functor(std::static_pointer_cast<const numeric_literal>(expression));

    default:
      throw std::runtime_error("invalid expression type."); // ここには絶対に来ないはず。
    }
  }
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file fusion_alogrithms_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-04
 */

#include "paal/utils/fusion_algorithms.hpp"
#include "paal/utils/functors.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/fusion/include/vector.hpp>

namespace ds = paal::data_structures;

struct Sum {
    template <typename Number, typename AccumulatorFun,
              typename AccumulatorData, typename Callback>
    auto operator()(Number n, AccumulatorFun aFun, AccumulatorData aData,
                    Callback call) const->decltype(call(aFun, aData + n)) {
        return call(aFun, aData + n);
    }
};

struct strange_weighted_sum {

    template <typename AccumulatorFun, typename AccumulatorData,
              typename Callback>
    auto operator()(int n, AccumulatorFun aFun, AccumulatorData aData,
                    Callback call) const->decltype(call(aFun, aData + n)) {
        return call(aFun, aData + n);
    }

    template <typename AccumulatorFun, typename AccumulatorData,
              typename Callback>
    auto operator()(float n, AccumulatorFun aFun, AccumulatorData aData,
                    Callback call) const->decltype(call(aFun, aData + n / 2)) {
        return call(aFun, aData + n / 2);
    }

    template <typename AccumulatorFun, typename AccumulatorData,
              typename Callback>
    auto operator()(long long n, AccumulatorFun aFun, AccumulatorData aData,
                    Callback call) const->decltype(call(aFun, aData + n / 3)) {
        return call(aFun, aData + n / 3);
    }
};

struct Find12 {
    template <typename Number> auto operator()(Number n) const->bool {
        return n == 12;
    }
};

struct Find10 {
    template <typename Number> auto operator()(Number n) const->bool {
        return n == 10;
    }
};

BOOST_AUTO_TEST_CASE(polymorfic_fold) {
    boost::fusion::vector<int, float, long long> v(12, 5.5f, 1ll);

    ds::polymorfic_fold fold{};
    paal::utils::identity_functor id{};
    BOOST_CHECK_EQUAL(12 + 5.5f + 1ll, fold(Sum{}, id, 0, v));
    BOOST_CHECK_EQUAL(12 + 5.5f / 2 + 1ll / 3,
                      fold(strange_weighted_sum{}, id, 0, v));

    ds::Satisfy satisfy{};
    BOOST_CHECK(satisfy(Find12{}, v));
    BOOST_CHECK(!satisfy(Find10{}, v));
}


/*=============================================================================
  Copyright (c) 2018 Nikita Kniazev

  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/detail/lightweight_test.hpp>
#include <boost/spirit/home/x3/support/numeric_utils/extract_int.hpp>
#include <cmath> // for std::pow
#include <cstdio>
#include <iosfwd>
#include <limits>

#ifdef _MSC_VER
# pragma warning(disable: 4127)   // conditional expression is constant
#endif

template <int Min, int Max>
struct custom_int
{
    custom_int() = default;
    constexpr custom_int(int value) : value_{value} {}

    custom_int operator+(custom_int x) const { return value_ + x.value_; }
    custom_int operator-(custom_int x) const { return value_ - x.value_; }
    custom_int operator*(custom_int x) const { return value_ * x.value_; }
    custom_int operator/(custom_int x) const { return value_ / x.value_; }

    custom_int& operator+=(custom_int x) { value_ += x.value_; return *this; }
    custom_int& operator-=(custom_int x) { value_ -= x.value_; return *this; }
    custom_int& operator*=(custom_int x) { value_ *= x.value_; return *this; }
    custom_int& operator/=(custom_int x) { value_ /= x.value_; return *this; }
    custom_int& operator++() { ++value_; return *this; }
    custom_int& operator--() { --value_; return *this; }
    custom_int operator++(int) { return value_++; }
    custom_int operator--(int) { return value_--; }

    custom_int operator+() { return +value_; }
    custom_int operator-() { return -value_; }

    bool operator< (custom_int x) const { return value_ <  x.value_; }
    bool operator> (custom_int x) const { return value_ >  x.value_; }
    bool operator<=(custom_int x) const { return value_ <= x.value_; }
    bool operator>=(custom_int x) const { return value_ >= x.value_; }
    bool operator==(custom_int x) const { return value_ == x.value_; }
    bool operator!=(custom_int x) const { return value_ != x.value_; }

    template <typename Char, typename Traits>
    friend std::basic_ostream<Char, Traits>&
    operator<<(std::basic_ostream<Char, Traits>& os, custom_int x) {
        return os << x.value_;
    }

    static constexpr int max = Max;
    static constexpr int min = Min;

private:
    int value_;
};

namespace utils {

template <int Min, int Max> struct digits;
template <> struct digits<-9,  9>  { static constexpr int r2 = 3, r10 = 1; };
template <> struct digits<-10, 10> { static constexpr int r2 = 3, r10 = 1; };
template <> struct digits<-15, 15> { static constexpr int r2 = 3, r10 = 1; };

}

namespace std {

template <int Min, int Max>
class numeric_limits<custom_int<Min, Max>> : public numeric_limits<int>
{
public:
    static constexpr custom_int<Min, Max> max() noexcept { return Max; }
    static constexpr custom_int<Min, Max> min() noexcept { return Min; }
    static constexpr custom_int<Min, Max> lowest() noexcept { return min(); }
    static_assert(numeric_limits<int>::radix == 2, "hardcoded for digits of radix 2");
    static constexpr int digits = utils::digits<Min, Max>::r2;
    static constexpr int digits10 = utils::digits<Min, Max>::r10;
};

}

namespace x3 = boost::spirit::x3;

template <typename T, int Base, int MaxDigits>
void test_overflow_handling(char const* begin, char const* end, int i)
{
    // Check that parser fails on overflow
    static_assert(std::numeric_limits<T>::is_bounded, "tests prerequest");
    BOOST_ASSERT_MSG(MaxDigits == -1 || static_cast<int>(std::pow(float(Base), MaxDigits)) > T::max,
                     "test prerequest");
    int initial = Base - i % Base; // just a 'random' non-equal to i number
    T x { initial };
    char const* it = begin;
    bool r = x3::extract_int<T, Base, 1, MaxDigits>::call(it, end, x);
    if (T::min <= i && i <= T::max) {
        BOOST_TEST(r);
        BOOST_TEST(it == end);
        BOOST_TEST_EQ(x, i);
    }
    else
        if (MaxDigits == -1) // TODO: Looks like a regression. See #430
    {
        BOOST_TEST(!r);
        BOOST_TEST(it == begin);
    }
}

template <typename T, int Base>
void test_unparsed_digits_are_not_consumed(char const* it, char const* end, int i)
{
    // Check that unparsed digits are not consumed
    static_assert(T::min <= -Base+1, "test prerequest");
    static_assert(T::max >=  Base-1, "test prerequest");
    bool has_sign = *it == '+' || *it == '-';
    auto len = end - it;
    int initial = Base - i % Base; // just a 'random' non-equal to i number
    T x { initial };
    bool r = x3::extract_int<T, Base, 1, 1>::call(it, end, x);
    BOOST_TEST(r);
    if (-Base < i && i < Base) {
        BOOST_TEST(it == end);
        BOOST_TEST_EQ(x, i);
    }
    else {
        BOOST_TEST_EQ(end - it, len - 1 - has_sign);
        BOOST_TEST_EQ(x, i / Base);
    }
}

template <typename T, int Base>
void run_tests(char const* begin, char const* end, int i)
{
    // Check that parser fails on overflow
    test_overflow_handling<T, Base, -1>(begin, end, i);
    // Check that MaxDigits > digits10 behave like MaxDigits=-1
    test_overflow_handling<T, Base, 2>(begin, end, i);
    // Check that unparsed digits are not consumed
    test_unparsed_digits_are_not_consumed<T, Base>(begin, end, i);
}

int main()
{
    for (int i = -30; i <= 30; ++i) {
        char s[4];
        std::snprintf(s, 4, "%d", i);
        auto begin = &s[0], end = begin + std::strlen(begin);

        // log(Base, abs(MinOrMax) + 1) == digits
        run_tests<custom_int<-9, 9>, 10>(begin, end, i);
        // (MinOrMax % Base) == 0
        run_tests<custom_int<-10, 10>, 10>(begin, end, i);
        // (MinOrMax % Base) != 0
        run_tests<custom_int<-15, 15>, 10>(begin, end, i);
    }

    return boost::report_errors();
}


//  Copyright (c) 2001-2011 Hartmut Kaiser
//  Copyright (c) 2011 Michael Caisse
//
//  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/spirit/include/qi.hpp>

namespace qi = boost::spirit::qi;

template <typename T>
struct german_real_policies : qi::real_policies<T>
{
    template <typename Iterator>
    static bool parse_dot(Iterator& first, Iterator const& last)
    {
        if (first == last || *first != ',')
            return false;
        ++first;
        return true;
    }
};

qi::real_parser<double, german_real_policies<double> > const german_double;

///////////////////////////////////////////////////////////////////////////////
int main()
{
    std::string input("123,456");
    std::string::iterator begin = input.begin();
    std::string::iterator end = input.end();

    double value = 0;
    if (!qi::parse(begin, end, german_double, value))
    {
        std::cout << "-------------------------------- \n";
        std::cout << "Parsing failed\n";
        std::cout << "-------------------------------- \n";
    }
    else
    {
        std::cout << "-------------------------------- \n";
        std::cout << "Parsing succeeded, got: " << value << "\n";
        std::cout << "---------------------------------\n";
    }
    return 0;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(aabb_test)
{

  {
    geometry::AABB<V> A;
    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.min()(0), -0.5, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(1), -0.5, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(2), -0.5, 0.01 );

    BOOST_CHECK_CLOSE(A.max()(0), 0.5, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(1), 0.5, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(2), 0.5, 0.01 );
  }

  {
    V const min_coord = V::make(-1.0, -1.0, -1.0);
    V const max_coord = V::make( 1.0,  1.0,  1.0);

    geometry::AABB<V> A = geometry::make_aabb(min_coord, max_coord);

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.min()(0), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(1), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(2), -1.0, 0.01 );

    BOOST_CHECK_CLOSE(A.max()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(1), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(2), 1.0, 0.01 );
  }

  {
    V const min_coord = V::make(-1.0, -1.0, -1.0);
    V const max_coord = V::make( 1.0,  1.0,  1.0);

    geometry::AABB<V> A;

    A.min() = min_coord;
    A.max() = max_coord;

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.min()(0), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(1), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(2), -1.0, 0.01 );

    BOOST_CHECK_CLOSE(A.max()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(1), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(2), 1.0, 0.01 );
  }

  {
    V const min_coord = V::make(-1.0, -1.0, -1.0);
    V const max_coord = V::make( 1.0,  1.0,  1.0);

    geometry::AABB<V> A;

    A.max() = min_coord;
    A.min() = max_coord;

    BOOST_CHECK( geometry::is_valid(A) == false );

    BOOST_CHECK_CLOSE(A.max()(0), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(1), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.max()(2), -1.0, 0.01 );

    BOOST_CHECK_CLOSE(A.min()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(1), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.min()(2), 1.0, 0.01 );
  }

  {
    V const min_coord = V::make(-1.0, -1.0, -1.0);
    V const max_coord = V::make( 1.0,  1.0,  1.0);

    geometry::AABB<V> A;

    A.min() = min_coord;
    A.max() = max_coord;

    geometry::AABB<V> B = A;
    geometry::AABB<V> C(A);

    BOOST_CHECK( geometry::is_valid(B) == true );
    BOOST_CHECK( geometry::is_valid(C) == true );

    BOOST_CHECK_CLOSE(B.min()(0), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.min()(1), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.min()(2), -1.0, 0.01 );

    BOOST_CHECK_CLOSE(B.max()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.max()(1), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.max()(2), 1.0, 0.01 );

    BOOST_CHECK_CLOSE(C.min()(0), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.min()(1), -1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.min()(2), -1.0, 0.01 );

    BOOST_CHECK_CLOSE(C.max()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.max()(1), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.max()(2), 1.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Sphere.hpp"

#include <ostream>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

std::ostream & operator<<(std::ostream & os, Sphere & sph)
{
    os << "Sphere radius " << sph.radius << std::endl;
    os << "Sphere center\n" << sph.center;

    return os;
}

void transfer_spheres(const std::vector<Sphere> & spheres,
                             Eigen::Matrix3Xd & sphereCenter, Eigen::VectorXd & sphereRadius)
{
    size_t nSpheres = spheres.size();
    sphereCenter.resize(Eigen::NoChange, nSpheres);
    sphereRadius.resize(nSpheres);
    for (size_t i = 0; i < nSpheres; ++i) {
        sphereCenter.col(i) = spheres[i].center;
        sphereRadius(i) = spheres[i].radius;
    }
}


#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/filesystem.hpp>

#include <CppADCodeGenEigenPy/ADModel.h>
#include <CppADCodeGenEigenPy/CompiledModel.h>
#include <CppADCodeGenEigenPy/Util.h>

#include "testing/models/BasicTestModel.h"

namespace CppADCodeGenEigenPy {
namespace BasicModelTest {

class BasicTestModelFixture : public ::testing::Test {
   protected:
    using Vector = CompiledModel<Scalar>::Vector;
    using Matrix = CompiledModel<Scalar>::Matrix;

    static void SetUpTestSuite() {
        // Compile and load our model
        boost::filesystem::create_directories(DIRECTORY_PATH);
        ad_model_ptr_.reset(new BasicTestModel<Scalar>());
        ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH,
                               DerivativeOrder::Second);
        compiled_model_ptr_.reset(
            new CompiledModel<Scalar>(MODEL_NAME, LIB_GENERIC_PATH));
    }

    static void TearDownTestSuite() {
        // Delete the compiled shared object.
        boost::filesystem::remove_all(DIRECTORY_PATH);
    }

    static std::unique_ptr<ADModel<Scalar>> ad_model_ptr_;
    static std::unique_ptr<CompiledModel<Scalar>> compiled_model_ptr_;
};

std::unique_ptr<ADModel<Scalar>> BasicTestModelFixture::ad_model_ptr_ = nullptr;
std::unique_ptr<CompiledModel<Scalar>>
    BasicTestModelFixture::compiled_model_ptr_ = nullptr;

TEST_F(BasicTestModelFixture, CreatesSharedLib) {
    EXPECT_TRUE(boost::filesystem::exists(LIB_REAL_PATH))
        << "Shared library file " << LIB_REAL_PATH << " not found.";
}

TEST_F(BasicTestModelFixture, Dimensions) {
    // test that the model shape is correct
    EXPECT_EQ(compiled_model_ptr_->get_input_size(), NUM_INPUT)
        << "Input size is incorrect.";
    EXPECT_EQ(compiled_model_ptr_->get_output_size(), NUM_OUTPUT)
        << "Output size is incorrect.";

    // generate an input with a size too large
    Vector input = Vector::Ones(NUM_INPUT + 1);

    EXPECT_THROW(compiled_model_ptr_->evaluate(input), std::runtime_error)
        << "Evaluate with input of wrong size did not throw.";
    EXPECT_THROW(compiled_model_ptr_->jacobian(input), std::runtime_error)
        << "Jacobian with input of wrong size did not throw.";
    EXPECT_THROW(compiled_model_ptr_->hessian(input, 0), std::runtime_error)
        << "Hessian with input of wrong size did not throw.";
}

TEST_F(BasicTestModelFixture, Evaluation) {
    Vector input = Vector::Ones(NUM_INPUT);

    Vector output_expected = evaluate<Scalar>(input);
    Vector output_actual = compiled_model_ptr_->evaluate(input);
    EXPECT_TRUE(output_actual.isApprox(output_expected))
        << "Function evaluation is incorrect.";
}

TEST_F(BasicTestModelFixture, Jacobian) {
    Vector input = Vector::Ones(NUM_INPUT);

    Matrix J_expected(NUM_OUTPUT, NUM_INPUT);
    J_expected.setZero();
    J_expected.diagonal() << 2, 2, 2;
    Matrix J_actual = compiled_model_ptr_->jacobian(input);

    EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect.";
}

TEST_F(BasicTestModelFixture, Hessian) {
    Vector input = Vector::Ones(NUM_INPUT);

    // Hessian of all output dimensions should be zero
    Matrix H_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT);
    Matrix H0_actual = compiled_model_ptr_->hessian(input, 0);
    Matrix H1_actual = compiled_model_ptr_->hessian(input, 1);
    Matrix H2_actual = compiled_model_ptr_->hessian(input, 2);

    EXPECT_TRUE(H0_actual.isApprox(H_expected))
        << "Hessian for dim 0 is incorrect.";
    EXPECT_TRUE(H1_actual.isApprox(H_expected))
        << "Hessian for dim 1 is incorrect.";
    EXPECT_TRUE(H2_actual.isApprox(H_expected))
        << "Hessian for dim 2 is incorrect.";

    EXPECT_THROW(compiled_model_ptr_->hessian(input, NUM_OUTPUT),
                 std::runtime_error)
        << "Hessian with too-large output_dim did not throw.";
}

}  // namespace BasicModelTest
}  // namespace CppADCodeGenEigenPy


#include <algorithm>
#include <vector>

#include <crypto++/tiger.h>
#include <boost/test/unit_test.hpp>

#include "bithorded/lib/treestore.hpp"
#include "bithorded/lib/hashtree.hpp"

#include "test_storage.hpp"

using namespace std;

typedef HashNode< CryptoPP::Tiger > MyNode;
typedef TestStorage< MyNode > Storage;
typedef HashTree< Storage > TigerTree;

std::string tree_hasher(const std::string& input) {
	std::string res;
	typedef TreeHasher< CryptoPP::Tiger > Hasher;
	byte buf[Hasher::DigestSize];
	Hasher::rootDigest((const byte*)input.c_str(), input.length(), buf);
	CryptoPP::StringSource pipe(buf, Hasher::DigestSize, false,
		new RFC4648Base32Encoder(
			new CryptoPP::StringSink(res)));
	pipe.PumpAll();
	return res;
}

BOOST_AUTO_TEST_CASE( tree_hash_test )
{
	BOOST_CHECK_EQUAL( tree_hasher("A"), "F33GDTSNFCYLSQSR32XFIH3DIDBSBF4GRLU76VA" );
	BOOST_CHECK_EQUAL( tree_hasher(string(1024, 'A')), "L66Q4YVNAFWVS23X2HJIRA5ZJ7WXR3F26RSASFA" );
	BOOST_CHECK_EQUAL( tree_hasher(string(1025, 'A')), "PZMRYHGY6LTBEH63ZWAHDORHSYTLO4LEFUIKHWY" );
	BOOST_CHECK_EQUAL( tree_hasher(string(2048, 'A')), "FSINHKGFD6E3PHTXSA5EATMEO7IND3ATJDSH45A" );
	BOOST_CHECK_EQUAL( tree_hasher(string(2049, 'A')), "2IFFIJQ22FKZA3NCSVOQHPVJVNPJKTGDKOB3LTI" );
	BOOST_CHECK_EQUAL( tree_hasher(string(4000, 'A')), "5CF7JLZQNXHDANLGXZBBWMX6ZYVSDUD3C5SMOXI" );
	BOOST_CHECK_EQUAL( tree_hasher(string(5000, 'A')), "UUP5PDB4H3O6DWLTNGDC6RO27HK5IYSEFPE2LLI" );
	BOOST_CHECK_EQUAL( tree_hasher(string(87234, 'A')), "5V7AM5PT6PVGTCWITETZUFPBTCDK2DPHBJMTFWI" );
}

BOOST_AUTO_TEST_CASE( hashtree_random_sequence )
{
	const uint LEAVES = 7;
	Storage store(treesize(LEAVES));
	const size_t blockSize = 4096;
	TigerTree tree(store, 2);

	byte block[blockSize];
	bzero(block, sizeof(block));

	auto root = tree.getRoot();

	tree.setData(0*blockSize, block, sizeof(block));
	tree.setData(1*blockSize, block, sizeof(block));

	tree.setData(6*blockSize, block, sizeof(block));

	tree.setData(4*blockSize, block, sizeof(block));
	tree.setData(5*blockSize, block, sizeof(block));
	tree.setData(3*blockSize, block, sizeof(block));

	BOOST_CHECK_EQUAL( root->state, MyNode::State::EMPTY);

	tree.setData(2*blockSize, block, sizeof(block));

	BOOST_CHECK_EQUAL( root->state, MyNode::State::SET );
	BOOST_CHECK_EQUAL( root->base32Digest(), "J7BVBFH4WRRVYPDVCZTIGVFABQSIDUTEEQJQFNQ" );
}

string rootBase32(std::string input) {
	size_t blockSize = 4096;
	uint LEAVES = (input.size() + blockSize - 1) / blockSize;
	Storage store(treesize(LEAVES));
	TigerTree tree(store, 2);

	const byte* data = (const byte*) input.c_str();
	for (size_t i=0; i < input.size(); i += blockSize) {
		size_t bl = std::min(input.size() - i, blockSize);
		tree.setData(i, data+i, bl);
	}

	auto root = tree.getRoot();

	BOOST_CHECK_EQUAL( root->state, MyNode::State::SET );
	return root->base32Digest();
}

BOOST_AUTO_TEST_CASE( test_vectors )
{
	BOOST_CHECK_EQUAL( rootBase32("A"), "F33GDTSNFCYLSQSR32XFIH3DIDBSBF4GRLU76VA" );
	BOOST_CHECK_EQUAL( rootBase32(string(1024, 'A')), "L66Q4YVNAFWVS23X2HJIRA5ZJ7WXR3F26RSASFA" );
	BOOST_CHECK_EQUAL( rootBase32(string(1025, 'A')), "PZMRYHGY6LTBEH63ZWAHDORHSYTLO4LEFUIKHWY" );
	BOOST_CHECK_EQUAL( rootBase32(string(2048, 'A')), "FSINHKGFD6E3PHTXSA5EATMEO7IND3ATJDSH45A" );
	BOOST_CHECK_EQUAL( rootBase32(string(2049, 'A')), "2IFFIJQ22FKZA3NCSVOQHPVJVNPJKTGDKOB3LTI" );
	BOOST_CHECK_EQUAL( rootBase32(string(4000, 'A')), "5CF7JLZQNXHDANLGXZBBWMX6ZYVSDUD3C5SMOXI" );
	BOOST_CHECK_EQUAL( rootBase32(string(5000, 'A')), "UUP5PDB4H3O6DWLTNGDC6RO27HK5IYSEFPE2LLI" );
	BOOST_CHECK_EQUAL( rootBase32(string(87234, 'A')), "5V7AM5PT6PVGTCWITETZUFPBTCDK2DPHBJMTFWI" );
}