/**
 * Generate ranges and relfectance from poses
 */
#include "OccupancyGrid/occgrid.hpp"
#include <opencv2/opencv.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include "OccupancyGrid/forward_sensor_model.h"
#include "OccupancyGrid/cvmat_serialization.h"
#include <boost/program_options.hpp>
namespace po = boost::program_options;

int main(int argc, const char *argv[])
{

  // parse arguments ///////////////////////////////////////////
  // Declare the supported options.
  po::options_description desc("Visualize data");
  desc.add_options()
    ("help", "produce help message")
    ("width", po::value<double>()->required(), "Width ")
    ("height", po::value<double>()->required(), "Height ")
    ("dir", po::value<std::string>()->default_value("."), "Data directory")
;

  po::positional_options_description pos;
  pos.add("width", 1);
  pos.add("height", 1);
  pos.add("dir", 1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
  po::notify(vm);

  double width = vm["width"].as<double>();
  double height = vm["height"].as<double>();
  std::string datadirectory = vm["dir"].as<std::string>();
  // end of parse arguments ////////////////////////////////////
  cv::Mat laser_pose, laser_ranges, scan_angles;
  loadMat(laser_pose, datadirectory + "/laser_pose_all.bin");
  loadMat(laser_ranges, datadirectory + "/laser_range_all.bin");
  loadMat(scan_angles, datadirectory + "/scan_angles_all.bin");
  cv::Mat laser_reflectance(laser_ranges.rows, laser_ranges.cols, CV_8U);
  std::string floorplanfile = datadirectory + "/floorplan.png";
  cv::Mat floorplan = cv::imread(floorplanfile, cv::IMREAD_GRAYSCALE);
    if(! floorplan.data )                              // Check for invalid input
    {
      std::cout <<  "Could not open or find the image" << std::endl ;
        return -1;
    }
  cv::transpose(floorplan, floorplan);
  cv::flip(floorplan, floorplan, 1);
  cv::Vec2d size_bitmap(width, height);
  cv::Vec2d margin(1, 1);
  cv::Vec2d min_pt(- margin(0) - size_bitmap(0)/2, - margin(1) - size_bitmap(1)/2);
  double max_range = 8;
  cv::Vec2i gridsize(floorplan.size[0], floorplan.size[1]);
  cv::Vec2d cellsize;
  cv::divide(size_bitmap , gridsize, cellsize);
  //std::cout << cellsize(0) << cellsize(1) << std::endl;
  cv::Vec2i ncells;
  cv::divide(min_pt, cellsize, ncells, -2);

  OccupancyGrid2D<double, int> map(
      min_pt(0),
      min_pt(1),
      cellsize(0),
      cellsize(1),
      ncells(0),
      ncells(1));
  // initialize map with occupancy with floorplan
  for (int r = 0; r < ncells(0); ++r) {
    for (int c = 0; c < ncells(1); ++c) {
      int fp_r = r - margin(0) / cellsize(0);
      int fp_c = c - margin(1) / cellsize(1);
      if ((0 <= fp_r) && (fp_r < floorplan.rows)
          && (0 <= fp_c) && (fp_c < floorplan.cols)) {
        map.og_.at<uint8_t>(r, c) = (floorplan.at<uint8_t>(fp_r, fp_c) > 127) ? map.FREE : map.OCCUPIED;
      } else {
        map.og_.at<uint8_t>(r, c) = map.OCCUPIED;
      }
    }
  }

  boost::mt19937 gen;
  boost::normal_distribution<> norm_dist(1, NOISE_VARIANCE);
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > norm_rand(gen, norm_dist);

  for (int r = 0; r < scan_angles.rows; r++) {
    double* pose = laser_pose.ptr<double>(r);
    double* angles = scan_angles.ptr<double>(r);
    double robot_angle = pose[2];
    for (int c = 0; c < scan_angles.cols; c++) {
      double total_angle = robot_angle + angles[c];
      cv::Vec2d final_pos;
      bool refl;
      double range = map.ray_trace(pose[0], pose[1], total_angle, max_range, final_pos, refl);
      range *= norm_rand();
      laser_ranges.at<double>(r, c) = range;
      laser_reflectance.at<uint8_t>(r, c) = (uint8_t) refl;
    }

    // draw input
    cv::Mat visin;
    cv::cvtColor(map.og_, visin, cv::COLOR_GRAY2BGR);
    cv::Vec2d position(pose[0], pose[1]);
    map.draw_lasers(visin, position, robot_angle, angles,
        laser_ranges.ptr<double>(r),
        laser_reflectance.ptr<uint8_t>(r),
        scan_angles.cols,
        CV_RGB(0, 255, 0));
    cv::imshow("c", visin);
    cv::waitKey(33);
  }
  saveMat(laser_ranges, "laser_range_all.bin");
  saveMat(laser_reflectance, "laser_reflectance_all.bin");
}


// Boost.Geometry
//
// Copyright (c) 2010-2021 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to 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)
//
// wxWidgets World Mapper example
//
// It will show a basic wxWidgets window, displaying world countries,
// highlighting the country under the mouse, and indicating position
// of the mouse in latitude/longitude and in pixels.

// To compile this program:
// Install wxWidgets (if not done before)
//   export BOOST_ROOT=.....
//   export WX_ROOT=.... (for example /home/myname/mylib/wxWidgets/Linux/x86_64)
//   mkdir build
//   cd build
//   cmake .. -G Ninja
//   ninja
// If necessary, CMakeLists.txt should be adapted, the options for wx
// are provided by "wx-config --cxxflags" and "... --libs"
// and might need a change in CMakeLists.txt

//#define EXAMPLE_WX_USE_GRAPHICS_CONTEXT 1

#include <fstream>
#include <sstream>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

#include "wx/wx.h"
#include "wx/math.h"
#include "wx/stockitem.h"

#ifdef EXAMPLE_WX_USE_GRAPHICS_CONTEXT
#include "wx/graphics.h"
#include "wx/dcgraph.h"
#endif

using point_2d = boost::geometry::model::d2::point_xy<double>;
using country_type = boost::geometry::model::multi_polygon
    <
        boost::geometry::model::polygon<point_2d>
    >;

// Adapt wxWidgets points to Boost.Geometry points such that they can be used
// in e.g. transformations (see below)
BOOST_GEOMETRY_REGISTER_POINT_2D(wxPoint, int, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_POINT_2D(wxRealPoint, double, cs::cartesian, x, y)

// ----------------------------------------------------------------------------
// Read an ASCII file containing WKT's
// ----------------------------------------------------------------------------
template <typename Geometry, typename Box>
inline void read_wkt(std::string const& filename, std::vector<Geometry>& geometries, Box& box)
{
    std::ifstream cpp_file(filename.c_str());
    if (cpp_file.is_open())
    {
        while (! cpp_file.eof() )
        {
            std::string line;
            std::getline(cpp_file, line);
            if (! line.empty())
            {
                Geometry geometry;
                boost::geometry::read_wkt(line, geometry);
                geometries.push_back(geometry);
                boost::geometry::expand(box, boost::geometry::return_envelope<Box>(geometry));
            }
        }
    }
}

// ----------------------------------------------------------------------------
class HelloWorldFrame: public wxFrame
{
public:
    HelloWorldFrame(wxFrame *frame, wxString const& title, wxPoint const& pos, wxSize const& size);

    void OnCloseWindow(wxCloseEvent& );
    void OnExit(wxCommandEvent& );

    DECLARE_EVENT_TABLE()
};

// ----------------------------------------------------------------------------
class HelloWorldCanvas: public wxWindow
{
public:
    HelloWorldCanvas(wxFrame *frame);

private:
    void DrawCountries(wxDC& dc);
    void DrawCountry(wxDC& dc, country_type const& country);

    void OnPaint(wxPaintEvent& );
    void OnMouseMove(wxMouseEvent&);

    using map_transformer_type = boost::geometry::strategy::transform::map_transformer
        <
            double, 2, 2,
            true, true
        >;

    using inverse_transformer_type = boost::geometry::strategy::transform::inverse_transformer
        <
            double, 2, 2
        >;

    std::shared_ptr<map_transformer_type> m_map_transformer;
    std::shared_ptr<inverse_transformer_type> m_inverse_transformer;

    boost::geometry::model::box<point_2d> m_box;
    std::vector<country_type> m_countries;
    int m_focus = -1;

    wxBrush m_orange = wxBrush(wxColour(255, 128, 0), wxBRUSHSTYLE_SOLID);
    wxBrush m_blue = wxBrush(wxColour(0, 128, 255), wxBRUSHSTYLE_SOLID);
    wxBrush m_green = wxBrush(wxColour(0, 255, 0), wxBRUSHSTYLE_SOLID);

    wxFrame* m_owner = nullptr;

DECLARE_EVENT_TABLE()
};

// ----------------------------------------------------------------------------
class HelloWorldApp: public wxApp
{
public:
    bool OnInit()
    {
        // Create the main frame window
        HelloWorldFrame *frame = new HelloWorldFrame(NULL, _T("Boost.Geometry for wxWidgets - Hello World!"), wxDefaultPosition, wxSize(640, 480));

        wxMenu *file_menu = new wxMenu;
        file_menu->Append(wxID_EXIT, wxGetStockLabel(wxID_EXIT));
        wxMenuBar* menuBar = new wxMenuBar;
        menuBar->Append(file_menu, _T("&File"));
        frame->SetMenuBar(menuBar);

        int width, height;
        frame->GetClientSize(&width, &height);

        (void) new HelloWorldCanvas(frame);

        // Show the frame
        frame->Show(true);

        return true;
    }
};

// ----------------------------------------------------------------------------
HelloWorldFrame::HelloWorldFrame(wxFrame *frame, wxString const& title, wxPoint const& pos, wxSize const& size)
    : wxFrame(frame, wxID_ANY, title, pos, size, wxDEFAULT_FRAME_STYLE | wxFULL_REPAINT_ON_RESIZE )
{
    CreateStatusBar(2);
}

void HelloWorldFrame::OnExit(wxCommandEvent& )
{
    this->Destroy();
}

void HelloWorldFrame::OnCloseWindow(wxCloseEvent& )
{
    static bool destroyed = false;
    if (! destroyed)
    {
        this->Destroy();
        destroyed = true;
    }
}

// ----------------------------------------------------------------------------
HelloWorldCanvas::HelloWorldCanvas(wxFrame *frame)
    : wxWindow(frame, wxID_ANY)
    , m_owner(frame)
{
    boost::geometry::assign_inverse(m_box);
    read_wkt("../data/world.wkt", m_countries, m_box);
}

void HelloWorldCanvas::OnMouseMove(wxMouseEvent &event)
{
    if (m_inverse_transformer)
    {
        // Boiler-plate wxWidgets code
        wxClientDC dc(this);
        PrepareDC(dc);
        m_owner->PrepareDC(dc);

        // Transform the point opn the screen back to Lon/Lat
        point_2d point;
        boost::geometry::transform(event.GetPosition(), point,
                                   *m_inverse_transformer);

        // Determine selected object
        int i = 0;
        int previous_focus = m_focus;
        m_focus = -1;
        for (country_type const& country : m_countries)
        {
            if (boost::geometry::within(point, country))
            {
                m_focus = i;
                break;
            }
            i++;
        }

        // On change:
        if (m_focus != previous_focus)
        {
            // Undraw old focus
            if (previous_focus >= 0)
            {
                dc.SetBrush(m_green);
                DrawCountry(dc, m_countries[previous_focus]);
            }
            // Draw new focus
            if (m_focus >= 0)
            {
                dc.SetBrush(m_orange);
                DrawCountry(dc, m_countries[m_focus]);
            }
        }

        // Create a string and set it in the status text
        std::ostringstream out;
        out << "Position: " << point.x() << ", " << point.y();
        m_owner->SetStatusText(wxString(out.str().c_str(), wxConvUTF8));
    }
}

void HelloWorldCanvas::OnPaint(wxPaintEvent& )
{
    if (m_countries.empty())
    {
        return;
    }

#if defined(EXAMPLE_WX_USE_GRAPHICS_CONTEXT)
    wxPaintDC pdc(this);
    wxGCDC gdc(pdc);
    wxDC& dc = (wxDC&) gdc;
#else
    wxPaintDC dc(this);
#endif

    PrepareDC(dc);

    static bool running = false;
    if (! running)
    {
        running = true;

        // Update the transformers
        wxSize sz = dc.GetSize();
        m_map_transformer.reset(new map_transformer_type(m_box, sz.x, sz.y));
        m_inverse_transformer.reset(new inverse_transformer_type(*m_map_transformer));

        DrawCountries(dc);

        running = false;
    }
}

void HelloWorldCanvas::DrawCountries(wxDC& dc)
{
    namespace bg = boost::geometry;

    dc.SetBackground(m_blue);
    dc.Clear();

    for (country_type const& country :  m_countries)
    {
        dc.SetBrush(m_green);
        DrawCountry(dc, country);
    }
    if (m_focus != -1)
    {
        dc.SetBrush(m_orange);
        DrawCountry(dc, m_countries[m_focus]);
    }
}

void HelloWorldCanvas::DrawCountry(wxDC& dc, country_type const& country)
{
    for (auto const& poly : country)
    {
        // Use only exterior ring, holes are (for the moment) ignored.
        // This would need a holey-polygon compatible wx object

        // Define a Boost.Geometry ring of wxPoints
        // Behind the scenes that is a vector, and a vector has .data(),
        // can be used for the *wxPoint pointer needed for wxWidget DrawPolygon
        boost::geometry::model::ring<wxPoint> ring;
        boost::geometry::transform(boost::geometry::exterior_ring(poly), ring,
                                   *m_map_transformer);
        dc.DrawPolygon(ring.size(), ring.data());
    }
}

// ----------------------------------------------------------------------------

BEGIN_EVENT_TABLE(HelloWorldFrame, wxFrame)
    EVT_CLOSE(HelloWorldFrame::OnCloseWindow)
    EVT_MENU(wxID_EXIT, HelloWorldFrame::OnExit)
END_EVENT_TABLE()

BEGIN_EVENT_TABLE(HelloWorldCanvas, wxWindow)
    EVT_PAINT(HelloWorldCanvas::OnPaint)
    EVT_MOTION(HelloWorldCanvas::OnMouseMove)
END_EVENT_TABLE()

IMPLEMENT_APP(HelloWorldApp)


#define BOOST_TEST_MODULE Lennard-Jones collisions test
#include <boost/test/included/unit_test.hpp>

#include <cstddef>
#include <iostream>
#include <iterator>
#include <utility>

#include <pastel/geometry/get.hpp>
#include <pastel/geometry/distance.hpp>
#include <pastel/particle/tags.hpp>
#include <pastel/container/aligned_simple_particles.hpp>
#include <pastel/container/get.hpp>
#include <pastel/container/maximal_speed.hpp>
#include <pastel/container/meta/point_of.hpp>
#include <pastel/container/meta/vector_of.hpp>
#include <pastel/neighbor/brute_force_neighbor_list.hpp>
#include <pastel/neighbor/brute_force_updater.hpp>
#include <pastel/neighbor/inexclusive_cells_neighbor_list.hpp>
#include <pastel/neighbor/inexclusive_cells_updater.hpp>
#include <pastel/force/lennard_jones.hpp>
#include <pastel/system/simple_system.hpp>
#include <pastel/system/particles.hpp>
#include <pastel/system/neighbor_list.hpp>
#include <pastel/system/boundary_neighbor_list.hpp>
#include <pastel/system/update_forces.hpp>
#include <pastel/system/periodic_boundary.hpp>
#include <pastel/system/initialize.hpp>
#include <pastel/system/total_kinetic_energy.hpp>
#include <pastel/system/total_potential_energy.hpp>
#include <pastel/system/total_boundary_potential_energy.hpp>
#include <pastel/integrate/vverlet/update.hpp>
#include <pastel/utility/pair.hpp>

BOOST_AUTO_TEST_CASE(lennard_jones_collisions_test, * boost::unit_test::tolerance(0.001))
{
  constexpr auto speed = 1.0;
  constexpr auto diameter = 1.0;
  constexpr auto cutoff_length = 3.0 * diameter;
  constexpr auto search_length = 4.0 * diameter;
  constexpr auto lower_bound_1d = -10.0 * diameter;
  constexpr auto upper_bound_1d = +10.0 * diameter;
  using force_type = pastel::force::lennard_jones<true>;
  auto force = force_type{1.0, diameter, cutoff_length};
  constexpr auto time_step = 0.001;
  constexpr auto num_steps = 40000u;

  using particles_type = pastel::container::aligned_simple_particles<2u>;
  using point_type = typename pastel::container::meta::point_of<particles_type>::type;
  using vector_type = typename pastel::container::meta::vector_of<particles_type>::type;
  {
  constexpr auto boundary_width = search_length;
  using x_boundary_type = pastel::system::periodic_boundary<>;
  using y_boundary_type = pastel::system::periodic_boundary<>;
  auto const x_boundary = x_boundary_type{lower_bound_1d, upper_bound_1d, boundary_width};
  auto const y_boundary = y_boundary_type{lower_bound_1d, upper_bound_1d, boundary_width};
  using boundary_type = std::tuple<x_boundary_type, y_boundary_type>;
  auto const boundary = boundary_type{x_boundary, y_boundary};

  auto particles = particles_type{};
  particles.reserve(2u);
  particles.emplace_back(point_type{lower_bound_1d/2.0, 0.0}, vector_type{+speed, 0.0}, vector_type{});
  particles.emplace_back(point_type{upper_bound_1d/2.0, 0.0}, vector_type{-speed, 0.0}, vector_type{});

  // Brute Force Neighbor List
  std::cout << "[Brute Force]\n" << std::flush;
  using neighbor_list_type = pastel::neighbor::brute_force_neighbor_list<force_type>;
  auto neighbor_list
    = neighbor_list_type{
        particles, std::move(force),
        pastel::neighbor::brute_force_updater<double>{
          search_length, cutoff_length, time_step, pastel::container::maximal_speed(particles)}};

  using system_type = pastel::system::simple_system<particles_type, neighbor_list_type, boundary_type>;
  auto system
    = system_type{
        std::move(particles), std::move(neighbor_list),
        std::move(boundary), time_step};

  auto const initial_total_energy
    = pastel::system::total_kinetic_energy<0u>(system)
      + pastel::system::total_potential_energy<0u>(system)
      + pastel::system::total_boundary_potential_energy<0u>(system);
  std::cout << "E_0 = " << initial_total_energy << std::endl;
  for (auto step = 1u; step <= num_steps; ++step)
    pastel::integrate::vverlet::update(system, time_step);
  /*
  for (auto step = 1u; step <= num_steps; ++step)
  {
    pastel::integrate::vverlet::update(system, time_step);
    std::cout
      << time_step*step
      << ' ' << pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 0u))
      << ' ' << pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 1u))
      << std::endl;
  }
  */

  auto const total_energy
    = pastel::system::total_kinetic_energy<0u>(system)
      + pastel::system::total_potential_energy<0u>(system)
      + pastel::system::total_boundary_potential_energy<0u>(system);
  std::cout << "E = " << total_energy << std::endl;
  BOOST_TEST(total_energy == initial_total_energy);
  }

  {
  constexpr auto num_cells_1d = 4u;
  constexpr auto boundary_width = (upper_bound_1d - lower_bound_1d) / static_cast<decltype(lower_bound_1d)>(num_cells_1d);
  using x_boundary_type = pastel::system::periodic_boundary<>;
  using y_boundary_type = pastel::system::periodic_boundary<>;
  auto const x_boundary = x_boundary_type{lower_bound_1d, upper_bound_1d, boundary_width};
  auto const y_boundary = y_boundary_type{lower_bound_1d, upper_bound_1d, boundary_width};
  using boundary_type = std::tuple<x_boundary_type, y_boundary_type>;
  auto const boundary = boundary_type{x_boundary, y_boundary};

  auto particles = particles_type{};
  particles.reserve(2u);
  particles.emplace_back(point_type{lower_bound_1d/2.0, 0.0}, vector_type{+speed, 0.0}, vector_type{});
  particles.emplace_back(point_type{upper_bound_1d/2.0, 0.0}, vector_type{-speed, 0.0}, vector_type{});

  // Inexclusive Cells Neighbor List
  std::cout << "[Inexclusive Cells]\n" << std::flush;
  auto const lower_bound = point_type{lower_bound_1d, lower_bound_1d};
  auto const upper_bound = point_type{upper_bound_1d, upper_bound_1d};
  using neighbor_list_type = pastel::neighbor::inexclusive_cells_neighbor_list<force_type, point_type>;
  auto neighbor_list
    = neighbor_list_type{
        particles, std::move(force),
        pastel::neighbor::inexclusive_cells_updater<point_type>{
          search_length, cutoff_length, time_step, pastel::container::maximal_speed(particles),
          lower_bound, upper_bound, num_cells_1d, num_cells_1d}};

  using system_type = pastel::system::simple_system<particles_type, neighbor_list_type, boundary_type>;
  auto system
    = system_type{
        std::move(particles), std::move(neighbor_list),
        std::move(boundary), time_step};

  auto const initial_total_energy
    = pastel::system::total_kinetic_energy<0u>(system)
      + pastel::system::total_potential_energy<0u>(system)
      + pastel::system::total_boundary_potential_energy<0u>(system);
  std::cout << "E_0 = " << initial_total_energy << std::endl;
  for (auto step = 1u; step <= num_steps; ++step)
    pastel::integrate::vverlet::update(system, time_step);
  /*
  for (auto step = 1u; step <= num_steps; ++step)
  {
    pastel::integrate::vverlet::update(system, time_step);
    std::cout
      << time_step*step
      << ' ' << pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 0u))
      << ' ' << pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 1u))
      << std::endl;
  }
  */

  auto const total_energy
    = pastel::system::total_kinetic_energy<0u>(system)
      + pastel::system::total_potential_energy<0u>(system)
      + pastel::system::total_boundary_potential_energy<0u>(system);
  std::cout << "E = " << total_energy << std::endl;
  BOOST_TEST(total_energy == initial_total_energy);
  }
}


#define BOOST_TEST_MODULE pcraster func summary_statistics
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "func_summarystatistics.h"

BOOST_AUTO_TEST_CASE(mean_)
{
  using namespace func;

  {
    float sources[] = { 100.0f, 200.0f, 300.0f };

    {
      double result;

      mean<float, double>(&sources[0], 3, result);
      BOOST_CHECK(dal::comparable<double>(result, 200.0));
    }

    {
      float result;

      mean<float, float>(&sources[0], 3, result);
      BOOST_CHECK(dal::comparable<float>(result, 200.0f));

      pcr::setMV(sources[0]);
      mean<float, float>(&sources[0], 3, result);
      BOOST_CHECK(dal::comparable<float>(result, 250.0f));

      pcr::setMV(sources[1]);
      mean<float, float>(&sources[0], 3, result);
      BOOST_CHECK(dal::comparable<float>(result, 300.0f));

      pcr::setMV(sources[2]);
      mean<float, float>(&sources[0], 3, result);
      BOOST_CHECK(pcr::isMV(result));
    }
  }

  {
    std::vector<float> sources;
    sources.push_back(100.0f);
    sources.push_back(200.0f);
    sources.push_back(300.0f);

    {
      double result;

      mean<std::vector<float>::const_iterator, double>(
         sources.begin(), sources.end(), result);
      BOOST_CHECK(dal::comparable<double>(result, 200.0));
    }

    {
      float result;

      mean<std::vector<float>::const_iterator, float>(
         sources.begin(), sources.end(), result);
      BOOST_CHECK(dal::comparable<float>(result, 200.0f));

      pcr::setMV(sources[0]);
      mean<std::vector<float>::const_iterator, float>(
         sources.begin(), sources.end(), result);
      BOOST_CHECK(dal::comparable<float>(result, 250.0f));

      pcr::setMV(sources[1]);
      mean<std::vector<float>::const_iterator, float>(
         sources.begin(), sources.end(), result);
      BOOST_CHECK(dal::comparable<float>(result, 300.0f));

      pcr::setMV(sources[2]);
      mean<std::vector<float>::const_iterator, float>(
         sources.begin(), sources.end(), result);
      BOOST_CHECK(pcr::isMV(result));
    }
  }
}


/*
 * Author: Janis Born
 */

#include "VertexToPointMap.hh"

#include <HomologyInference/Extern/ACG_BSP/Geometry/Algorithms.hh>
#include <HomologyInference/Extern/ACG_BSP/Geometry/bsp/TriangleBSPT.hh>
#include <HomologyInference/HomologyBasis.hh>
#include <HomologyInference/Utils/CotanWeights.hh>

#include <Eigen/SparseCholesky>
#include <memory>

namespace HomologyInference
{

using BSP = OpenMeshTriangleBSPT<TriMesh>;
using BSPptr = std::unique_ptr<BSP>;

BSPptr build_bsp(
        const TriMesh& _mesh)
{
    BSPptr bsp(new BSP(_mesh));
    bsp->reserve(_mesh.n_faces());
    for (const FH fh : _mesh.faces())
        bsp->push_back(fh);
    bsp->build(10, 100); // max vertices per leaf 10, max depth 100

    return bsp;
}

bool is_dense(const VertexToPointMap& _vtpm)
{
    for (const auto& bary : _vtpm.container_)
        if (!bary.is_valid())
            return false;
    return true;
}

VertexToPointMap generate_vtpm_by_projection(
        const TriMesh& _mesh_A,
        const TriMesh& _mesh_B)
{
    BSPptr bspt_B = build_bsp(_mesh_B);

    VertexToPointMap vtpm(_mesh_A);
    for (const auto& vh_A : _mesh_A.vertices())
    {
        // Query point
        const auto p_A = to_acg3(_mesh_A.point(vh_A));
        // Closest triangle
        const FH fh_B = bspt_B->nearest(p_A).handle;
        ISM_ASSERT(fh_B.is_valid());
        const auto c_B = _mesh_B.fv_range(fh_B).to_array<3>([&](auto _vh_B){ return to_acg3(_mesh_B.point(_vh_B)); });
        // Point on triangle
        const Vec3d p_B = to_eigen(ACG::Geometry::closestPointTri(p_A, c_B[0], c_B[1], c_B[2]));
        // Barycentric point
        vtpm[vh_A] = BarycentricPoint(p_B, fh_B, _mesh_B);
        ISM_ASSERT_FINITE(vtpm[vh_A].alpha());
        ISM_ASSERT_FINITE(vtpm[vh_A].beta());
    }
    return vtpm;
}

VertexToPointMap
generate_vtpm_from_landmarks(
        const TriMesh& _mesh_A,
        const TriMesh& _mesh_B,
        const std::vector<VH>& _landmarks_A,
        const std::vector<VH>& _landmarks_B)
{
    ISM_ASSERT_EQ(_landmarks_A.size(), _landmarks_B.size());
    VertexToPointMap vtpm(_mesh_A, BarycentricPoint());
    const size_t n = _landmarks_A.size();
    for (size_t i = 0; i < n; ++i)
    {
        const VH& vh_A = _landmarks_A[i];
        const VH& vh_B = _landmarks_B[i];
        ISM_ASSERT(_mesh_A.is_valid_handle(vh_A));
        ISM_ASSERT(_mesh_B.is_valid_handle(vh_B));

        BarycentricPoint bary_B = BarycentricPoint(vh_B, _mesh_B);
        vtpm[vh_A] = bary_B;
    }
    return vtpm;
}

VertexToPointMap generate_vtpm_from_overlay_meshes(
        const TriMesh& _mesh_A,
        const TriMesh& _mesh_B,
        const TriMesh& _overlay_on_A,
        const TriMesh& _overlay_on_B)
{
    ISM_ASSERT_EQ(_overlay_on_A.n_vertices(), _overlay_on_B.n_vertices());

    // Build bsp trees for overlay on A
    // and mesh B
    BSPptr bsp_overlay_A = build_bsp(_overlay_on_A);
    BSPptr bsp_B = build_bsp(_mesh_B);

    VertexToPointMap vtpm(_mesh_A);
    for (const auto v : _mesh_A.vertices())
    {
        // Express vertex via barycentric coordiantes on overlay A
        const auto p_source = _mesh_A.point(v);
        const auto nn_overlay = bsp_overlay_A->nearest(to_acg3(p_source));
        ISM_ASSERT(nn_overlay.handle.is_valid());
        ISM_ASSERT(_overlay_on_A.is_valid_handle(nn_overlay.handle));
        ISM_ASSERT_L(nn_overlay.dist, 1e-4);
        const auto bary = BarycentricPoint(p_source, nn_overlay.handle, _overlay_on_A);

        // Evaluate barycentric coordinates on overlay B
        auto p_target = bary.point(_overlay_on_B);
        ISM_ASSERT_FINITE_MAT(p_target);

        // Express as barycentric point on B
        const auto nn_B = bsp_B->nearest(to_acg3(p_target));
        ISM_ASSERT(nn_B.handle.is_valid());
        ISM_ASSERT(_overlay_on_A.is_valid_handle(nn_B.handle));
        ISM_ASSERT_L(nn_B.dist, 1e-4);
        vtpm[v] = BarycentricPoint(p_target, nn_B.handle, _mesh_B);
    }

    return vtpm;
}

ExternalProperty<VH, Complex> interpolate_field(const TriMesh& _mesh_A,
        const TriMesh& _mesh_B,
        const VertexToPointMap& _vtpm,
        const ExternalProperty<VH, Complex>& _field_A,
        const double _w_interp)
{
    ISM_ASSERT(_vtpm.size_okay(_mesh_A));
    ISM_ASSERT(_field_A.size_okay(_mesh_A));

    // Setup constraint system:
    // diag(w) * A * x = diag(w) * b
    const int constr_rows = _mesh_A.n_vertices() + _mesh_B.n_vertices();
    const int constr_cols = _mesh_B.n_vertices();
    SparseMatrix A(constr_rows, constr_cols);
    VecXd w = VecXd::Zero(constr_rows);
    VecX<Complex> b = VecX<Complex>::Zero(constr_rows);
    {
        std::vector<Triplet> A_triplets;
        int row = 0;

        for (const auto& vh_A : _mesh_A.vertices())
        {
            const BarycentricPoint& bary = _vtpm[vh_A];
            // In a sparse VTPM, vertices of A may have no corresponding point on B.
            if (!bary.is_valid())
                continue;

            const Complex& value = _field_A[vh_A];

            A_triplets.emplace_back(row, bary.vh_a(_mesh_B).idx(), bary.alpha());
            A_triplets.emplace_back(row, bary.vh_b(_mesh_B).idx(), bary.beta());
            A_triplets.emplace_back(row, bary.vh_c(_mesh_B).idx(), bary.gamma());
            b[row] = value;
            w[row] = _w_interp;

            ++row;
        }

        for (const auto& vh_B : _mesh_B.vertices())
        {
            double area = 0.0;
            double weight_sum = 0.0;
            for (const auto& heh_B : _mesh_B.voh_range(vh_B)) {
                const VH& vh_to_B = _mesh_B.to_vertex_handle(heh_B);
                const double weight = cotan_weight(_mesh_B, heh_B);

                A_triplets.emplace_back(row, vh_to_B.idx(), weight);

                const HEH& heh_prev_B = _mesh_B.prev_halfedge_handle(heh_B);
                area += _mesh_B.calc_sector_area(heh_prev_B);
                weight_sum += weight;
            }

            A_triplets.emplace_back(row, vh_B.idx(), -weight_sum);
            b[row] = 0;
            w[row] = area / 3;

            ++row;
        }

        A.setFromTriplets(A_triplets.cbegin(), A_triplets.cend());
    }

    SparseMatrix AtWA = A.transpose() * w.asDiagonal() * A;
    VecX<Complex> AtWb = A.transpose() * w.asDiagonal() * b;

    Eigen::SimplicialLDLT<SparseMatrix> solver(AtWA);
    VecX<Complex> x = solver.solve(AtWb);
    ISM_ASSERT_EQ(x.rows(), _mesh_B.n_vertices());

    ExternalProperty<VH, Complex> field_B(_mesh_B);
    for (const auto& vh_B : _mesh_B.vertices())
    {
        field_B[vh_B] = x[vh_B.idx()] / std::abs(x[vh_B.idx()]);
    }
    return field_B;
}

std::vector<ExternalProperty<VH, Complex>>
interpolate_fields(
        const TriMesh& _mesh_A,
        const TriMesh& _mesh_B,
        const VertexToPointMap& _vtpm,
        const std::vector<ExternalProperty<VH, Complex>>& _fields_A,
        const double _w_interp)
{
    std::vector<ExternalProperty<VH, Complex>> result;
    for (const auto& f_A : _fields_A)
        result.push_back(interpolate_field(_mesh_A, _mesh_B, _vtpm, f_A, _w_interp));
    return result;
}

void sanitize_vtpm(
        VertexToPointMap& _vtpm,
        const double _epsilon)
{
    const double scale = 1.0 - _epsilon;
    const double o_alpha = 1.0 / 3.0;
    const double o_beta = 1.0 / 3.0;
    for (auto& bary : _vtpm.container_)
    {
        if (!std::isfinite(bary.alpha()) || !std::isfinite(bary.beta()))
            bary.set_alpha_beta(o_alpha, o_beta);
        int tries = 0;
        while (!bary.is_inside_exclusive() && tries < 10000)
        {
            bary.set_alpha_beta(
                (bary.alpha() - o_alpha) * scale + o_alpha,
                (bary.beta()  - o_beta)  * scale + o_beta
            );
            ++tries;
        }
        if (!bary.is_inside_exclusive())
        {
            bary.set_alpha_beta(o_alpha, o_beta);
        }
    }
}

void randomly_permute_vtpm(
        VertexToPointMap& _vtpm,
        int _n_transpositions)
{
    srand(0);

    ISM_ASSERT_GEQ(_vtpm.container_.size(), 2);
    int n_transposed = 0;
    while (n_transposed < _n_transpositions) {
        const size_t i0 = rand() % _vtpm.container_.size();
        const size_t i1 = rand() % _vtpm.container_.size();
        if (i0 != i1)
        {
            std::swap(_vtpm.container_[i0], _vtpm.container_[i1]);
            ++n_transposed;
        }
    }
}

void randomly_permute_vtpm(
        VertexToPointMap& _vtpm,
        double _ratio)
{
    _vtpm = randomly_permuted_vtpm(_vtpm, _ratio);
}

VertexToPointMap randomly_permuted_vtpm(
        const VertexToPointMap& _vtpm,
        int _n_transpositions)
{
    VertexToPointMap result = _vtpm;
    randomly_permute_vtpm(result, _n_transpositions);
    return result;
}

VertexToPointMap randomly_permuted_vtpm(
        const VertexToPointMap& _vtpm,
        double _ratio)
{
    srand(0);

    std::vector<size_t> orig_indices(_vtpm.size());
    std::iota(orig_indices.begin(), orig_indices.end(), 0);

    // Only keep a (random) n-element subset of the original indices.
    std::random_shuffle(orig_indices.begin(), orig_indices.end());
    const size_t n = std::round(_vtpm.size() * _ratio);
    orig_indices.resize(n);

    // Randomly permute whithin this subset.
    std::vector<size_t> new_indices = orig_indices;
    std::random_shuffle(new_indices.begin(), new_indices.end());

    VertexToPointMap result = _vtpm;
    for (size_t i = 0; i < n; ++i)
    {
        const size_t orig_index = orig_indices[i];
        const size_t new_index = new_indices[i];
        result.container_[new_index] = _vtpm.container_[orig_index];
    }
    return result;
}

void subsample_vtpm(
        VertexToPointMap& _vtpm,
        double _ratio)
{
    _vtpm = subsampled_vtpm(_vtpm, _ratio);
}

void subsample_vtpm(
        VertexToPointMap& _vtpm,
        int _n_keep)
{
    _vtpm = subsampled_vtpm(_vtpm, _n_keep);
}

VertexToPointMap subsampled_vtpm(
        const VertexToPointMap& _vtpm,
        double _ratio)
{
    const int n = std::round(_vtpm.size() * _ratio);
    return subsampled_vtpm(_vtpm, n);
}

VertexToPointMap subsampled_vtpm(
        const VertexToPointMap& _vtpm,
        int _n_keep)
{
    srand(684);

    std::vector<size_t> orig_indices(_vtpm.size());
    std::iota(orig_indices.begin(), orig_indices.end(), 0);

    // Only keep a (random) n-element subset of the original indices.
    std::random_shuffle(orig_indices.begin(), orig_indices.end());
    orig_indices.resize(_n_keep);

    // Delete all others samples
    VertexToPointMap result(_vtpm.size());
    for (int i = 0; i < _n_keep; ++i)
    {
        const size_t orig_index = orig_indices[i];
        result.container_[orig_index] = _vtpm.container_[orig_index];
    }

    return result;
}

}


// Data structures
#define BOOST_TEST_MODULE test_evaluation

#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <core/storage/sframe_interface/unity_sarray.hpp>
#include <core/storage/sframe_interface/unity_sframe.hpp>
#include <core/storage/sframe_data/sframe.hpp>

#include <vector>
#include <string>
#include <core/random/random.hpp>

#include <core/storage/sframe_data/testing_utils.hpp>
#include <core/util/testing_utils.hpp>
#include <toolkits/recsys/models.hpp>
#include <toolkits/evaluation/metrics.hpp>
#include <core/storage/sframe_data/testing_utils.hpp>
#include <core/util/testing_utils.hpp>

namespace turi {
namespace evaluation {

BOOST_AUTO_TEST_CASE(test_rmse_and_max_error) {
    size_t num_observations = 5000;
    random::seed(0);

    auto predictions = std::vector<flexible_type>(num_observations);
    auto targets = std::vector<flexible_type>(num_observations);
    double total = 0.0;
    double true_max_error = 0;
    for (size_t i = 0; i < num_observations; ++i) {
      predictions[i] = random::fast_uniform<double>(0, 1);
      targets[i] = random::fast_uniform<double>(0, 1);
      double err = std::abs((double)predictions[i] - (double)targets[i]);
      total += std::pow(err, 2);
      true_max_error = std::max(true_max_error, err);
    }
    double true_rmse = std::pow(total / (double) num_observations, .5);

    std::shared_ptr<sarray<flexible_type> > predictions_sa = make_testing_sarray(
          flex_type_enum::FLOAT, predictions);
    std::shared_ptr<sarray<flexible_type> > targets_sa = make_testing_sarray(
          flex_type_enum::FLOAT, targets);
    std::shared_ptr<unity_sarray> unity_targets_sa = std::make_shared<unity_sarray>();
    unity_targets_sa->construct_from_sarray(targets_sa);
    std::shared_ptr<unity_sarray> unity_predictions_sa= std::make_shared<unity_sarray>();
    unity_predictions_sa->construct_from_sarray(predictions_sa);

    variant_type rmse = evaluation::_supervised_streaming_evaluator(
                        unity_targets_sa, unity_predictions_sa, "rmse");
    variant_type max_error = evaluation::_supervised_streaming_evaluator(
                        unity_targets_sa, unity_predictions_sa, "max_error");
    TS_ASSERT(std::abs(variant_get_value<double>(rmse) - true_rmse) < 1e-15);
    TS_ASSERT(std::abs(variant_get_value<double>(max_error) - true_max_error) < 1e-15);
}

BOOST_AUTO_TEST_CASE(test_roc_curve) {
    // Arrange
    size_t num_observations = 20;
    random::seed(0);
    size_t true_positive = 0;
    size_t true_negative = 0;
    size_t false_positive = 0;
    size_t false_negative = 0;

    auto predictions = std::vector<flexible_type>(num_observations);
    auto targets = std::vector<flexible_type>(num_observations);

    for (size_t i = 0; i < num_observations; ++i) {
      predictions[i] = float(i) / num_observations;
      // random::fast_uniform<double>(0, 1);
      targets[i] = random::fast_uniform<size_t>(0, 1);
      if (targets[i] == 1) {
        if (predictions[i] == 1) {
          true_positive += 1;
        } else {
          false_negative += 1;
        }
      } else {
        if (predictions[i] == 1) {
          false_positive += 1;
        } else {
          true_negative += 1;
        }
      }
      size_t n_bins = 100;
      size_t bin = std::floor((double) predictions[i] * (double) n_bins);
      if (bin == n_bins) bin -= 1;
      //logprogress_stream << predictions[i]
      //                   << " "
      //                   << bin
      //                   << " "
      //                   << targets[i]
      //                   << " "
      //                   << std::endl;
    }

    std::shared_ptr<sarray<flexible_type> > predictions_sa = make_testing_sarray(
          flex_type_enum::FLOAT, predictions);
    std::shared_ptr<sarray<flexible_type> > targets_sa = make_testing_sarray(
          flex_type_enum::INTEGER, targets);
    std::shared_ptr<unity_sarray> unity_targets_sa = std::make_shared<unity_sarray>();
    unity_targets_sa->construct_from_sarray(targets_sa);
    std::shared_ptr<unity_sarray> unity_predictions_sa= std::make_shared<unity_sarray>();
    unity_predictions_sa->construct_from_sarray(predictions_sa);

    // Act
    std::map<std::string, flexible_type> kwargs {
               {"average", FLEX_UNDEFINED},
               {"binary", true}};
    variant_type result = evaluation::_supervised_streaming_evaluator(
             unity_targets_sa, unity_predictions_sa, "roc_curve", kwargs);

    // Assert
    //logprogress_stream << "Now printing." << std::endl;
    //auto sf = *(variant_get_value<std::shared_ptr<unity_sframe>>(result))->get_underlying_sframe();
    //sf.debug_print();
}

BOOST_AUTO_TEST_CASE(test_accuracy) {
    size_t num_observations = 5000;
    random::seed(0);
    size_t true_positive = 0;
    size_t true_negative = 0;
    size_t false_positive = 0;
    size_t false_negative = 0;

    auto predictions = std::vector<flexible_type>(num_observations);
    auto pred_vectors = std::vector<flexible_type>(num_observations);
    auto pred_dicts = std::vector<flexible_type>(num_observations);
    auto targets = std::vector<flexible_type>(num_observations);

    for (size_t i = 0; i < num_observations; ++i) {
      double prob = random::fast_uniform<double>(0.0, 1.0);
      predictions[i] = prob > 0.5 ? 1 : 0;
      pred_vectors[i] = flex_vec({1.0 - prob, prob});
      pred_dicts[i] = flex_dict({{0, 1.0 - prob}, {1, prob}});
      targets[i] = random::fast_uniform<size_t>(0, 1);
      if (targets[i] == 1) {
        if (predictions[i] == 1) {
          true_positive += 1;
        } else {
          false_negative += 1;
        }
      } else {
        if (predictions[i] == 1) {
          false_positive += 1;
        } else {
          true_negative += 1;
        }
      }
    }

    std::shared_ptr<sarray<flexible_type> > predictions_sa = make_testing_sarray(
          flex_type_enum::FLOAT, predictions);
    std::shared_ptr<sarray<flexible_type> > targets_sa = make_testing_sarray(
          flex_type_enum::INTEGER, targets);
    std::shared_ptr<unity_sarray> unity_targets_sa = std::make_shared<unity_sarray>();
    unity_targets_sa->construct_from_sarray(targets_sa);
    std::shared_ptr<unity_sarray> unity_predictions_sa= std::make_shared<unity_sarray>();
    unity_predictions_sa->construct_from_sarray(predictions_sa);

    std::map<std::string, flexible_type> kwargs {
               {"average", "micro"},
               {"binary", true}};
    variant_type result = evaluation::_supervised_streaming_evaluator(
             unity_targets_sa, unity_predictions_sa, "roc_curve", kwargs);
    variant_type accuracy = evaluation::_supervised_streaming_evaluator(
                        unity_targets_sa, unity_predictions_sa, "accuracy");
    double true_accuracy =
        static_cast<double>(true_positive + true_negative) / num_observations;
    TS_ASSERT_DELTA(variant_get_value<double>(accuracy), true_accuracy, 1e-15);

    // Test compute_classifier_metric API
    gl_sframe data({{"target", gl_sarray(unity_targets_sa)}});
    std::map<std::string, flexible_type> opts = {{"classes", flex_list({0,1})}};
    variant_map_type vec_result = compute_classifier_metrics(
        data, "target", "accuracy", gl_sarray(pred_vectors), opts);
    TS_ASSERT_DELTA(variant_get_value<double>(vec_result["accuracy"]),
                    true_accuracy, 1e-15);

    variant_map_type dict_result = compute_classifier_metrics(
        data, "target", "accuracy", gl_sarray(pred_dicts), opts);
    TS_ASSERT_DELTA(variant_get_value<double>(dict_result["accuracy"]),
                    true_accuracy, 1e-15);
}

}  // namespace evaluation
}  // namespace turi


/*
 * Group.cpp
 *
 *  Created on: 2012-03-17
 *      Author: leo
 */

#include <fruitpunch/Graphics/Group.h>
#include <fruitpunch/Graphics/Matrices.h>
#include <armadillo>

using namespace arma;

namespace fp_core {
using namespace fp_core::common;

Group::Group() : Object() {
	point zero;
	zero.x = 0;
	zero.y = 0;

	point one;
	one.x = 1;
	one.y = 1;

	m_center = zero;
	m_position = zero;
	m_scale = one;
	m_rotation = 0;
}

Group::~Group() {
	// TODO Auto-generated destructor stub
}

//void Group::move(point delta) {
//	std::vector<TransformablePtr>::iterator it;
//	for (it = m_content.begin(); it != m_content.end(); it++) {
//		point pos = (**it).getPosition();
//		pos.x = pos.x + delta.x;
//		pos.y = pos.y + delta.y;
//		(**it).setPosition(pos);
//	}
//}

void Group::add(TransformablePtr transformable) {
	m_content.push_back(transformable);
	m_center = groupCenter();
}

point Group::groupCenter() const {
	std::vector<point> bounds = boundaries();
	point center;
	point min = bounds[0];
	point max = bounds[1];
	center.x = (min.x + max.x) / 2;
	center.y = (min.y + max.y) / 2;
	return center;
}

std::vector<point> Group::boundaries() const {
	std::vector<point> result;
	point max;
	point min;

	if (m_content.size() == 0)
		return result;
	max = m_content[0]->getPosition();
	min = max;

	std::vector<TransformablePtr>::const_iterator it;

	for (it = m_content.begin(); it != m_content.end(); it++) {
		point p = (**it).getPosition();
		if (p.x < min.x)
			min.x = p.x;
		if (p.y < min.y)
			min.y = p.y;
		if (p.x > max.x)
			max.x = p.x;
		if (p.y > max.y)
			max.y = p.y;
	}
	result.push_back(min);
	result.push_back(max);

	return result;
}

void Group::setPosition(point pos) {
	/**
	 * We calculate a delta to be added/subtracted from
	 * each transformable in the list, because we want
	 * to preserve the transformable original position
	 * within the group
	 */

	point delta;
	delta.x = pos.x - m_position.x;
	delta.y = pos.y - m_position.y;

	std::vector<TransformablePtr>::iterator it;
	for (it = m_content.begin(); it != m_content.end(); it++) {
		point contpos = (**it).getPosition();
		contpos.x = pos.x + delta.x;
		contpos.y = pos.y + delta.y;
		(**it).setPosition(contpos);
	}
	m_position = pos;

}

point Group::getPosition() const {
	return m_position;
}

void Group::setRotation(float angle) {
	float delta = angle - m_rotation;

	// calculates the transformation matrix
	fmat33 trans = to_local_space_matrix(m_center);
	trans = (arma::fmat33) (rotation_matrix(delta) * trans);
	trans = (arma::fmat33) (to_global_space_matrix(m_center) * trans);

	std::vector<TransformablePtr>::iterator it;
	for (it = m_content.begin(); it != m_content.end(); it++) {
		point oldpos = (**it).getPosition();
		point newpos = m_renderer->transform_point(oldpos, trans);
		(**it).setPosition(newpos);
		(**it).setRotation(angle);
	}

	m_rotation = angle;

}

float Group::getRotation() const {
	return m_rotation;
}

void Group::setScale(point scal) {
	point delta;
	delta.x = scal.x - m_scale.x;
	delta.y = scal.y - m_scale.y;

	// calculates the transformation matrix
	fmat33 trans = to_local_space_matrix(m_center);
	trans = (arma::fmat33) (scale_matrix(delta) * trans);
	trans = (arma::fmat33) (to_global_space_matrix(m_center) * trans);

	std::vector<TransformablePtr>::iterator it;
	for (it = m_content.begin(); it != m_content.end(); it++) {
		point oldpos = (**it).getPosition();
		point newpos = m_renderer->transform_point(oldpos, trans);
		(**it).setPosition(newpos);
		(**it).setScale(scal);
	}

	m_scale = scal;
}

point Group::getScale() const {
	return m_scale;
}

} /* namespace fp_core */


#pragma once
#ifndef HUSKYSTATE_H
#define HUSKYSTATE_H

//C Libraries
#include <stdint.h>

#include <iostream>
#include <memory>

//External Libraries
#include <Eigen/Dense>
#include "ros/ros.h"

//Husky Libraries
#include "utils/imu.hpp"
#include "utils/joint_state.hpp"
#include "utils/utils.hpp"
// #include "RosPublisher.h"
// #include "PassiveTimeSync.h"

namespace husky_inekf{

class HuskyState {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        HuskyState();

        template <typename T>
        void setImu(
            const std::shared_ptr<husky_inekf::ImuMeasurement<T>>& next_imu) {

            const auto imu_data = next_imu;

            Eigen::Vector3d euler = Rotation2Euler(this->getRotation());
            // Set orientation rates
            Eigen::Vector3d angularVelocity, eulerRates;
            angularVelocity <<  imu_data.get()->angular_velocity.x,
                                imu_data.get()->angular_velocity.y,
                                imu_data.get()->angular_velocity.z;
            eulerRates = AngularVelocity2EulerRates(euler, angularVelocity);
            dq_.block<3,1>(3,0) = eulerRates;

            return;
        }

        void setJointState(
            const std::shared_ptr<husky_inekf::JointStateMeasurement>
            next_joint_state);
        void setBaseRotation(const Eigen::Matrix3d& R);
        void setBasePosition(const Eigen::Vector3d& p);
        void setBaseVelocity(const Eigen::Vector3d& v);
        void setImuBias(const Eigen::VectorXd& bias);
        void clear();

        Eigen::Matrix<double,10,1> q() const;
        Eigen::Matrix<double,10,1> dq() const;
        Eigen::Vector3d getPosition() const;
        Eigen::Quaternion<double> getQuaternion() const;
        Eigen::Matrix3d getRotation() const;
        Eigen::Vector3d getEulerAngles() const;
        Eigen::Vector3d getEulerRates() const;
        Eigen::Matrix<double, 4, 1> getEncoderPositions() const;
        Eigen::Matrix<double, 4, 1> getEncoderVelocities() const;
//         Eigen::Matrix<double,10,1> getMotorPositions() const;
//         Eigen::Matrix<double,10,1> getMotorVelocities() const;
//         Eigen::Matrix<double,4,1> getGRF() const;
        Eigen::Vector3d getAngularVelocity() const;
        Eigen::Vector3d getBodyVelocity() const;
        Eigen::Vector3d getWorldVelocity() const;
        Eigen::VectorXd getImuBias() const;

        // Extract robot pose:
        double x() const;
        double y() const;
        double z() const;
        double yaw() const;
        double pitch() const;
        double roll() const;
        // Extract robot joint values:
        double rightFrontMotor() const;
        double leftFrontMotor() const;
        double rightHindMotor() const;
        double leftHindMotor() const;

        // Extract robot velocity:
        double dx() const;
        double dy() const;
        double dz() const;
        double dyaw() const;
        double dpitch() const;
        double droll() const;
        // Extract robot joint velocities:
        double drightFrontMotor() const;
        double dleftFrontMotor() const;
        double drightHindMotor() const;
        double dleftHindMotor() const;

        void setTime(double time_in){time_stamp_ = time_in;};
        double getTime() const{return time_stamp_;};

        friend std::ostream& operator<<(std::ostream& os, const  HuskyState& obj);

    private:
        double time_stamp_;
        Eigen::Matrix<double, 10,1> q_;
        Eigen::Matrix<double, 10,1> dq_;
        Eigen::Matrix<double, 6,1> imu_bias_;
        Eigen::Matrix<double,4,1> GRF_; //!< ground reaction force
};

} // end namespace husky_inekf
#endif


/* ================================================================
 *
 * PyCA Project
 *
 * Copyright (c) J. Samuel Preston, Linh K. Ha, Sarang C. Joshi. All
 * rights reserved.  See Copyright.txt or for details.
 *
 * This software is distributed WITHOUT ANY WARRANTY; without even the
 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the above copyright notice for more information.
 *
 * ================================================================ */

#include "GVFieldOpers.h"
#include "GVFieldOperKernels.h"
#include <MemoryManager.h>
#include <pycaUtils.h>
#include <boost_mem.h>

namespace PyCA {

void GVFieldOpers::
SetToZero(Field3D& d_v, StreamT stream) {

    Vec3Di sz = d_v.grid().size();
    PyCA::SetToZero(d_v.x, d_v.y, d_v.z,
		    sz.x, sz.y, sz.z,
		    stream);

}

void GVFieldOpers::
toH(Field3D& d_h, const Field3D& d_v, const float& delta, StreamT stream, bool onDev){

    Vec3Di sz = d_h.grid().size();
    Vec3Df sp = d_h.grid().spacing();
    PyCA::toH(d_h.x, d_h.y, d_h.z,
	      sp.x, sp.y, sp.z,
	      d_v.x, d_v.y, d_v.z,
	      sz.x, sz.y, sz.z,
	      delta,
	      stream, onDev);
}

void GVFieldOpers::
toH_I(Field3D& d_h, const float& delta, StreamT stream, bool onDev){

    Vec3Di sz = d_h.grid().size();
    Vec3Df sp = d_h.grid().spacing();

    PyCA::toH_I(d_h.x, d_h.y, d_h.z,
		sp.x, sp.y, sp.z,
		sz.x, sz.y, sz.z,
		delta,
		stream, onDev);

}

// this needs to be in .cxx file to sequester reference to boost
// shared_ptr
void GVFieldOpers::
Splat(Field3D& d_o, const Field3D& d_h, const Field3D& d_i,
             bool normalize, StreamT stream)
{
    MK_CHECK3_SIZE(d_o, d_h, d_i);

    Vec3Di size = d_o.grid().size();
    size_t nVox = size.prod();

    STATIC_ASSERT(sizeof(int) == sizeof(float));

    if (normalize) {
        boost::shared_ptr<int> distBuffer = CreateSharedArray<MEM_DEVICE, int>(nVox);
        int* i_dd = (int*) distBuffer.get();

        splatNormalized(d_o.x, d_o.y, d_o.z, i_dd,
			size.x, size.y, size.z,
			d_i.x, d_i.y, d_i.z,
			d_h.x, d_h.y, d_h.z, nVox, stream);
    } else {
        splatUnnormalized(d_o.x, d_o.y, d_o.z,
			  size.x, size.y, size.z,
			  d_i.x, d_i.y, d_i.z,
			  d_h.x, d_h.y, d_h.z, nVox, stream);

    }
}

} // end namespace PyCA


/*
  Copyright (c) 2020, Robin Zimmeck <robin@robin-zimmeck.de>

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice, this
     list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
     this list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.

  3. Neither the name of the copyright holder nor the names of its contributors
     may be used to endorse or promote products derived from this software
     without specific prior written permission.

  This software is provided by the copyright holders and contributors "as is"
  and any express or implied warranties, including, but not limited to, the
  implied warranties of merchantability and fitness for a particular purpose are
  disclaimed. In no event shall the copyright holder or contributors be liable
  for any direct, indirect, incidental, special, exemplary, or consequential
  damages (including, but not limited to, procurement of substitute goods or
  services; loss of use, data, or profits; or business interruption) however
  caused and on any theory of liability, whether in contract, strict liability,
  or tort (including negligence or otherwise) arising in any way out of the use
  of this software, even if advised of the possibility of such damage.
*/

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "minisi/minisi.h"
#include "minisi/io.h"

using namespace minisi;

BOOST_AUTO_TEST_CASE(value_access_test)
{
  dimensionless_t dimless{42.0};

  length_t l{23.0};
  length_t length = length_t::from_value(10.0);
  BOOST_CHECK_EQUAL(length.value(), 10.0);
  BOOST_CHECK_EQUAL(length, 10.0 * metre);

  BOOST_CHECK(metre / metre == 1.0);
}

BOOST_AUTO_TEST_CASE(unary_arithmetic_operators_test)
{
  BOOST_CHECK_EQUAL(1.0 * metre, +(1.0 * metre));
  BOOST_CHECK_EQUAL(-1.0 * metre, -(1.0 * metre));
}

BOOST_AUTO_TEST_CASE(binary_arithmetic_operators_test)
{
  BOOST_CHECK_EQUAL(10.0 * metre + 20.0 * metre, 30.0 * metre);
  BOOST_CHECK_EQUAL(30.0 * metre - 10.0 * metre, 20.0 * metre);

  BOOST_CHECK_EQUAL(10.0 * metre / ( 5.0 * second ), 2.0 * metre / second);
  BOOST_CHECK_EQUAL(1000.0 * watt * 1.0 * hour, 3600000.0 * joule);
}

BOOST_AUTO_TEST_CASE(arithmetic_assignment_operators_test)
{
  auto timer = 10.0 * second;

  timer += 2.0 * second;
  BOOST_CHECK_EQUAL(timer, 12.0 * second);

  timer -= 4.0 * second;
  BOOST_CHECK_EQUAL(timer, 8.0 * second);

  timer *= 2.0;
  BOOST_CHECK_EQUAL(timer, 16.0 * second);

  timer /= 4.0;
  BOOST_CHECK_EQUAL(timer, 4.0 * second);

  timer *= (5.0 * dimensionless);
  BOOST_CHECK_EQUAL(timer, 20.0 * second);

  timer /= (10.0 * dimensionless);
  BOOST_CHECK_EQUAL(timer, 2.0 * second);
}

BOOST_AUTO_TEST_CASE(free_operators_test)
{
  BOOST_CHECK_EQUAL(10.0 * metre, metre * 10.0);
  BOOST_CHECK_EQUAL(440.0 / second, 880.0 / second / 2.0);
}

BOOST_AUTO_TEST_CASE(comparison_operators_test)
{
  BOOST_CHECK_EQUAL(true, 8.0 * metre < 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 9000.0 * metre < 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 32767.0 * metre < 9000.0 * metre);

  BOOST_CHECK_EQUAL(true, 8.0 * metre <= 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 9000.0 * metre <= 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 32767.0 * metre <= 9000.0 * metre);

  BOOST_CHECK_EQUAL(false, 8.0 * metre == 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 9000.0 * metre == 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 32767.0 * metre == 9000.0 * metre);

  BOOST_CHECK_EQUAL(true, 8.0 * metre != 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 9000.0 * metre != 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 32767.0 * metre != 9000.0 * metre);

  BOOST_CHECK_EQUAL(false, 8.0 * metre >= 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 9000.0 * metre >= 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 32767.0 * metre >= 9000.0 * metre);

  BOOST_CHECK_EQUAL(false, 8.0 * metre > 9000.0 * metre);
  BOOST_CHECK_EQUAL(false, 9000.0 * metre > 9000.0 * metre);
  BOOST_CHECK_EQUAL(true, 32767.0 * metre > 9000.0 * metre);
}


#include <iostream>
#include <cstdlib>
#include <cstdio>
#include <armadillo>
#include <random>

using namespace std;
using namespace arma;

int main(int argc, char **argv) {
  if(argc <= 2) {
    fprintf(stderr, "Usage: %s OUT-FILE NUM-AGENTS\n", argv[0]);
    return 0;
  }

  const char *filename = argv[1];
  int N = atoi(argv[2]);
  size_t K = 10000000;
  double m0 = 1.0;

  // pseudorandom number generator
  default_random_engine G;

  // seed PRNG with true random number
  random_device source;
  unsigned int seed = source();
  G.seed(seed);

  // uniform distributions
  uniform_int_distribution<> idist(0, N-1);  // discrete distribution on [0,N)
  uniform_real_distribution<> rdist(0, 1);   // real distribution on [0,1)

  // list of agents' money
  vector<double> m(N, m0);

  // sum of m^2
  double m2sq = N * m0 * m0;

  // lists of measured variance at different steps
  std::vector<int> step;
  std::vector<double> variance;

  // do switches
  size_t powerOfTen = 1;
  for(size_t k = 0; k < K; k++) {
    // pick two random, different agents
    int i = idist(G), j = idist(G);
    while(i == j) j = idist(G);

    // calculate total money of the two agents
    double mtot = m[i] + m[j];

    // redistribute money between the two agents
    double e = rdist(G);
    m2sq += (e * e + (1 - e) * (1 - e)) * mtot * mtot - m[i] * m[i] - m[j] * m[j];
    m[i] = e * mtot;
    m[j] = (1 - e) * mtot;

    if(k >= 100 * powerOfTen)
      powerOfTen *= 10;
    if(k % powerOfTen == 0) {
      // calculate variance
      double V = m2sq / N - m0 * m0;

      step.push_back(k);
      variance.push_back(V);
    }
  }

  // save to file
  FILE *fp = fopen(filename, "w");
  fprintf(fp, "k\tV\n");
  for(size_t i = 0; i < step.size() && i < variance.size(); i++) {
    fprintf(fp, "%d\t%.3E\n", step[i], variance[i]);
  }
  fclose(fp);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 combinatorial toolbox - rat/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of combinatorial components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 07/03/2011
///
#include <nt2/toolbox/combinatorial/include/functions/rat.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/saturate.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/toolbox/constant/constant.hpp>

NT2_TEST_CASE_TPL ( rat_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::rat;
  using nt2::tag::rat_;

  // specific values tests
  T n, d;
  rat(T(1.5), n, d);
  std::cout << "n " << n << " d " <<  d <<  std::endl;
  NT2_TEST_ULP_EQUAL(n, T(3), 0);
  NT2_TEST_ULP_EQUAL(d, T(2), 0);
  rat(T(3.14159), n, d);
  std::cout << "n " << n << " d " <<  d <<  std::endl;
  NT2_TEST_ULP_EQUAL(n, T(355), 0);
  NT2_TEST_ULP_EQUAL(d, T(113), 0);
} // end of test for floating_


#include <sstream>
#include <iomanip>

#include "operation/field/setup.h"

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

BOOST_AUTO_TEST_SUITE(field)

BOOST_AUTO_TEST_CASE_TEMPLATE(test_field_constructors, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE( "  Testing " << F::getName() );
  // Default constructor
  // F xx;
  // BOOST_WARN_MESSAGE(!xx.template is<0>(),
  //                    "Element should not be set on default constructor" );
  F x(0);
  BOOST_CHECK(x.template is<0>() );
  BOOST_CHECK(x.isPositive() );
  BOOST_CHECK(!x.hasInverse() );

  for (int i = -10; i < 0; ++i) {
    F y(i);
    BOOST_CHECK(y.isNegative());
    BOOST_CHECK(y.is(i));
  }
  for (int i = 0; i < 10; ++i) {
    F y(i);
    BOOST_CHECK(y.isPositive());
    BOOST_CHECK(y.is(i));
  }

  // Initialize from integer
  F y(1);
  //  BOOST_CHECK(y == F(1));
  BOOST_CHECK(y.template is<1>());
  BOOST_CHECK(y.isPositive());
  BOOST_CHECK(y.hasInverse());

  F z(-1);
  BOOST_CHECK(z.isNegative());
  BOOST_CHECK(z.template is<-1>());
  BOOST_CHECK(z.hasInverse());

  // Check copy constructor
  F q = y+z;
  BOOST_CHECK(q.template is<0>());
}

//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(setTo_is_operations, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  F x;

  x.template setTo<1>();
  BOOST_CHECK_EQUAL(x, F(1));
  BOOST_CHECK_MESSAGE(x.template is<1>()," for field " << F::getName());
  BOOST_CHECK_MESSAGE(!x.template is<0>()," for field " << F::getName());
  BOOST_CHECK(x.is(1));

  x.template setTo<-1>();
  BOOST_CHECK_EQUAL(x, F(-1));
  BOOST_CHECK_MESSAGE(!x.template is<0>(), " for field " << F::getName());
  BOOST_CHECK_MESSAGE(x.template is<-1>(), " for field " << F::getName());
  BOOST_CHECK_MESSAGE(x.is(-1), " for field " << F::getName());

  x.template setTo<0>();
  BOOST_CHECK_EQUAL(x, F(0));
  BOOST_CHECK_MESSAGE(x.template is<0>(),
      "x.template is<0>() failed, for x==" << x
      << "for field " << F::getName());
  BOOST_CHECK_MESSAGE(x.is(0),
      "x.is(0) failed, for x==" << x << " for field " << F::getName());

  x.template setTo<-101>();
  BOOST_CHECK_EQUAL( x, -F(101) );
  BOOST_CHECK_MESSAGE( x.template is<-101>(),
      "x.template is<-101>() failed, x==" << x
      << " for field " << F::getName());
  BOOST_CHECK_MESSAGE( x.is(-101),
      "x.is(-101) failed, x==" << x << " for field " << F::getName());

  x.setTo(-202);
  BOOST_CHECK_EQUAL(x, -F(202));
  BOOST_CHECK_MESSAGE(x.template is<-202>(),
      "x.template is<-202>() failed, x==" << x
      << " for field " << F::getName());
  BOOST_CHECK_MESSAGE(x.is(-202),
      "x.is(-202) failed,  x==" << x << " for field " << F::getName());
}

//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(random, F, math::FieldTypes) {
  // @todo, make a better test case for random
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F x;
  F y;
  const int MAX_TESTS = 10;
  for (int i = 0; i < MAX_TESTS; ++i) {
    x.invertableRandom();  // Check that random does not return anything higher
                           // than supported MOD.
    y.template setTo<-1>();
    F z = x+y;
    // Stream to string
    char strBuf[100];
    z.toString(strBuf);
    // Read from string
    std::istringstream iss(strBuf);
    typename F::sbase_type q;
    iss >> q;
    if (iss) {
      BOOST_CHECK_MESSAGE(z.is(q), i << " z==" << z << " and q==" << q
          << " buf==" << strBuf << " for " << F::getName());
      BOOST_CHECK_MESSAGE(x==(z+F(1))," x==" << x << " and z==" << z
          << " for " << F::getName());
      BOOST_CHECK_MESSAGE(x.is(q+1), " x=="<< x <<" and q==" << q
          << " for " << F::getName());
      BOOST_CHECK_MESSAGE(iss.eof(), "Input stream is not fully read in");
    } else {
      BOOST_WARN_MESSAGE(false, "Failed to read in '" << strBuf
          << "' into integer of " << sizeof(q)
          << " bytes for " << F::getName());
    }
  }
}

//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(field_size, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  BOOST_TEST_MESSAGE("    sizeof(F)==" << sizeof(F));
  BOOST_TEST_MESSAGE("    sizeof(void*)==" << sizeof(void*));
  // Test that field size is at least of pointer size.
  BOOST_WARN_MESSAGE(sizeof(F) >= sizeof(void*),F::getName() << " of size "
      << sizeof(F) << " is less than sizeof(void*)" << sizeof(void*));
  // Warn if it is larger than a pointer size.
  BOOST_WARN_MESSAGE(sizeof(F) <= sizeof(void*),F::getName() << " of size "
      << sizeof(F) << "is greater than sizeof(void*)" << sizeof(void*));
  BOOST_CHECK(F::getSize() > (1 << 16));
}

//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(field_name, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  const char* name1 = F::getName();
  std::string name1Str = name1;
  const char* name2 = F::getName();
  std::string name2Str = name2;
  //BOOST_CHECK_EQUAL(name1, name2); // Returned same address.
  BOOST_CHECK_EQUAL(name1Str, name2Str); // Returned same value.
  BOOST_CHECK(name1Str.size() > 0); // Is not empty string.
}

//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(swap, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F x;
  F y;
  x.template setTo<15>();
  y.template setTo<-1>();
  BOOST_CHECK(x.template is<15>());
  BOOST_CHECK(y.template is<-1>());
  x.swap(y);
  BOOST_CHECK(x.template is<-1>());
  BOOST_CHECK(y.template is<15>());
}
//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(invert, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.

  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  F x(-1);
  F y = x.getInverse();
  BOOST_CHECK_MESSAGE(x==y, "is x==y with x=" << x << " y=" << y);
  y = F(2);
  x = y;
  y.invert();
  BOOST_CHECK_MESSAGE(x * y == F(1), "is x*y==1 with x=" << x << " y=" << y);
}
//=============================================================================
BOOST_AUTO_TEST_CASE_TEMPLATE(sqr, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  F x(1);
  BOOST_REQUIRE_EQUAL(x, F(1));
  x.sqr();
  BOOST_CHECK_EQUAL(x, F(1));
  x.template setTo<-1>();
  BOOST_CHECK_EQUAL(x, F(-1));
  x.sqr();
  BOOST_CHECK(x.template is<1>());

  // sqr
  F s1(-2);
  s1.sqr();
  BOOST_CHECK(s1.template is<4>());
  s1.sqr();
  BOOST_CHECK_EQUAL(s1, F(16));
  s1.sqr();
  BOOST_CHECK_EQUAL(s1, F(256));
  s1.sqr();
  BOOST_CHECK_EQUAL(s1, F(256)*F(256));
}

BOOST_AUTO_TEST_CASE_TEMPLATE( powerBy, F, math::FieldTypes ) {
  math::FieldFixture<F> f;
  BOOST_CHECK( f ); // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F s1;

  // x.powerBy(2)
  s1 = F(2);
  s1.powerBy(3);
  BOOST_CHECK_EQUAL(s1, F(8));
  s1.powerBy(2);
  BOOST_CHECK_EQUAL(s1, F(64));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(square_root, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F x;
  x.template setTo<4>();
  BOOST_CHECK_EQUAL(x, F(4));
  F y(2);
  BOOST_CHECK_EQUAL(y, F(2));

  F z = x.getSqrt();
  BOOST_CHECK_EQUAL(z, F(2));

  BOOST_CHECK_EQUAL(x.getSqrt(), y);

  x.template setTo<9>();
  BOOST_CHECK_EQUAL(x.getSqrt(), F(3));

  x.template setTo<1>();
  BOOST_CHECK_EQUAL(x.getSqrt(), F(1));

  x.template setTo<-1>();
  z = x.getSqrt();
  BOOST_CHECK_EQUAL(z * z, x);

  // F sqrt() const
  {
    F v;
    v.template setTo<9>();
    F sq = v.getSqrt();
    BOOST_CHECK((sq.template is<3>()) || (sq.template is<-3>()));

    v.template setTo<1>();
    sq = v.getSqrt();
    BOOST_CHECK_EQUAL(sq, F(1));
  }

}

template <typename F>
void powerBy2p(int d, F* val) {
  // Max power we can compute is 2^32-1,
  // if depth is > 31 we need to power in a loop.
  // Using a^(2^i) = (a^(2^j))^(2^k) where i=j+k.
  int n31 = d / 31;
  int r31 = d % 31;
  val->powerBy(1 << r31);
  for (int i = 0; i < n31; i++) {
    val->powerBy(1UL << 31);
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE( primitive_roots, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.

  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  // Unlikely to encounter fields which have larger order.
  const unsigned int limit = 1024;
  F primRoots[limit];
  F primRootsInv[limit];

  unsigned int order = math::field::computePrimitiveRoots<F>(
      F(-1), primRoots, primRootsInv, limit);

  BOOST_CHECK_EQUAL(order, math::field::getFourierOrder<F>());

  for (unsigned int i = 0; i < order; ++i) {
    F omega2 = primRoots[i];
    powerBy2p<F>(i + 1, &omega2);  // Compute omega2^(2^depth).
    BOOST_CHECK_EQUAL(omega2, F(1));
    BOOST_CHECK_EQUAL(primRoots[i].getInverse(), primRootsInv[i]);
  }
  BOOST_CHECK_EQUAL(primRoots[0],F(-1));
  for (unsigned int i = 1; i < order; ++i) {
    BOOST_CHECK_EQUAL(primRoots[i] * primRoots[i], primRoots[i-1]);
  }

  const F* roots = F::getPrimRoots();
  for (unsigned int i = 1; i < order; ++i) {
    BOOST_CHECK_EQUAL(roots[i], primRoots[i]);
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(fourier_order, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.

  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  F omega;
  omega.template setTo<1>();
  omega.negate();
  BOOST_CHECK_EQUAL(omega, F(-1));
  BOOST_CHECK(omega.template is<-1>());
  F old_omega;
  F omega2;
  F twoPow(2);

  int depth;
  // Maximum depth is is limited by max i where 2^i < Field size.
  for (depth = 1; omega.hasInverse(); ++depth) {
    old_omega = omega;
    omega2 = omega;

    // See p215 Modern Computer Algebra, Gathen/Gerhald.
    // Check that omega is n = 2^depth root of unity in F
    powerBy2p<F>(depth, &omega2);  // Compute omega2^(2^depth).
    BOOST_CHECK_EQUAL(omega2, F(1));

    // check that omega^(n/t)-1 has inverse of t=2
    omega2 = omega;
    powerBy2p<F>(depth-1, &omega2); // omega^(n/2)
    BOOST_CHECK(omega2 != F(1));
    BOOST_CHECK((omega2-F(1)).hasInverse());

    omega = omega.getSqrt();
    if (omega.hasInverse()) {
      BOOST_CHECK_EQUAL(omega * omega, old_omega);
      BOOST_CHECK((!omega.template is<0>()));
    }
  }
  BOOST_CHECK(!omega.hasInverse());

  powerBy2p<F>(depth, &old_omega);
  BOOST_CHECK_EQUAL(old_omega, F(1));
  depth--;
  BOOST_CHECK_EQUAL(depth, math::field::getFourierOrder<F>());

  // static long double size()
  long double size = F::getSize();
  BOOST_TEST_MESSAGE("Field size "
      << std::fixed << std::setprecision(30) << size);
  BOOST_CHECK(size >= (static_cast<uint64_t>(1) << depth));
  // Check that size-1 is divisible by 2^depth.
  long double rem = std::fmod(size - 1,
      static_cast<long double>(static_cast<uint64_t>(1) << depth));
  BOOST_CHECK_EQUAL(rem, 0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(fromString, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK( f ); // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F s;
  BOOST_CHECK(math::field::util::fromString("-101", &s));
  // Check constructor from string.
  BOOST_CHECK_MESSAGE(s.isNegative(), "with s=" << s );
  //BOOST_CHECK_EQUAL( s , F(-101) );
  //BOOST_CHECK( s == F(-101) );
  BOOST_CHECK_MESSAGE(s.template is<-101>(), "with s=" << s);
  BOOST_CHECK_MESSAGE(s.is(-101), "with s=" << s);
  BOOST_CHECK_MESSAGE(s.hasInverse(), "with s=" << s);

  BOOST_CHECK(math::field::util::fromString("-0", &s));
  char buffer[100];
  s.toDebugString(buffer);
  BOOST_TEST_MESSAGE("  debug str for -0: " << buffer);
  F x(-1);
  x.toDebugString(buffer);
  BOOST_TEST_MESSAGE("  debug str for -1: " << buffer);
  BOOST_CHECK_EQUAL( x, F(-1));
  BOOST_CHECK(x.template is<-1>());

  BOOST_CHECK(!math::field::util::fromString("", &s));
  BOOST_CHECK(!math::field::util::fromString(" 1", &s));
  BOOST_CHECK(!math::field::util::fromString("-", &s));
  BOOST_CHECK(!math::field::util::fromString("+", &s));
  BOOST_CHECK(!math::field::util::fromString("A", &s));

  BOOST_CHECK(math::field::util::fromString("+0", &s));
  BOOST_CHECK(s.template is<0>());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(toString, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.

  // Convert negative number to string.
  F x(-101);
  char* buf = new char[F::getMaxStringRepLength()];
  x.toString(buf);
  BOOST_CHECK_EQUAL(buf[0], '-');
  BOOST_CHECK_EQUAL(buf[1], '1');
  BOOST_CHECK_EQUAL(buf[2], '0');
  BOOST_CHECK_EQUAL(buf[3], '1');
  BOOST_CHECK_EQUAL(buf[4], '\0');

  // Convert 0 to string
  x.template setTo<0>();
  x.toString(buf);
  BOOST_CHECK_EQUAL(buf[0], '0');
  BOOST_CHECK_EQUAL(buf[1], '\0');

  // Convert -1/2 to string and back
  F z;
  z.template setTo<-1>();
  z /= F(2);
  z.toString(buf);

  if (math::field::util::fromString<F>(buf, &x)) {
    BOOST_CHECK_MESSAGE(z == x, "z == x with z ==" << z << " and x == " << x
        << " and buf == " << buf << " for " << F::getName());
  } else {
    BOOST_ERROR("Failed to convert back to field: " << buf);
  }
/*
  x.template setTo<0>();
  for (int i = (buf[0]=='-')?1:0; buf[i]; ++i) {
    x *= F(10);
    x += F(buf[i]-'0');
  }
  if (buf[0]=='-') {
    x.negate();
  }
  BOOST_CHECK_MESSAGE(z==x, "z==x with z=="<<z<<" and x==" << x
    << " and buf==" << buf << " for " << F::getName());
*/
  delete[] buf;
}

BOOST_AUTO_TEST_CASE_TEMPLATE(mult_eq, F, math::FieldTypes) {
  math::FieldFixture<F> f; BOOST_CHECK( f ); // avoid unreferenced local variable warning
  BOOST_TEST_MESSAGE( "  Testing " << F::getName() );
  //F& F::operator*=(const F& m);
  F x(-1);
  F y(-1);
  x *= y;
  BOOST_CHECK_EQUAL(x, F(1));

  // Multiply by one gives same number back
  y.template setTo<1>();
  x *= y;
  BOOST_CHECK_EQUAL(x, F(1));

  // Multiply by -1 negates
  x.template setTo<101>();
  y.template setTo<-1>();
  x *= y;
  BOOST_CHECK_EQUAL(x, F(-101));

  // Multiply by zero gives zero
  y.template setTo<0>();
  x *= y;
  BOOST_CHECK_EQUAL(x, F(0));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(negate, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  BOOST_CHECK(F(0).isPositive());

  F x(1);
  BOOST_CHECK_MESSAGE(x.template is<1>(), "Is x==1 wit x=" << x);

  F y = -x;  // operator-
  BOOST_CHECK_MESSAGE(y.isNegative(),
      "Is y negative with y=-x=" << y << " and x=" << x);

  x.negate();

  BOOST_CHECK_EQUAL(x,y);

  BOOST_CHECK_MESSAGE(x.template is<-1>(), "Is x == -1 with x=" << x);
  BOOST_CHECK_MESSAGE((-x) == (-(-(-x))),
      "Is -x == -(-(-x)) with x=" << x); // operator-
  BOOST_CHECK_MESSAGE((-x).template is<1>(), "Is -x==1 with x=" << x);
  x.negate();
  BOOST_CHECK_MESSAGE(x.isPositive(), " with x=" << x);

  BOOST_CHECK_EQUAL(x, -y);

  y = F(1);
  x = F(-1);
  BOOST_CHECK_MESSAGE(x.isNegative(), "isNegative with x=-1==" << x);
  BOOST_CHECK_MESSAGE(y.isPositive(), "isPositive with y=1==" << y);
  x /= F(2);
  y /= F(2);
  BOOST_CHECK_MESSAGE(x.isPositive(), "isPositive with x=-1/2==" << x);
  BOOST_CHECK_MESSAGE((-x).isNegative(),
      "(-x).isNegative() with x=(-1/2)==" << x << " and -x==" << -x);
  BOOST_CHECK_MESSAGE(y.isNegative(), "isNegative with y=1/2==" << y);
  x.negate();
  y.negate();
  BOOST_CHECK_MESSAGE(x.isNegative(), "isNegative with x=1/2==" << x);
  BOOST_CHECK_MESSAGE(y.isPositive(), "isPositive with y=-1/2==" << y);
  x -= F(1);
  y += F(1);
  BOOST_CHECK_MESSAGE(x.isPositive(), "isPositive with x=1/2-1==" << x);
  BOOST_CHECK_MESSAGE(y.isNegative(), "isNegative with y=-1/2+1==" << y);
  BOOST_CHECK_MESSAGE(x + y == F(0),"x+y==0 with x=="<<x<< " y==" << y);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(add_eq, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  // Test F& F::operator+=(const F& m);

  F y;
  y.template setTo<0>();
  y += F(1);
  BOOST_CHECK(y.template is<1>());

  F x(-1);
  x += y;
  BOOST_CHECK_EQUAL(x, F(0));

  // Add zero gives same number back.
  x.template setTo<-3>();
  y.template setTo<0>();
  x += y;
  BOOST_CHECK_EQUAL(x, F(-3));

  // Add -1 is oposite of +1.
  x.template setTo<101>();
  y.template setTo<-1>();
  x += y;
  BOOST_CHECK_EQUAL(x + F(1), F(101));

  F z;
  x.template setTo<-441849981>();
  z = x + y;
  BOOST_CHECK_EQUAL(x - F(1), z);

}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_field_operations, F, math::FieldTypes)  {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  F x(1);
  BOOST_CHECK(x.template is<1>());

  // -=
  F y2;
  y2.template setTo<0>();
  BOOST_CHECK(y2.template is<0>());
  y2-=x;
  BOOST_CHECK(y2.template is<-1>());
  // /=
  // x.inverse()
  // y = x.inv()
  // x.setToOne();
  // x.setToZero();
  // F::setFill(dest, val, count)
  // F::setFill2P(dest, val, count)
  // x.setTo(int)
  // int toInt() const
  // std::string toString() const
  // std::string toStringRepresentation()
  // F operator+(Field32 right) const
  // F operator-(Field32 right) const
  // F operator*(Field32 right) const
  // F operator /(Field32 right) const
  // F pow(unsigned int p) const
  F p1(2);
  F p2 = p1;
  p2.sqr();
  std::stringstream ss;
  BOOST_CHECK_EQUAL(p1.getPow(0), F(1));
  BOOST_CHECK_EQUAL(p1.getPow(1), p1);
  BOOST_CHECK_EQUAL(p1.getPow(2), p2);
  BOOST_CHECK_EQUAL(p1.getPow(4), p2*p2);
  BOOST_CHECK_EQUAL(p1.getPow(4), F(16));

  // void random()
  // void nonZeroRandom()
  // static F getRandom()
  // static F one()
  // static F negOne()
  // static F zero()
  // static void randomInit(unsigned int seed)
  // static void swap(F& A, Field32& B)
  // static void add(F& dest, const F& src1, const F& src2)
  // static void subtract(F& dest, const F& src1, const F& src2)
  // static void neg(F& dest, const F& src)
  // static void multiply(F& dest, const F& src1, const F& src2)
  // static void divide(F& dest, const F& src1, const F& src2)
  // static void pow(F& dest, F& src, unsigned int p)

  // std::istream& operator>>(std::istream& is, F& f);
  // std::ostream& operator<<(std::ostream& os, const F& f);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_field_det,F,math::FieldTypes)  {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  BOOST_TEST_MESSAGE("  Testing " << F::getName());
  // 3x3 identity matrix.
  F mtx[9];
  mtx[0].template setTo<1>();
  mtx[1].template setTo<0>();
  mtx[2].template setTo<0>();
  mtx[3].template setTo<0>();
  mtx[4].template setTo<1>();
  mtx[5].template setTo<0>();
  mtx[6].template setTo<0>();
  mtx[7].template setTo<0>();
  mtx[8].template setTo<1>();
  // test static void det(F& dest, F* first, int size)

  BOOST_TEST_MESSAGE("mtx");
  BOOST_TEST_MESSAGE(mtx[0] << " " << mtx[1] << " " << mtx[2]);
  BOOST_TEST_MESSAGE(mtx[3] << " " << mtx[4] << " " << mtx[5]);
  BOOST_TEST_MESSAGE(mtx[6] << " " << mtx[7] << " " << mtx[8]);

  F dest;
  math::field::util::det<F>(&dest, mtx, 3);
  BOOST_CHECK(dest.template is<1>());
  BOOST_TEST_MESSAGE("Det = " << dest);

  boost::timer t;
  const int sz = 11;
  boost::shared_ptr<F> mtxptr(F::allocSq(sz),F::dealloc);
  F* mtx3 = mtxptr.get();
  for (int i = 0; i < sz; i++) {
    for (int j = 0; j < sz; j++) {
      mtx3[i*sz+j].template setTo<0>();
    }
  }
  BOOST_TEST_MESSAGE("Constructed " << sz << "x" << sz << " on heap in " << t.elapsed() << "s");
  t.restart();
  for (int i = 0; i < sz; i++) {
    mtx3[(sz-1)*(i+1)].template setTo<1>();
  }
  BOOST_TEST_MESSAGE("Filled diagonal in " << t.elapsed() << "s");
  for (int i = 0; i < sz; i++) {
    for (int j = 0; j < sz; j++) {
      if ((sz-i-1) == j) BOOST_CHECK(mtx3[i*sz+j].template is<1>());
      else BOOST_CHECK(mtx3[i*sz+j].template is<0>());
    }
  }

  t.restart();
  math::field::util::det<F>(&dest, mtx3, sz);
  BOOST_TEST_MESSAGE("Computed Determinant " << dest << " in " << t.elapsed() << "s");
  BOOST_CHECK( (sz>>1)&1?dest.template is<-1>():dest.template is<1>() );
}

//=============================================================================
/*
BOOST_AUTO_TEST_CASE_TEMPLATE(test_field_det2p, F, FieldTypes)  {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // Avoid unreferenced local variable warning.
  // static void det2p(F& dest, F* first, int size)
  const int sz2 = 10;
  unsigned int p2w = math::intlog(sz2);
  BOOST_CHECK_EQUAL( p2w, 4 );
  F* mtx2 = F::allocSq2p(sz2);
  for (int i = 0; i < sz2; ++i) {
    for (int j = 0; j < sz2; ++j) {
      mtx2[(i << p2w) + j].setTo<0>();
    }
  }
  for (int i = 0; i < sz2; ++i) {
    mtx2[(i<<p2w)+sz2-i-1].setTo<1>();
  }
  for (int i = 0; i < sz2; ++i) {
    for (int j = 0; j < sz2; ++j) {
      if (i == sz2-j-1) {
        BOOST_CHECK_EQUAL(mtx2[(i << p2w) + j] , 1);
      } else {
        BOOST_CHECK_EQUAL(mtx2[(i << p2w) + j] , 0);
      }
    }
  }
  F dest2;
  dest2.setTo<0>();
  BOOST_CHECK_EQUAL(dest2, 0);
  math::util::det2p<F>(&dest2, mtx2, sz2);
  delete[] mtx2;
  BOOST_CHECK_EQUAL(dest2,(sz2 >> 1) & 1 ? -1 : 1);
  //BOOST_CHECK_EQUAL( dest2, F(1) );
  BOOST_TEST_MESSAGE("Computed Determinant " << dest2);
}
*/
BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test() {
  return true;
}


#include "../../def_submodule.hpp"

#include "../../../../../type/math/coord.hpp"
#include "../../../../../type/math/interval.hpp"

#include <boost/python.hpp>

typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_scalar1D GTs_interval1D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_scalar2D GTs_interval2D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_scalar3D GTs_interval3D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_scalar4D GTs_interval4D;

typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_size1D GTs_range1D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_size2D GTs_range2D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_size3D GTs_range3D;
typedef  GS_DDMRM::S_IceRay::S_type::S_interval::GT_size4D GTs_range4D;

typedef GS_DDMRM::S_IceRay::S_type::GT_size               GTs_size;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size2D    GTs_cell2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size3D    GTs_cell3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size4D    GTs_cell4D;

typedef GS_DDMRM::S_IceRay::S_type::GT_scalar             GTs_scalar;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar2D  GTs_coord2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D  GTs_coord3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar4D  GTs_coord4D;

void expose_math_type_interval()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  typedef  GTs_coord3D const& (GTs_interval3D::*Tf_getLo3D )(void) const;
  Tf_getLo3D I_getLo3D = &GTs_interval3D::lo;

  //typedef  GTs_interval3D & (GTs_interval3D::*Tf_setLo3D   )(GTs_coord3D const&);
  //Tf_setLo3D I_setLo3D = &GTs_interval3D::lo;

  typedef  GTs_coord3D const& (GTs_interval3D::*Tf_getHi3D )(void) const;
  Tf_getHi3D I_getHi3D = &GTs_interval3D::hi;

  //typedef  GTs_interval3D & (GTs_interval3D::*Tf_setHi3D   )(GTs_coord3D const&);
  //Tf_setHi3D I_setHi3D = &GTs_interval3D::hi;

  boost::python::class_<GTs_interval3D>( "MathTypeInterval3D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_coord3D>() )
   .def( boost::python::init<GTs_coord3D,GTs_coord3D>() )
   .def( "lo", I_getLo3D, boost::python::return_value_policy<boost::python::copy_const_reference>() )
   //.def( "lo", I_setLo3D, boost::python::return_value_policy<boost::python::reference_existing_object>() )
   .def( "hi", I_getHi3D, boost::python::return_value_policy<boost::python::copy_const_reference>() )
   //.def( "hi", I_setHi3D, boost::python::return_value_policy<boost::python::reference_existing_object>() )
   //.def( "load",     &GTs_interval3D::F_load, boost::python::return_value_policy<boost::python::reference_existing_object>() )
   //.def( "inside",   &GTs_interval3D::F_in    )
   //.def( "outside",  &GTs_interval3D::F_out  )
   //.def( "correct",  &GTs_interval3D::F_correct, boost::python::return_value_policy<boost::python::reference_existing_object>() )

   //.def( "intersect",   )
   //.def( "extend",   )

   //.def( boost::python::self * GTs_scalar3D() )
   //.def( GTs_scalar3D() * boost::python::self  )
   //.def( boost::python::self / GTs_scalar3D() )
   //
   //.def( boost::python::self /= GTs_scalar3D() )
   //.def( boost::python::self *= GTs_scalar3D() )
   //
   //.def( boost::python::self += GTs_coord3D() )
   //.def( boost::python::self -= GTs_coord3D() )
  ;
  typedef  GTs_coord2D const& (GTs_interval2D::*Tf_getLo2D  )(void) const;
  Tf_getLo2D I_getLo2D = &GTs_interval2D::lo;

  //typedef  GTs_interval2D & (GTs_interval2D::*Tf_setLo2D   )(GTs_coord2D const&);
  //Tf_setLo2D I_setLo2D = &GTs_interval2D::lo;

  typedef  GTs_coord2D const& (GTs_interval2D::*Tf_getHi2D)(void) const;
  Tf_getHi2D I_getHi2D = &GTs_interval2D::hi;

  //typedef  GTs_interval2D & (GTs_interval2D::*Tf_setHi2D  )(GTs_coord2D const&);
  //Tf_setHi2D I_setHi2D = &GTs_interval2D::hi;

  boost::python::class_<GTs_interval2D>( "MathTypeInterval2D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_coord3D>() )
   .def( boost::python::init<GTs_coord2D,GTs_coord2D>() )
   .def( "lo",        I_getLo2D,                  boost::python::return_value_policy<boost::python::copy_const_reference>()      )
   //.def( "lo",        I_setLo2D,                  boost::python::return_value_policy<boost::python::reference_existing_object>() )
   .def( "hi",        I_getHi2D,                  boost::python::return_value_policy<boost::python::copy_const_reference>()      )
   //.def( "hi",        I_setHi2D,                  boost::python::return_value_policy<boost::python::reference_existing_object>() )
   //.def( "load",      &GTs_interval2D::F_load,    boost::python::return_value_policy<boost::python::reference_existing_object>() )
   //.def( "inside",    &GTs_interval2D::F_in        )
   //.def( "outside",   &GTs_interval2D::F_out       )
   //.def( "correct",   &GTs_interval2D::F_correct, boost::python::return_value_policy<boost::python::reference_existing_object>() )
   ;

  typedef  GTs_scalar const& (GTs_interval1D::*Tf_getLo1D  )(void) const;
  Tf_getLo1D I_getLo1D = &GTs_interval1D::lo;

  //typedef  GTs_interval1D & (GTs_interval1D::*Tf_setLo1D  )(GTs_scalar const&);
  //Tf_setLo1D I_setLo1D = &GTs_interval1D::lo;

  typedef  GTs_scalar const& (GTs_interval1D::*Tf_getHi1D )(void) const;
  Tf_getHi1D I_getHi1D = &GTs_interval1D::hi;

  //typedef  GTs_interval1D & (GTs_interval1D::*Tf_setHi1D )(GTs_scalar const&);
  //Tf_setHi1D I_setHi1D = &GTs_interval1D::hi;

  boost::python::class_<GTs_interval1D>( "MathTypeInterval1D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_scalar>() )
   .def( boost::python::init<GTs_scalar,GTs_scalar>() )
   .def( "lo", I_getLo1D,                      boost::python::return_value_policy<boost::python::copy_const_reference>()       )
   //.def( "lo", I_setLo1D,                      boost::python::return_value_policy<boost::python::reference_existing_object>()  )
   .def( "hi", I_getHi1D,                      boost::python::return_value_policy<boost::python::copy_const_reference>()       )
   //.def( "hi", I_setHi1D,                      boost::python::return_value_policy<boost::python::reference_existing_object>()  )
   //.def( "load",      &GTs_interval1D::F_load, boost::python::return_value_policy<boost::python::reference_existing_object>()  )
   //.def( "inside",    &GTs_interval1D::F_in   )
   //.def( "outside",   &GTs_interval1D::F_out  )
   //.def( "correct",   &GTs_interval1D::F_correct, boost::python::return_value_policy<boost::python::reference_existing_object>() )
   ;

  boost::python::class_<GTs_range1D>( "MathTypeRange1D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_scalar>() )
   .def( boost::python::init<GTs_size,GTs_size>() )
  ;
  boost::python::class_<GTs_range2D>( "MathTypeRange2D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_scalar>() )
   .def( boost::python::init<GTs_cell2D,GTs_cell2D>() )
    ;
  boost::python::class_<GTs_range3D>( "MathTypeRange3D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_scalar>() )
   .def( boost::python::init<GTs_cell3D,GTs_cell3D>() )
    ;
  boost::python::class_<GTs_range4D>( "MathTypeRange4D" )
   .def( boost::python::init<>() )
 //.def( boost::python::init<GTs_scalar>() )
   .def( boost::python::init<GTs_cell4D,GTs_cell4D>() )
   ;
 }