#include "generator/hierarchy.hpp"

#include "indexer/feature_algo.hpp"

#include "geometry/mercator.hpp"
#include "geometry/rect2d.hpp"

#include "base/assert.hpp"
#include "base/stl_helpers.hpp"

#include <algorithm>
#include <cmath>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iterator>
#include <limits>
#include <numeric>

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

BOOST_GEOMETRY_REGISTER_POINT_2D(m2::PointD, double, boost::geometry::cs::cartesian, x, y);
BOOST_GEOMETRY_REGISTER_RING(std::vector<m2::PointD>);

using namespace feature;

namespace generator
{
namespace hierarchy
{
namespace
{
double CalculateOverlapPercentage(std::vector<m2::PointD> const & lhs,
                                  std::vector<m2::PointD> const & rhs)
{
  if (!boost::geometry::intersects(lhs, rhs))
    return 0.0;

  using BoostPolygon = boost::geometry::model::polygon<m2::PointD>;
  std::vector<BoostPolygon> coll;
  boost::geometry::intersection(lhs, rhs, coll);
  auto const min = std::min(boost::geometry::area(lhs), boost::geometry::area(rhs));
  CHECK_GREATER(min, 0.0, (min));
  auto const binOp = [](double x, BoostPolygon const & y) { return x + boost::geometry::area(y); };
  auto const sum = std::accumulate(std::cbegin(coll), std::cend(coll), 0.0, binOp);
  return sum * 100 / min;
}
}  // namespace

bool FilterFeatureDefault(feature::FeatureBuilder const &) { return true; }

HierarchyPlace::HierarchyPlace(FeatureBuilder const & fb)
  : m_id(MakeCompositeId(fb))
  , m_name(fb.GetMultilangName())
  , m_types(fb.GetTypes())
  , m_rect(fb.GetLimitRect())
  , m_center(fb.GetKeyPoint())
{
  if (fb.IsPoint())
  {
    m_isPoint = true;
  }
  else if (fb.IsArea())
  {
    m_polygon = fb.GetOuterGeometry();
    boost::geometry::correct(m_polygon);
    m_area = boost::geometry::area(m_polygon);
  }
}

bool HierarchyPlace::Contains(HierarchyPlace const & smaller) const
{
  if (IsPoint())
    return false;

  if (smaller.IsPoint())
    return Contains(smaller.GetCenter());

  return smaller.GetArea() <= GetArea() &&
         CalculateOverlapPercentage(m_polygon, smaller.m_polygon) > 80.0;
}

bool HierarchyPlace::Contains(m2::PointD const & point) const
{
  return boost::geometry::covered_by(point, m_polygon);
}

HierarchyLinker::HierarchyLinker(Node::Ptrs && nodes)
  : m_nodes(std::move(nodes)), m_tree(MakeTree4d(m_nodes))
{
}

// static
HierarchyLinker::Tree4d HierarchyLinker::MakeTree4d(Node::Ptrs const & nodes)
{
  Tree4d tree;
  for (auto const & n : nodes)
    tree.Add(n, n->GetData().GetLimitRect());
  return tree;
}

HierarchyLinker::Node::Ptr HierarchyLinker::FindPlaceParent(HierarchyPlace const & place)
{
  Node::Ptr parent = nullptr;
  auto minArea = std::numeric_limits<double>::max();
  auto const point = place.GetCenter();
  m_tree.ForEachInRect({point, point}, [&](auto const & candidateNode) {
    // https://wiki.openstreetmap.org/wiki/Simple_3D_buildings
    // An object with tag 'building:part' is a part of a relation with outline 'building' or
    // is contained in a object with tag 'building'. This case is second. We suppose a building part is
    // only inside a building.
    static auto const & buildingChecker = ftypes::IsBuildingChecker::Instance();
    static auto const & buildingPartChecker = ftypes::IsBuildingPartChecker::Instance();
    auto const & candidate = candidateNode->GetData();
    if (buildingPartChecker(place.GetTypes()) &&
        !(buildingChecker(candidate.GetTypes()) || buildingPartChecker(candidate.GetTypes())))
    {
      return;
    }

    // A building part must have children only with 'building:part' type.
    if (!buildingPartChecker(place.GetTypes()) && buildingPartChecker(candidate.GetTypes()))
      return;

    if (place.GetCompositeId() == candidate.GetCompositeId())
      return;

    if (candidate.GetArea() < minArea && candidate.Contains(place))
    {
      // Sometimes there can be two places with the same geometry. We must check place node and
      // its parents to avoid cyclic connections.
      auto node = candidateNode;
      while (node->HasParent())
      {
        node = node->GetParent();
        if (node->GetData().GetCompositeId() == place.GetCompositeId())
          return;
      }

      parent = candidateNode;
      minArea = candidate.GetArea();
    }
  });
  return parent;
}

HierarchyLinker::Node::Ptrs HierarchyLinker::Link()
{
  for (auto & node : m_nodes)
  {
    auto const & place = node->GetData();
    auto const parentPlace = FindPlaceParent(place);
    if (!parentPlace)
      continue;

    tree_node::Link(node, parentPlace);
  }
  return m_nodes;
}

HierarchyEntryEnricher::HierarchyEntryEnricher(std::string const & osm2FtIdsPath,
                                             std::string const & countryFullPath)
  : m_featureGetter(countryFullPath)
{
  CHECK(m_osm2FtIds.ReadFromFile(osm2FtIdsPath), (osm2FtIdsPath));
}

std::optional<m2::PointD> HierarchyEntryEnricher::GetFeatureCenter(CompositeId const & id) const
{
  auto const optIds = m_osm2FtIds.GetFeatureIds(id);
  if (optIds.empty())
    return {};

  // A CompositeId id may correspond to several feature ids. These features can be represented by
  // three types of geometry. Logically, their centers coincide, but in practice they don’t,
  // because the centers are calculated differently. For example, for an object with a type area,
  // the area will be computed using the triangles geometry, but for an object with a type line,
  // the area will be computed using the outer geometry of a polygon.
  std::unordered_map<std::underlying_type_t<feature::GeomType>, m2::PointD> m;
  for (auto optId : optIds)
  {
    auto const ftPtr = m_featureGetter.GetFeatureByIndex(optId);
    if (!ftPtr)
      continue;

    CHECK(m.emplace(base::Underlying(ftPtr->GetGeomType()), feature::GetCenter(*ftPtr)).second,
          (id, optIds));
  }

  for (auto type : {
       base::Underlying(feature::GeomType::Point),
       base::Underlying(feature::GeomType::Area),
       base::Underlying(feature::GeomType::Line)})
  {
    if (m.count(type) != 0)
      return m[type];
  }

  return {};
}

HierarchyLinesBuilder::HierarchyLinesBuilder(HierarchyLinker::Node::Ptrs && trees)
  : m_trees(std::move(trees))
{
}

void HierarchyLinesBuilder::SetGetMainTypeFunction(GetMainTypeFn const & getMainType)
{
  m_getMainType = getMainType;
}

void HierarchyLinesBuilder::SetGetNameFunction(GetNameFn const & getName) { m_getName = getName; }

void HierarchyLinesBuilder::SetCountry(storage::CountryId const & country)
{
  m_countryName = country;
}

void HierarchyLinesBuilder::SetHierarchyEntryEnricher(
    std::unique_ptr<HierarchyEntryEnricher> && enricher)
{
  m_enricher = std::move(enricher);
}

std::vector<HierarchyEntry> HierarchyLinesBuilder::GetHierarchyLines()
{
  CHECK(m_getName, ());
  CHECK(m_getMainType, ());

  std::vector<HierarchyEntry> lines;
  for (auto const & tree : m_trees)
  {
    tree_node::PreOrderVisit(tree, [&](auto const & node) {
      lines.emplace_back(Transform(node));
    });
  }

  return lines;
}

m2::PointD HierarchyLinesBuilder::GetCenter(HierarchyLinker::Node::Ptr const & node)
{
  auto const & data = node->GetData();
  if (!m_enricher)
    return data.GetCenter();

  auto const optCenter = m_enricher->GetFeatureCenter(data.GetCompositeId());
  return optCenter ? *optCenter : data.GetCenter();
}

HierarchyEntry HierarchyLinesBuilder::Transform(HierarchyLinker::Node::Ptr const & node)
{
  HierarchyEntry line;
  auto const & data = node->GetData();
  line.m_id = data.GetCompositeId();
  auto const parent = node->GetParent();
  if (parent)
    line.m_parentId = parent->GetData().GetCompositeId();

  line.m_country = m_countryName;
  line.m_depth = GetDepth(node);
  line.m_name = m_getName(data.GetName());
  line.m_type = m_getMainType(data.GetTypes());
  line.m_center = GetCenter(node);
  return line;
}

HierarchyLinker::Node::Ptrs BuildHierarchy(std::vector<feature::FeatureBuilder> && fbs,
                                           GetMainTypeFn const & getMainType,
                                           std::shared_ptr<FilterInterface> const & filter)
{
  base::EraseIf(fbs, [&](auto const & fb) { return !filter->IsAccepted(fb); });
  HierarchyLinker::Node::Ptrs places;
  places.reserve(fbs.size());
  base::Transform(fbs, std::back_inserter(places), [](auto const & fb) {
    return tree_node::MakeTreeNode(HierarchyPlace(fb));
  });
  auto nodes = HierarchyLinker(std::move(places)).Link();
  // We leave only the trees.
  base::EraseIf(nodes, [](auto const & node) {
    return node->HasParent();
  });
  return nodes;
}

void AddChildrenTo(HierarchyLinker::Node::Ptrs & trees,
                   std::function<std::vector<HierarchyPlace>(CompositeId const &)> const & fn)
{
  for (auto & tree : trees)
  {
    CHECK(!tree->HasParent(), ());

    tree_node::PostOrderVisit(tree, [&](auto const & n) {
      auto const id = n->GetData().GetCompositeId();
      auto const & places = fn(id);
      for (auto place : places)
      {
        auto const newNode = tree_node::MakeTreeNode(std::move(place));
        tree_node::Link(newNode, n);
      }
    });
  }
}

void FlattenBuildingParts(HierarchyLinker::Node::Ptrs & trees)
{
  for (auto & tree : trees)
  {

    CHECK(!tree->HasParent(), ());

    std::vector<
        std::pair<hierarchy::HierarchyLinker::Node::Ptr, hierarchy::HierarchyLinker::Node::Ptr>>
        buildingPartsTrees;

    static auto const & buildingPartChecker = ftypes::IsBuildingPartChecker::Instance();
    std::function<void(hierarchy::HierarchyLinker::Node::Ptr const &)> visit;
    visit = [&](auto const & n) {
      if (buildingPartChecker(n->GetData().GetTypes()))
      {
        CHECK(n->HasParent(), ());
        auto building = n->GetParent();
        buildingPartsTrees.emplace_back(building, n);
        return;
      }

      CHECK(!buildingPartChecker(n->GetData().GetTypes()), ());
      for (auto const & ch : n->GetChildren())
        visit(ch);
    };

    visit(tree);

    for (auto const & buildingAndParts : buildingPartsTrees)
    {
      Unlink(buildingAndParts.second, buildingAndParts.first);
      tree_node::PostOrderVisit(buildingAndParts.second, [&](auto const & buildingPartNode) {
        CHECK(buildingPartChecker(buildingPartNode->GetData().GetTypes()), ());
        buildingPartNode->RemoveChildren();
        tree_node::Link(buildingPartNode, buildingAndParts.first);
      });
    }
  }
}
}  // namespace hierarchy
}  // namespace generator


#include <fstream>
#include <iostream>
#include <string>
#include <cmath>
#include <vector>
#include <cfloat>
#include <boost/filesystem.hpp>
#include "canvas.h"
#include "vector.h"
#include "matrix.h"
#include "shapes.h"
#include "model.h"
#include "light.h"

// Local function prototypes
void swap(vec3&, vec3&);
void sort_desc(std::vector<vec3>&);

////////////

// Constructor
Canvas::Canvas(int w, int h) : width(w), height(h)
{
    canvas.resize(w, std::vector<color>(h));
    depth_buffer.resize(w, std::vector<float>(h));
    for (int i = 0; i < w; ++i)
    {
        std::fill(depth_buffer[i].begin(),
                  depth_buffer[i].end(),
                  -std::numeric_limits<float>::max()
        );
    }
}

vec3 Canvas::convert_ndc_to_canvas(const vec3 &p)
{
    vec3 canvas_coords(
        (int)(0.5f * (width * p.x() + width)),
        (int)(0.5f * (height * p.y() + height)),
              0
    );

    return canvas_coords;
}

vec3 Canvas::convert_canvas_to_ndc(const vec3 &p)
{
    vec3 ndc_coords(
        (2 * p.x() - width) / width,
        (2 * p.y() - height) / height,
         0
    );

    return ndc_coords;
}

void Canvas::draw_shapes()
{
    // Draw all the shapes that are stored in the shapes_list
    for (std::vector<shape*>::const_iterator it = shapes_list.begin();
           it != shapes_list.end(); ++it)
    {
        // TODO refactor this to be just one function
        //(*it)->draw(canvas);
    }
}

void Canvas::apply_transform(const mat4& m_transform)
{
    // Apply transformation matrix on all shapes
    // and their corresponding vertices
    for (std::vector<shape *>::const_iterator it = shapes_list.begin();
           it != shapes_list.end(); ++it)
    {
        (*it)->apply_transform(m_transform);
    }
}

void Canvas::reset_canvas(const color& _color)
{
    for (int h = 0; h < height; ++h)
    {
        for (int w = 0; w < width; ++w)
        {
            canvas[w][h] = _color;

            // Set depth_buffer values to -FLT_MAX
            // which serves as the furthest point from the camera
            depth_buffer[w][h] = -FLT_MAX;
        }
    }
}

void Canvas::print_canvas()
{
    // Output canvas into ppm file
    std::cout << "P3\n" << width << " " << height << "\n255\n";
    for (int y = height-1; y >= 0; --y)
    {
        for (int x = 0; x < width; ++x)
        {
            int r = (int)(canvas[x][y].r() * 255.99f);
            int g = (int)(canvas[x][y].g() * 255.99f);
            int b = (int)(canvas[x][y].b() * 255.99f);

            std::cout << r << " "
                      << g << " "
                      << b << std::endl;
        }
    }
}

void Canvas::print_canvas(std::string _title)
{
    // Create an output directory to store renders
    // using the boost filesystem lib
    std::string output_dir = "../renders/";
    if (!boost::filesystem::exists(output_dir))
    {
        std::cout << "Creating output directory for renders at: " << output_dir << std::endl;
        boost::filesystem::create_directory(output_dir);
    }

    // Output canvas into ppm file
    std::ofstream image_file;
    image_file.open (output_dir + _title.c_str());
    image_file << "P3\n" << width << " " << height << "\n255\n";
    for (int y = height-1; y >= 0; --y)
    {
        for (int x = 0; x < width; ++x)
        {
            int r = (int)(canvas[x][y].r() * 255.99f);
            int g = (int)(canvas[x][y].g() * 255.99f);
            int b = (int)(canvas[x][y].b() * 255.99f);

            image_file << r << " "
                       << g << " "
                       << b << "\n";
        }
    }
    image_file.close();

    std::cout << "Printing " << _title << " to " << output_dir << std::endl;
}

void Canvas::put_pixel(int x, int y, const color& _color)
{
    // Only color the pixel if it is within the bounds of the canvas
    if (x > canvas.size() ||
        y > canvas[0].size() ||
        x < 0 || y < 0)
    {
        return;
    }

    // TODO
    // This is done due to the issues with convert_ndc_to_canvas
    // the 0.5f multiplication factor causes a 1.0 ndc value to be out of range
    // using 0.49999f can work but can be inaccurate based on the resolution
    if (x == canvas.size())
    {
        x--;
    }
    if (y == canvas[0].size())
    {
        y--;
    }

    canvas[x][y] = _color;
}

void swap(vec3 &p0, vec3 &p1)
{
    vec3 temp(p0);
    p0 = p1;
    p1 = temp;
}

// ==========================================
// This function orders the list of vertices
// by descending Y.
// So that v[0].y >= v[1].y >= .. >= v[n-1].y
// ==========================================
void sort_desc(std::vector<vec3> &verts)
{
    for (int i = 0; i < verts.size(); ++i)
    {
        for (int j = i + 1; j < verts.size(); ++j)
        {
            if (verts[i].y() < verts[j].y())
            {
                swap(verts[i], verts[j]);
            }
        }
    }
}

// ==========================================
// Draws a model from a *.obj format
// This is done by processing three vertices
// within the face
// ==========================================
void Canvas::draw_model(Model model, const color& _color, bool is_wire)
{
    vec3 new_color = _color;
    int nfaces = model.num_faces();
    for (int i = 0; i < nfaces; ++i)
    {
        bool is_back_face = false;
        // Check backface by using normal with scene camera

        // Get the index of the vertices that comprise the face
        std::vector<int> vert_indices = model.face(i);

        vec3 p0 = model.vert(vert_indices[0]);
        vec3 p1 = model.vert(vert_indices[1]);
        vec3 p2 = model.vert(vert_indices[2]);

        // TODO rethink how to handle drawing to NDC
        // Need to rethink how to tie with camera functions
        // Compute the direction of the normal and compare it with the light
        vec3 normal = cross((p1-p0), (p2-p0));
        normal.make_unit_vector();

        // TODO check backface based on camera position?
        vec3 cameraDir = camera.look_from - camera.look_at;
        cameraDir.make_unit_vector();
        is_back_face = dot(cameraDir, normal) < 0.0f;

        if (!is_wire && !lights.empty() && !is_back_face)
        {
            // TODO calculate direction off each fragment instead of a single vertex for more accurate results
            // TODO base light dir off of the center of the triangle
            vec3 midPoint = (p2 - ((p0 - p1)*0.5f)) * 0.5f;

            vec3 diffuse(0.0f);
            for (auto light : lights)
            {
                vec3 light_dir = (light->pos) - midPoint;
                light_dir.make_unit_vector();

                diffuse += std::max(0.0f, dot(normal, light_dir)) * light->color;
            }

            float ambient = 0.2f;
            new_color = (ambient + diffuse) * _color;
        }

        // TODO add depth buffer

        // Back-face culling - don't draw if it's a back-face
        if (!is_back_face)
        {
            // Draw the triangles based on the position of the three vertices
            draw_triangle(convert_ndc_to_canvas(p0),
                    convert_ndc_to_canvas(p1),
                    convert_ndc_to_canvas(p2),
                    new_color,
                    is_wire);
        }
    }
}

void Canvas::draw_line(vec3 p0, vec3 p1, const color& _color)
{
    int dx = p1.x() - p0.x();
    int dy = p1.y() - p0.y();

    // Check if p0 == p1, then just paint just that point
    if (dx == 0 && dy == 0)
    {
        put_pixel(p0.x(), p0.y(), _color);
        return;
    }

    // Draw line using y = f(x)
    if (abs(dx) > abs(dy))
    {
        if (p0.x() > p1.x())
        {
            swap(p0, p1);
        }

        int x_end = p1.x();
        float y = p0.y();
        float m = (p1.y() - p0.y())/(p1.x() - p0.x());  // slope of line

        for (int x = p0.x(); x < x_end; ++x)
        {
            put_pixel(x, (int)y, _color);
            y += m;
        }
    }
    // Draw line using x = f(y)
    else
    {
        if (p0.y() > p1.y())
        {
            swap(p0, p1);
        }

        int y_end = p1.y();
        float x = p0.x();
        float m = (p1.x() - p0.x())/(p1.y() - p0.y());  // slope of line

        for (int y = p0.y(); y < y_end; ++y)
        {
            put_pixel((int)x, y, _color);
            x += m;
        }
    }
}

// ==========================================
// Fills triangle by drawing lines from
// bottom-most point p1/p2 to the top-most
// point p0.
//
//     p0
//    /  \
//   p1--p2
//
// precondition: p0.y > p1.y == p2.y
// ==========================================
void Canvas::fill_flat_bottom_triangle(vec3 p0, vec3 p1, vec3 p2, const color &_color)
{
    int dy = p0.y() - p1.y();
    float slope_p1_p0 = (p0.x() - p1.x())/(p0.y() - p1.y());
    float slope_p2_p0 = (p0.x() - p2.x())/(p0.y() - p2.y());

    for (int i = 0; i < dy; ++i)
    {
        p1.e[0] += slope_p1_p0;
        p1.e[1] ++;
        p2.e[0] += slope_p2_p0;
        p2.e[1] ++;

        draw_line(p1, p2, _color);
    }
}

// =====================================
// Fills triangle by drawing lines from
// bottom most point p2 to the
// top-most points p0 and p1.
//
//   p0--p1
//    \  /
//     p2
//
// precondition: p0.y == p1.y > p2.y
// =====================================
void Canvas::fill_flat_top_triangle(vec3 p0, vec3 p1, vec3 p2, const color &_color)
{
    int dy = p0.y() - p2.y();
    float slope_p2_p0 = (p0.x() - p2.x())/(p0.y() - p2.y());
    float slope_p2_p1 = (p1.x() - p2.x())/(p1.y() - p2.y());

    vec3 pa = p2;
    vec3 pb = p2;

    for (int i = 0; i < dy; ++i)
    {
        pa.e[0] += slope_p2_p0;
        pa.e[1] ++;
        pb.e[0] += slope_p2_p1;
        pb.e[1] ++;

        draw_line(pa, pb, _color);
    }
}

//==========================================
// Calculates the triangles barycentric coordinate
//=========================================
vec3 barycentric(vec3 p0, vec3 p1, vec3 p2)
{
    vec3 bp;
}

//==========================================
// Draws a filled triangle, by splitting
// a triangle into a flat bottom part and
// flat top part. Then fills each line
// horizontally.
// =========================================
void Canvas::draw_triangle_filled(vec3 p0, vec3 p1, vec3 p2, const color &_color)
{
    // sort vertices on descending y
    std::vector<vec3> verts = {p0, p1, p2};
    sort_desc(verts);

    p0 = verts[0];
    p1 = verts[1];
    p2 = verts[2];

    if ((int)p1.y() == (int)p2.y())
    {
        fill_flat_bottom_triangle(p0, p1, p2, _color);
    }
    else if ((int)p0.y() == (int)p1.y())
    {
        fill_flat_top_triangle(p0, p1, p2, _color);
    }
    else
    {
        // Split the triangle into 2 triangles
        // one with a flat top and one with a flat bottom part
        //
        // precondition: p0.y > p1.y > p2.y
        //

        // slope from p0 to p2
        float mx = (p2.x() - p0.x())/(p2.y() - p0.y());
        // Get pa using pa = p0 + t(p2 - p0)
        vec3 pa(p0.x() + (p1.y() - p0.y())*mx, p1.y(), 0.0f);

        // Current use requires that p0.y > p1.y == pa.y
        //                           p1.y == pa.y > p2.y
        // due to the fill triangle method signature
        fill_flat_bottom_triangle(p0, p1, pa, _color);
        fill_flat_top_triangle(p1, pa, p2, _color);
    }
}

void Canvas::draw_triangle_wireframe(vec3 p0, vec3 p1, vec3 p2, const color &_color)
{
    draw_line(p0, p1, _color);
    draw_line(p1, p2, _color);
    draw_line(p2, p0, _color);
}

void Canvas::draw_triangle(vec3 p0, vec3 p1, vec3 p2, const color &_color, bool is_wire)
{
    if (is_wire)
    {
        draw_triangle_wireframe(p0, p1, p2, _color);
    }
    else
    {
        draw_triangle_filled(p0, p1, p2, _color);
    }
}


#include <ros/ros.h>
#include <ndt_generic/utils.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Eigen>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <sensor_msgs/PointCloud2.h>

#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>

#include <boost/circular_buffer.hpp>
#include <laser_geometry/laser_geometry.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>

#include <visualization_msgs/MarkerArray.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_srvs/Empty.h>

#include <boost/foreach.hpp>

#include <ndt_offline/PoseInterpolationNavMsgsOdo.h>
#include <ndt_calibration/ndt_calib.h>

//! Simple interface class implementation to be compatible with the NDTCalib API.
class PoseInterpolationTF : public PoseInterpolationInteface {

public:
  PoseInterpolationTF(const std::string &fixedframe,tf::Transformer &transformer) : fixedframe_(fixedframe), transformer_(transformer) {

  }

  bool getTransformationForTime(ros::Time t0, const std::string &frame_id,Eigen::Affine3d &T){
    std::string str;
    if (!transformer_.canTransform(fixedframe_, frame_id, t0, &str)) {
      return false;
    }
    tf::StampedTransform transform;
    transformer_.lookupTransform(fixedframe_,frame_id,t0, transform);
    tf::transformTFToEigen (transform, T);
    return true;
  }

private:
  tf::Transformer &transformer_;
  std::string fixedframe_;
};

class NDTCalibOnlineNode {

protected:
  // Our NodeHandle
  ros::NodeHandle nh_;

  ros::Subscriber points2_sub_;
  ros::Subscriber laser_sub_;

  // Components for publishing
  tf::TransformListener tf_listener_; // Keep the duration very long for this...
  tf::Transformer tr_transformer_;

  Eigen::Affine3d pose_, T, Ts;
  double sensor_pose_x,sensor_pose_y,sensor_pose_z,
  sensor_pose_r,sensor_pose_p,sensor_pose_t;
  double sensor_offset_t_;

  laser_geometry::LaserProjection projector_;

  ros::Publisher map_publisher_,laser_publisher_,point2_publisher_,odom_publisher_,adjusted_odom_publisher_,fuser_odom_publisher_;
  nav_msgs::Odometry fuser_odom,adjusted_odom_msg;
  Eigen::Affine3d last_odom, this_odom,last_gt_pose;

  std::string points_topic;
  std::string laser_topic;
  bool visualize;

  std::string pose_frame_;
  tf::Transform tf_sensor_pose_;
  double min_laser_range_;
  double varz_;
  double resolution_;
  double max_translation_;
  double min_rotation_;

  NDTCalibScanPair scan_pair_;
  NDTCalibScanPairs pairs_;
  PoseInterpolationTF pose_interp_;
  bool initialized;

  NDTCalibOptimize::ObjectiveType objective_type_;
  bool calib_;
  int score_type_;

  int min_nb_calibration_pairs_;
  int max_nb_calibration_pairs_;

public:
  // Constructor
  NDTCalibOnlineNode(ros::NodeHandle param_nh) : tf_listener_(ros::Duration(3600)), pose_interp_("/world", tf_listener_)
  {
    ///topic to wait for point clouds, if available
    param_nh.param<std::string>("points_topic",points_topic,"points");
    ///topic to wait for laser scan messages, if available
    param_nh.param<std::string>("laser_topic",laser_topic,"laser_scan");

    param_nh.param("visualize",visualize,true);

    ///initial pose and timing offset of the sensor w.r.t. the vehicle frame
    param_nh.param("sensor_pose_x",sensor_pose_x,0.);
    param_nh.param("sensor_pose_y",sensor_pose_y,0.);
    param_nh.param("sensor_pose_z",sensor_pose_z,0.);
    param_nh.param("sensor_pose_r",sensor_pose_r,0.);
    param_nh.param("sensor_pose_p",sensor_pose_p,0.);
    param_nh.param("sensor_pose_t",sensor_pose_t,0.);
    param_nh.param("sensor_offset_t",sensor_offset_t_,0.);

    // what to optimize
    bool cx, cy, cz, cex, cey, cez, ct;
    param_nh.param("cx", cx, true);
    param_nh.param("cy", cy, true);
    param_nh.param("cz", cz, false);
    param_nh.param("cex", cex, false);
    param_nh.param("cey", cey, false);
    param_nh.param("cez", cez, true);
    param_nh.param("ct", ct, true);
    this->objective_type_.setObjective(cx,cy,cz,cex,cey,cez,ct);

    param_nh.param<std::string>("pose_frame",pose_frame_,"/state_base_link");

    param_nh.param<double>("min_laser_range", min_laser_range_, 1.);
    param_nh.param<double>("resolution", resolution_, 0.6);
    param_nh.param<double>("laser_variance_z",varz_,resolution_/4);

    param_nh.param<double>("max_translation", max_translation_, 2);
    param_nh.param<double>("min_rotation",min_rotation_, 0.1);

    param_nh.param("calib", calib_, true);

    param_nh.param<int>("score_type", score_type_, 3);
    param_nh.param<int>("min_nb_calibration_pairs", min_nb_calibration_pairs_, 5);
    param_nh.param<int>("max_nb_calibration_pairs", max_nb_calibration_pairs_, 50);

    Ts =  Eigen::Translation<double,3>(sensor_pose_x,sensor_pose_y,sensor_pose_z)*
        Eigen::AngleAxis<double>(sensor_pose_r,Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis<double>(sensor_pose_p,Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis<double>(sensor_pose_t,Eigen::Vector3d::UnitZ()) ;

    tf::poseEigenToTF(Ts,tf_sensor_pose_);
  initialized = false;

   laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>(laser_topic, 1, &NDTCalibOnlineNode::laserCallback, this);
   points2_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(points_topic, 1, &NDTCalibOnlineNode::points2Callback, this);
  }

  // The T is given in global vehicle coordinates (not any relative ones as commonly done in the registration / fuser approaches.
  void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, Eigen::Affine3d T, ros::Time stamp) {

        if (!initialized) {
        scan_pair_.first = NDTCalibScan(cloud, T, cloud.header.stamp);
        initialized = true;
          return;
    }

    Eigen::Affine3d Tmotion = scan_pair_.first.getOriginalPose().inverse()*T;
    // We have gone to far without any "rotation", simply reset the system.
    if (Tmotion.translation().norm() > max_translation_) {
      scan_pair_.first = NDTCalibScan(cloud, T, cloud.header.stamp);
    }
    Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
    normalizeEulerAngles(rot);
    if (rot.norm() < min_rotation_) {
      return;
    }

    // Good pair found...
    scan_pair_.second = NDTCalibScan(cloud, T, stamp.toSec());

    pairs_.push_back(scan_pair_);

    if (pairs_.size() > min_nb_calibration_pairs_) {
      if (pairs_.size() > max_nb_calibration_pairs_) {
        pairs_.erase(pairs_.begin(), pairs_.begin()+pairs_.size() - max_nb_calibration_pairs_);
      }
      this->calibrate();
    }

    // Reset the first one...
    scan_pair_.first = scan_pair_.second;

    ROS_INFO_STREAM("# pairs : " << pairs_.size());
  }

  // Callback
  void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
  {
    sensor_msgs::PointCloud2 cloud;
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud_unfiltered, pcl_cloud;
    // TODO, check if the projector class could be used for individual interpolation of laser beams.
    projector_.projectLaser(*msg, cloud);
    pcl::fromROSMsg (cloud, pcl_cloud_unfiltered);

    pcl::PointXYZ pt;
    //add some variance on z
    for(int i=0; i<pcl_cloud_unfiltered.points.size(); i++) {
      pt = pcl_cloud_unfiltered.points[i];
      if(sqrt(pt.x*pt.x+pt.y*pt.y) > min_laser_range_) {
        pt.z += varz_*((double)rand())/(double)INT_MAX;
        pcl_cloud.points.push_back(pt);
      }
    }

    Eigen::Affine3d T;
    ros::Time t = msg->header.stamp-ros::Duration(sensor_offset_t_);
    ROS_INFO_STREAM("L @ time:" << t);
    if (pose_interp_.getTransformationForTime(t, pose_frame_, T)) {
      this->processFrame(pcl_cloud,T, t);
    }

  }

  void points2Callback(const sensor_msgs::PointCloud2::ConstPtr& msg){

    Eigen::Affine3d Tm;
    static bool last_odom_found=false;
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg (*msg, cloud);

    Eigen::Affine3d T;
    ros::Time t = msg->header.stamp-ros::Duration(sensor_offset_t_);
    if (pose_interp_.getTransformationForTime(t, pose_frame_, T)) {
      this->processFrame(cloud,T, msg->header.stamp-ros::Duration(sensor_offset_t_));
    }
}

  void calibrate() {
    if (pairs_.empty()) {
      ROS_ERROR_STREAM("No calibration scan pairs");
      return;
    }
    ROS_INFO_STREAM("got : " << pairs_.size() << " scan pairs");
    ROS_INFO_STREAM("Computing NDTMap...");
    pairs_.computeNDTMap(resolution_);
    ROS_INFO_STREAM("done.");

    NDTCalibOptimize opt(pairs_, score_type_, objective_type_, pose_interp_, pose_frame_);
    ROS_INFO_STREAM("Optimize time : " << objective_type_.optimizeTime());

    double delta_sensor_time_offset = 0.;
    opt.interpPairPoses(delta_sensor_time_offset); // We already moved the interpolation points while adding the clouds to the pairs, the sensortime reported will be the provided on added with the output.

    ROS_INFO_STREAM("initial sensor pose est : " << affine3dToString(Ts) << " dt : " << sensor_offset_t_+delta_sensor_time_offset << " error : " << opt.getScore6d(Ts));
    if (calib_) {
      ROS_INFO_STREAM("starting calibration, using score type : " << score_type_);
      opt.calibrate(Ts, delta_sensor_time_offset);
      opt.interpPairPoses(delta_sensor_time_offset);
      std::cout << " new sensor pose est : " << affine3dToString(Ts) << " dt : " << sensor_offset_t_+delta_sensor_time_offset << " error : " << opt.getScore6d(Ts) << std::endl;
    }
  }

public:
  // map publishing function
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ndt_calib_online_node");
  ros::NodeHandle param("~");
  NDTCalibOnlineNode n(param);
  ros::spin();

  return 0;
}


#ifndef FMCOEFS_HH
#define FMCOEFS_HH

#include <Eigen/Core>
#include "EriCoefs.hh"

class FmCoefs
{
public:
	FmCoefs(int mmax, int p_size, int q_size):
		_p_size(p_size), _q_size(q_size), _C(p_size, (mmax+1)*q_size),
		_mmin(0), _mmax(0)
	{
		block(0).setOnes();
	}

	int maxM() const { return _mmax; }
	int minM() const { return _mmin; }

	Eigen::Block<Eigen::ArrayXXd> operator[](int m)
	{
		return block(m);
	}
	const Eigen::Block<const Eigen::ArrayXXd> operator[](int m) const
	{
		return block(m);
	}

	Eigen::Block<Eigen::ArrayXXd> block(int m, int count=1)
	{
		return _C.block(0, (m-_mmin)*_q_size, _p_size, count*_q_size);
	}
	const Eigen::Block<const Eigen::ArrayXXd> block(int m, int count=1) const
	{
		return _C.block(0, (m-_mmin)*_q_size, _p_size, count*_q_size);
	}

	template <typename Derived0, typename Derived1>
	void multiplyCol(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1);
	template <typename Derived0, typename Derived1>
	void multiplyRow(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1);
	template <typename Derived0, typename Derived1>
	void multiply(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1);
	template <typename Derived1>
	void multiply_noC0(const Eigen::ArrayBase<Derived1>& C1);
	template <typename Derived0, typename Derived1, typename Derived2>
	void multiplyCol(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1,
		const Eigen::ArrayBase<Derived2>& C2);
	template <typename Derived0, typename Derived1, typename Derived2>
	void multiplyRow(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1,
		const Eigen::ArrayBase<Derived2>& C2);
	template <typename Derived0, typename Derived1, typename Derived2>
	void multiply(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1,
		const Eigen::ArrayBase<Derived2>& C2);
	template <typename Derived1, typename Derived2>
	void multiply_noC0(const Eigen::ArrayBase<Derived1>& C1,
		const Eigen::ArrayBase<Derived2>& C2);
	void multiply(const EriCoefs& coefs, int l1, int l2);

private:
	int _p_size;
	int _q_size;
	Eigen::ArrayXXd _C;
	int _mmin;
	int _mmax;
};

template <typename Derived0, typename Derived1>
void FmCoefs::multiplyCol(const Eigen::ArrayBase<Derived0>& C0,
	const Eigen::ArrayBase<Derived1>& C1)
{
	block(_mmax+1) = block(_mmax) * C1;
	for (int m = _mmax; m > _mmin; --m)
		block(m) = block(m).colwise()*C0 + block(m-1)*C1;
	block(_mmin).colwise() *= C0;
	++_mmax;
}

template <typename Derived0, typename Derived1>
void FmCoefs::multiplyRow(const Eigen::ArrayBase<Derived0>& C0,
		const Eigen::ArrayBase<Derived1>& C1)
{
	block(_mmax+1) = block(_mmax) * C1;
	for (int m = _mmax; m > _mmin; --m)
		block(m) = block(m).rowwise()*C0 + block(m-1)*C1;
	block(_mmin).rowwise() *= C0;
	++_mmax;
}

template <typename Derived0, typename Derived1>
void FmCoefs::multiply(const Eigen::ArrayBase<Derived0>& C0,
	const Eigen::ArrayBase<Derived1>& C1)
{
	block(_mmax+1) = block(_mmax) * C1;
	for (int m = _mmax; m > _mmin; --m)
		block(m) = block(m)*C0 + block(m-1)*C1;
	block(_mmin) *= C0;
	++_mmax;
}

template <typename Derived1>
void FmCoefs::multiply_noC0(const Eigen::ArrayBase<Derived1>& C1)
{
	block(_mmin, _mmax-_mmin+1) *= C1.replicate(1, _mmax-_mmin+1);
	++_mmin;
	++_mmax;
}

template <typename Derived0, typename Derived1, typename Derived2>
void FmCoefs::multiplyCol(const Eigen::ArrayBase<Derived0>& C0,
	const Eigen::ArrayBase<Derived1>& C1,
	const Eigen::ArrayBase<Derived2>& C2)
{
	if (_mmax == _mmin)
	{
		block(_mmin+2) = block(_mmin)*C2;
		block(_mmin+1) = block(_mmin)*C1;
		block(_mmin).colwise() *= C0;
	}
	else
	{
		block(_mmax+2) = block(_mmax)*C2;
		block(_mmax+1) = block(_mmax)*C1 + block(_mmax-1)*C2;
		for (int m = _mmax; m > _mmin+1; --m)
		{
			block(m) = block(m).colwise()*C0 + block(m-1)*C1
				+ block(m-2)*C2;
		}
		block(_mmin+1) = block(_mmin+1).colwise()*C0 + block(_mmin)*C1;
		block(_mmin).colwise() *= C0;
	}
	_mmax += 2;
}

template <typename Derived0, typename Derived1, typename Derived2>
void FmCoefs::multiplyRow(const Eigen::ArrayBase<Derived0>& C0,
	const Eigen::ArrayBase<Derived1>& C1,
	const Eigen::ArrayBase<Derived2>& C2)
{
	if (_mmax == _mmin)
	{
		block(_mmin+2) = block(_mmin)*C2;
		block(_mmin+1) = block(_mmin)*C1;
		block(_mmin).rowwise() *= C0;
	}
	else
	{
		block(_mmax+2) = block(_mmax)*C2;
		block(_mmax+1) = block(_mmax)*C1 + block(_mmax-1)*C2;
		for (int m = _mmax; m > _mmin+1; --m)
		{
			block(m) = block(m).rowwise()*C0 + block(m-1)*C1
				+ block(m-2)*C2;
		}
		block(_mmin+1) = block(_mmin+1).rowwise()*C0 + block(_mmin)*C1;
		block(_mmin).rowwise() *= C0;
	}
	_mmax += 2;
}

template <typename Derived0, typename Derived1, typename Derived2>
void FmCoefs::multiply(const Eigen::ArrayBase<Derived0>& C0,
	const Eigen::ArrayBase<Derived1>& C1,
	const Eigen::ArrayBase<Derived2>& C2)
{
	if (_mmax == _mmin)
	{
		block(_mmin+2) = block(_mmin)*C2;
		block(_mmin+1) = block(_mmin)*C1;
		block(_mmin) *= C0;
	}
	else
	{
		block(_mmax+2) = block(_mmax)*C2;
		block(_mmax+1) = block(_mmax)*C1 + block(_mmax-1)*C2;
		for (int m = _mmax; m > _mmin+1; --m)
		{
			block(m) = block(m)*C0 + block(m-1)*C1
				+ block(m-2)*C2;
		}
		block(_mmin+1) = block(_mmin+1)*C0 + block(_mmin)*C1;
		block(_mmin) *= C0;
	}
	_mmax += 2;
}

template <typename Derived1, typename Derived2>
void FmCoefs::multiply_noC0(const Eigen::ArrayBase<Derived1>& C1,
	const Eigen::ArrayBase<Derived2>& C2)
{
	if (_mmax == _mmin)
	{
		block(_mmin+1) = block(_mmin)*C2;
		block(_mmin) *= C1;
	}
	else
	{
		block(_mmax+1) = block(_mmax)*C2;
		for (int m = _mmax; m > _mmin; --m)
			block(m) = block(m)*C1 + block(m-1)*C2;
		block(_mmin) *= C1;
	}
	++_mmin;
	_mmax += 2;
}

#endif // FMCOEFS_HH


#include <iostream>
#include <fstream>
#include <string>
#include <stdlib.h>
#include <iomanip>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/timer.hpp>

namespace numerics = boost::numeric::ublas ;

template < typename T >
bool eq (const T& a, const T& b, double abstol)
{
   return std::abs(a-b)<abstol;
}

template < typename T >
struct is_equal
{
   is_equal (double tol) : abstol(tol) {}
   bool operator()(const T& a, const T&b) { return eq(a,b,abstol);}
   double abstol;
};

template < typename T >
void random_initialise(T& v)
{ v = 10.0 * rand()/(RAND_MAX+1.0) ; }

template < typename T >
void random_initialise_vector(numerics::vector< T >& v)
{
  size_t size = v.size();
  for(size_t i = 0 ; i < size ; ++i ) random_initialise( v[i] ) ;
}

template < typename T, typename Orientation >
void random_initialise_matrix(numerics::matrix< T, Orientation >& m)
{
  size_t size1 = m.size1();
  size_t size2 = m.size2();
  for(size_t i = 0 ; i < size1 ; ++i )
    for(size_t j = 0 ; j < size2 ; ++j )
      random_initialise( m(i,j) ) ;
}

template < typename T >
struct assign_multiplier
{
   T operator()() const
   { return 1.0000002 ; } // otherwise the accumulated result of the range will result in 0.0 or over/under-flow
};

template < typename T >
struct assign_multiplier< std::complex< T > >
{
  std::complex< T > operator()() const
  { return std::complex< T >( cos(0.5),sin(0.5) ) + ( assign_multiplier< T >().operator() - 1.0 ) ; }
};

template<class T>
double flops(int multiplies, int plus, int runs, double elapsed)
{ return ( multiplies * boost::numeric::ublas::type_traits<T>::multiplies_complexity + plus * boost::numeric::ublas::type_traits<T>::plus_complexity ) * runs / (1024 * 1024 * elapsed) ; }

template<class T>
struct peak_c_plus {
  typedef T value_type;

  void operator () (int runs) const {
    try {
      static T s (0);
      boost::timer t;
      for (int i = 0; i < runs; ++ i) {
        s += T (0);
      }
      std::cerr << flops<value_type>(0, 1, runs, t.elapsed ()) << " Mflops\n";
    }
    catch (std::exception &e) {
      std::cerr << e.what () << std::endl;
    }
    catch (...) {
      std::cerr << "unknown exception" << std::endl;
    }
  }
};

template<class T>
struct peak_c_multiplies {
  typedef T value_type;

  void operator () (int runs) const {
    try {
      static T s (1);
      boost::timer t;
      for (int i = 0; i < runs; ++ i) {
        s *= T (1);
      }
      std::cerr << flops<value_type>(0, 1, runs, t.elapsed ()) << " Mflops\n";
    }
    catch (std::exception &e) {
      std::cerr << e.what () << std::endl;
    }
    catch (...) {
      std::cerr << "unknown exception" << std::endl;
    }
  }
};

template<class T>
struct peak
{
  void operator () (int runs)
  {
    std::cerr << "plus       :";
    peak_c_plus<T> () (runs);
    std::cerr << "multiplies :";
    peak_c_multiplies<T> () (runs);
  }
};

template < typename value_type >
void check(value_type a, value_type b)
{
  if ( ! eq< value_type >( a, b, 1e-5 ) ) {
    std::cerr << "\n\nregression test failure : results are not identical" << std::endl;
    std::cerr << a << " != " << b << std::endl;
    exit( 1 );
  }
}

template < typename Iterator0, typename Iterator1 >
void check(Iterator0 begin0, Iterator0 end0, Iterator1 begin1)
{
  Iterator0 it0 = begin0 ;
  Iterator1 end1 = begin1 + std::distance(begin0,end0);
  for(bool fail = false ; it0 != end0 || fail ; ++it0 ) ; // fail = ! ( *it0 < std::numeric_limits< typename Iterator0::value_type >::max() );

  Iterator1 it1 = begin1 ;
  for(bool fail = false ; it1 != end1 || fail ; ++it1 ) ; // fail = ! ( *it1 < std::numeric_limits< typename Iterator0::value_type >::max() ) ;

  if ( it0 != end0 || it1 != end1 ) {
    std::cerr << "\n\nregression test failure : results overflowed" << std::endl;
    std::copy( begin0, end0, std::ostream_iterator< typename Iterator0::value_type >( std::cerr, " " ) ); std::cerr << std::endl;
    std::copy( begin1, end1, std::ostream_iterator< typename Iterator1::value_type >( std::cerr, " " ) ); std::cerr << std::endl;
    exit(1);
  }

  if ( ! std::equal( begin0, end0, begin1, is_equal< typename Iterator0::value_type >( std::abs( *begin0 ) * 1e-5 ) ) ) {
    std::cerr << "\n\nregression test failure : results are not identical" << std::endl;
    std::cerr << std::setprecision( 20 ) ;
    std::copy( begin0, end0, std::ostream_iterator< typename Iterator0::value_type >( std::cerr, " " ) ); std::cerr << std::endl;
    std::copy( begin1, end1, std::ostream_iterator< typename Iterator1::value_type >( std::cerr, " " ) ); std::cerr << std::endl;
    exit( 1 );
  }
}

template < typename value_type >
void check(numerics::matrix< value_type > &a, numerics::matrix< value_type > &b)
{
  bool ret = true ;
  size_t num_rows = a.size1() ;
  size_t num_cols = a.size2() ;

  for(size_t i = 0 ; i < num_rows && ret ; ++i )
    for(size_t j = 0 ; j < num_cols && ret ; ++j )
      ret = eq< value_type >( a(i,j), b(i,j), 1e-5 );

  if ( ! ret ) {
    std::cerr << "\n\nregression test failure : matrices not identical" << std::endl;
    exit( 1 );
  }
}

template < typename T >
void report(std::ostream& os, int runs, int runs_i, int size_i, double time)
{
  double normed_time = runs * time / ( runs_i * size_i ) ;
  double mflops = flops<T>(size_i,0,runs_i,time) ;
  std::cerr << std::setw(12) << normed_time << std::setw(12) << mflops ;
  os        << std::setw(12) << normed_time << std::setw(12) << mflops ;
}

template < typename FunctorType >
void loop(std::ostream& os, int start, int step, int stop, int runs, FunctorType functor)
{
  for(int size_i = start ; size_i <= stop ; size_i = std::max( static_cast< int >( size_i * (1 + 1.0 / step ) ), size_i + 1 ) ) {
    int runs_i = 10 * runs / size_i ;

    std::cerr << size_i << "\t";
    os        << size_i << "\t";

    functor.operator()( os, stop, size_i, runs, runs_i ) ;

    std::cerr << std::endl;
    os        << std::endl;
  }
}

template < typename FunctorType, typename CallFunctor >
void loop(std::ostream& os, int start, int step, int stop, int runs, FunctorType functor, CallFunctor ublas_call)
{
  for(int size_i = start ; size_i <= stop ; size_i = std::max( static_cast< int >( size_i * (1 + 1.0 / step ) ), size_i + 1 ) ) {
    int runs_i = 10 * runs / size_i ;

    std::cerr << size_i << "\t";
    os        << size_i << "\t";

    functor.operator()( os, stop, size_i, runs, runs_i, ublas_call ) ;

    std::cerr << std::endl;
    os        << std::endl;
  }
}


/*
 * AlgoPerfTest.cpp
 *
 *  Created on: 2017. 4. 30.
 *      Author: cho
 */

#include <boost/timer/timer.hpp>
#include <boost/algorithm/clamp.hpp>
#include <iostream>

using namespace std;

int main() {

	int count = 1000*1000;

	// min max
	cout << "--- Minimum/maximum operations" << endl;
	cout << "------ clamp() " << count << endl;
	{
		boost::timer::cpu_timer timer;
		timer.start();
		for ( int i = 0; i< count; ++i ) {
			boost::algorithm::clamp(i, 10.0, 20.0 );
		}

		timer.stop();
		cout << "clamp() : " << timer.format() << endl;
	}
	return 0;
}


#include <boost/geometry/algorithms/is_simple.hpp>


// Copyright 2016-2019 Doug Moen
// Licensed under the Apache License, version 2.0
// See accompanying file LICENSE or https://www.apache.org/licenses/LICENSE-2.0

#include <libcurv/builtin.h>

#include <libcurv/analyser.h>
#include <libcurv/array_op.h>
#include <libcurv/die.h>
#include <libcurv/dir_record.h>
#include <libcurv/exception.h>
#include <libcurv/function.h>
#include <libcurv/sc_compiler.h>
#include <libcurv/sc_context.h>
#include <libcurv/import.h>
#include <libcurv/math.h>
#include <libcurv/pattern.h>
#include <libcurv/picker.h>
#include <libcurv/program.h>
#include <libcurv/source.h>
#include <libcurv/system.h>

#include <boost/math/constants/constants.hpp>
#include <boost/filesystem.hpp>

#include <cassert>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <string>

using namespace std;
using namespace boost::math::double_constants;

namespace curv {

Shared<Meaning>
Builtin_Value::to_meaning(const Identifier& id) const
{
    return make<Constant>(share(id), value_);
}

struct Is_Null_Function : public Legacy_Function
{
    static const char* name() { return "is_null"; }
    Is_Null_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].is_null()};
    }
};
struct Is_Bool_Function : public Legacy_Function
{
    static const char* name() { return "is_bool"; }
    Is_Bool_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {isbool(args[0])};
    }
};
struct Is_Num_Function : public Legacy_Function
{
    static const char* name() { return "is_num"; }
    Is_Num_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {isnum(args[0])};
    }
};
struct Is_String_Function : public Legacy_Function
{
    static const char* name() { return "is_string"; }
    Is_String_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<String>() != nullptr};
    }
};
struct Is_List_Function : public Legacy_Function
{
    static const char* name() { return "is_list"; }
    Is_List_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {islist(args[0])};
    }
};
struct Is_Record_Function : public Legacy_Function
{
    static const char* name() { return "is_record"; }
    Is_Record_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Record>() != nullptr};
    }
};
struct Is_Fun_Function : public Legacy_Function
{
    static const char* name() { return "is_fun"; }
    Is_Fun_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {args[0].dycast<Function>() != nullptr};
    }
};

struct Bit_Function : public Legacy_Function
{
    static const char* name() { return "bit"; }
    Bit_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        return {double(args[0].to_bool(At_Arg(*this, args)))};
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (arg.type != SC_Type::Bool())
            throw Exception(At_SC_Arg(0, f),
                stringify(name(),": argument is not a bool"));
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = float("<<arg<<");\n";
        return result;
    }
};

#define UNARY_NUMERIC_FUNCTION(Class_Name,curv_name,c_name,glsl_name) \
struct Class_Name : public Legacy_Function \
{ \
    static const char* name() { return #curv_name; } \
    Class_Name() : Legacy_Function(1,name()) {} \
    struct Scalar_Op { \
        static double call(double x) { return c_name(x); } \
        Shared<Operation> make_expr(Shared<Operation> x) const \
        { \
            return make<Call_Expr>( \
                cx.call_frame_.call_phrase_, \
                make<Constant>( \
                    func_part(cx.call_frame_.call_phrase_), \
                    Value{share(cx.fun_)}), \
                x); \
        } \
        static Shared<const String> callstr(Value x) { \
            return stringify(x); \
        } \
        At_Arg cx; \
        Scalar_Op(Function& fun, Frame& args) : cx(fun,args) {} \
    }; \
    static Unary_Numeric_Array_Op<Scalar_Op> array_op; \
    Value call(Frame& args) override \
    { \
        return array_op.op(Scalar_Op(*this, args), args[0]); \
    } \
    SC_Value sc_call(SC_Frame& f) const override \
    { \
        return sc_call_unary_numeric(f, #glsl_name); \
    } \
}; \

UNARY_NUMERIC_FUNCTION(Sqrt_Function, sqrt, sqrt, sqrt)
UNARY_NUMERIC_FUNCTION(Log_Function, log, log, log)
UNARY_NUMERIC_FUNCTION(Abs_Function, abs, abs, abs)
UNARY_NUMERIC_FUNCTION(Floor_Function, floor, floor, floor)
UNARY_NUMERIC_FUNCTION(Ceil_Function, ceil, ceil, ceil)
UNARY_NUMERIC_FUNCTION(Trunc_Function, trunc, trunc, trunc)
UNARY_NUMERIC_FUNCTION(Round_Function, round, rint, roundEven)

inline double frac(double n) { return n - floor(n); }
UNARY_NUMERIC_FUNCTION(Frac_Function, frac, frac, fract)

UNARY_NUMERIC_FUNCTION(Sin_Function, sin, sin, sin)
UNARY_NUMERIC_FUNCTION(Cos_Function, cos, cos, cos)
UNARY_NUMERIC_FUNCTION(Tan_Function, tan, tan, tan)
UNARY_NUMERIC_FUNCTION(Acos_Function, acos, acos, acos)
UNARY_NUMERIC_FUNCTION(Asin_Function, asin, asin, asin)
UNARY_NUMERIC_FUNCTION(Atan_Function, atan, atan, atan)

UNARY_NUMERIC_FUNCTION(Sinh_Function, sinh, sinh, sinh)
UNARY_NUMERIC_FUNCTION(Cosh_Function, cosh, cosh, cosh)
UNARY_NUMERIC_FUNCTION(Tanh_Function, tanh, tanh, tanh)
UNARY_NUMERIC_FUNCTION(Acosh_Function, acosh, acosh, acosh)
UNARY_NUMERIC_FUNCTION(Asinh_Function, asinh, asinh, asinh)
UNARY_NUMERIC_FUNCTION(Atanh_Function, atanh, atanh, atanh)

double fhash(double f)
{
    union {
        double f;
        uint64_t u;
    } data;
    data.f = f;
    data.u =
        ((data.u & 0x000F'FFFF'FFFF'FFFF) // strip sign bit and exponent
        | 0x3FF0'0000'0000'0000) // set sign to 0 and exponent to 0
        ^ (data.u >> 12) // xor exponent and sign on top of mantissa
        ;
    // If f is normalized, then 1 <= data.f < 2.
    // If f is denormalized, then 0 <= data.f < 1. This includes f==0.
    return data.f - floor(data.f);
}
UNARY_NUMERIC_FUNCTION(Fhash_Function, fhash, fhash, fhash)

struct Atan2_Function : public Legacy_Function
{
    static const char* name() { return "atan2"; }
    Atan2_Function() : Legacy_Function(2,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) { return atan2(x, y); }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            throw Exception(cx,
                "Internal error: atan2 applied to a reactive value");
            //return make<Divide_Expr>(share(syntax), std::move(x), std::move(y));
        }
        static const char* name() { return "atan2"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun, args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.op(Scalar_Op(*this, args), args[0], args[1]);
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto x = f[0];
        auto y = f[1];

        SC_Type rtype = SC_Type::Bool();
        if (x.type == y.type)
            rtype = x.type;
        else if (x.type == SC_Type::Num())
            rtype = y.type;
        else if (y.type == SC_Type::Num())
            rtype = x.type;
        if (rtype == SC_Type::Bool())
            throw Exception(At_SC_Phrase(f.call_phrase_, f),
                "domain error");

        SC_Value result = f.sc_.newvalue(rtype);
        f.sc_.out() <<"  "<<rtype<<" "<<result<<" = atan(";
        sc_put_as(f, x, At_SC_Arg(0, f), rtype);
        f.sc_.out() << ",";
        sc_put_as(f, y, At_SC_Arg(1, f), rtype);
        f.sc_.out() << ");\n";
        return result;
    }
};

SC_Value sc_minmax(const char* name, Operation& argx, SC_Frame& f)
{
    auto list = dynamic_cast<List_Expr*>(&argx);
    if (list) {
        std::list<SC_Value> args;
        SC_Type type = SC_Type::Num();
        for (auto op : *list) {
            auto val = sc_eval_op(f, *op);
            args.push_back(val);
            if (val.type == SC_Type::Num())
                ;
            else if (val.type.is_vec()) {
                if (type == SC_Type::Num())
                    type = val.type;
                else if (type != val.type)
                    throw Exception(At_SC_Phrase(op->syntax_, f), stringify(
                        name, ": vector arguments of different lengths"));
            } else {
                throw Exception(At_SC_Phrase(op->syntax_, f), stringify(
                    name,": argument has bad type"));
            }
        }
        auto result = f.sc_.newvalue(type);
        if (args.size() == 0)
            f.sc_.out() << "  " << type << " " << result << " = -0.0/0.0;\n";
        else if (args.size() == 1)
            return args.front();
        else {
            f.sc_.out() << "  " << type << " " << result << " = ";
            int rparens = 0;
            while (args.size() > 2) {
                f.sc_.out() << name << "(" << args.front() << ",";
                args.pop_front();
                ++rparens;
            }
            f.sc_.out() << name << "(" << args.front() << "," << args.back() << ")";
            while (rparens > 0) {
                f.sc_.out() << ")";
                --rparens;
            }
            f.sc_.out() << ";\n";
        }
        return result;
    } else {
        auto arg = sc_eval_op(f, argx);
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = ";
        if (arg.type == SC_Type::Vec(2))
            f.sc_.out() << name <<"("<<arg<<".x,"<<arg<<".y);\n";
        else if (arg.type == SC_Type::Vec(3))
            f.sc_.out() << name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z);\n";
        else if (arg.type == SC_Type::Vec(4))
            f.sc_.out() << name<<"("<<name<<"("<<name<<"("<<arg<<".x,"<<arg<<".y),"
                <<arg<<".z),"<<arg<<".w);\n";
        else
            throw Exception(At_SC_Phrase(argx.syntax_, f), stringify(
                name,": argument is not a vector"));
        return result;
    }
}

struct Max_Function : public Legacy_Function
{
    static const char* name() { return "max"; }
    Max_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) {
            // return NaN if either argument is NaN.
            if (x >= y) return x;
            if (x < y) return y;
            return 0.0/0.0;
        }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            Shared<List_Expr> args =
                List_Expr::make({x, y}, arg_part(cx.call_frame_.call_phrase_));
            args->init();
            return make<Call_Expr>(
                cx.call_frame_.call_phrase_,
                make<Constant>(
                    func_part(cx.call_frame_.call_phrase_),
                    Value{share(cx.fun_)}),
                args);
        }
        static const char* name() { return "max"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun,args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(Scalar_Op(*this, args), -INFINITY, args[0]);
    }
    SC_Value sc_call_expr(Operation& argx, Shared<const Phrase>, SC_Frame& f)
    const override
    {
        return sc_minmax(name(),argx,f);
    }
};

struct Min_Function : public Legacy_Function
{
    static const char* name() { return "min"; }
    Min_Function() : Legacy_Function(1,name()) {}

    struct Scalar_Op {
        static double call(double x, double y) {
            // return NaN if either argument is NaN
            if (x <= y) return x;
            if (x > y) return y;
            return 0.0/0.0;
        }
        Shared<Operation> make_expr(
            Shared<Operation> x, Shared<Operation> y) const
        {
            Shared<List_Expr> args =
                List_Expr::make({x, y}, arg_part(cx.call_frame_.call_phrase_));
            args->init();
            return make<Call_Expr>(
                cx.call_frame_.call_phrase_,
                make<Constant>(
                    func_part(cx.call_frame_.call_phrase_),
                    Value{share(cx.fun_)}),
                args);
        }
        static const char* name() { return "min"; }
        static Shared<const String> callstr(Value x, Value y) {
            return stringify("[",x,",",y,"]");
        }
        At_Arg cx;
        Scalar_Op(Function& fun, Frame& args) : cx(fun, args) {}
    };
    static Binary_Numeric_Array_Op<Scalar_Op> array_op;
    Value call(Frame& args) override
    {
        return array_op.reduce(Scalar_Op(*this, args), INFINITY, args[0]);
    }
    SC_Value sc_call_expr(Operation& argx, Shared<const Phrase>, SC_Frame& f)
    const override
    {
        return sc_minmax("min",argx,f);
    }
};

// Generalized dot product that includes vector dot product and matrix product.
// Same as Mathematica Dot[A,B]. Like APL A+.×B, Python numpy.dot(A,B)
struct Dot_Function : public Legacy_Function
{
    static const char* name() { return "dot"; }
    Dot_Function() : Legacy_Function(2,name()) {}
    Value call(Frame& args) override
    {
        return dot(args[0], args[1], At_Arg(*this, args));
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto a = f[0];
        auto b = f[1];
        if (!a.type.is_vec())
            throw Exception(At_SC_Arg(0, f), "dot: argument is not a vector");
        if (a.type != b.type)
            throw Exception(At_SC_Arg(1, f), "dot: arguments have different types");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = dot("<<a<<","<<b<<");\n";
        return result;
    }
};

struct Mag_Function : public Legacy_Function
{
    static const char* name() { return "mag"; }
    Mag_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        // TODO: use hypot() or BLAS DNRM2 or Eigen stableNorm/blueNorm?
        // Avoids overflow/underflow due to squaring of large/small values.
        // Slower.  https://forum.kde.org/viewtopic.php?f=74&t=62402
        auto list = args[0].to<List>(At_Arg(*this, args));
        // Fast path: assume we have a list of number, compute a result.
        double sum = 0.0;
        for (auto val : *list) {
            double x = val.get_num_or_nan();
            sum += x * x;
        }
        if (sum == sum)
            return {sqrt(sum)};
        // The computation failed. Second fastest path: assume a mix of numbers
        // and reactive numbers, try to return a reactive result.
        Shared<List_Expr> rlist =
            List_Expr::make(list->size(),arg_part(args.call_phrase_));
        for (unsigned i = 0; i < list->size(); ++i) {
            Value val = list->at(i);
            if (val.is_num()) {
                rlist->at(i) = make<Constant>(arg_part(args.call_phrase_), val);
                continue;
            }
            auto r = val.dycast<Reactive_Value>();
            if (r && r->sctype_ == SC_Type::Num()) {
                rlist->at(i) = r->expr(*arg_part(args.call_phrase_));
                continue;
            }
            rlist = nullptr;
            break;
        }
        if (rlist) {
            rlist->init();
            return {make<Reactive_Expression>(
                SC_Type::Num(),
                make<Call_Expr>(
                    args.call_phrase_,
                    make<Constant>(
                        func_part(args.call_phrase_),
                        Value{share(*this)}),
                    rlist),
                At_Arg(*this, args))};
        }
        throw Exception(At_Arg(*this, args),
            stringify(args[0],": domain error"));
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (!arg.type.is_vec())
            throw Exception(At_SC_Arg(0, f), "mag: argument is not a vector");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = length("<<arg<<");\n";
        return result;
    }
};

struct Count_Function : public Legacy_Function
{
    static const char* name() { return "count"; }
    Count_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>())
            return {double(list->size())};
        if (auto string = args[0].dycast<const String>())
            return {double(string->size())};
        if (auto re = args[0].dycast<const Reactive_Value>()) {
            if (re->sctype_.is_list())
                return {double(re->sctype_.count())};
            //TODO:
            //if (re->sctype_ == SC_Type::Any())
        }
        throw Exception(At_Arg(*this, args), "not a list or string");
    }
    SC_Value sc_call(SC_Frame& f) const override
    {
        auto arg = f[0];
        if (!arg.type.is_list())
            throw Exception(At_SC_Arg(0, f), "count: argument is not a list");
        auto result = f.sc_.newvalue(SC_Type::Num());
        f.sc_.out() << "  float "<<result<<" = "<<arg.type.count()<<";\n";
        return result;
    }
};
struct Fields_Function : public Legacy_Function
{
    static const char* name() { return "fields"; }
    Fields_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto record = args[0].dycast<const Record>())
            return {record->fields()};
        throw Exception(At_Arg(*this, args), "not a record");
    }
};

struct Strcat_Function : public Legacy_Function
{
    static const char* name() { return "strcat"; }
    Strcat_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        if (auto list = args[0].dycast<const List>()) {
            String_Builder sb;
            for (auto val : *list) {
                if (auto str = val.dycast<const String>())
                    sb << str;
                else
                    sb << val;
            }
            return {sb.get_string()};
        }
        throw Exception(At_Arg(*this, args), "not a list");
    }
};
struct Repr_Function : public Legacy_Function
{
    static const char* name() { return "repr"; }
    Repr_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& args) override
    {
        String_Builder sb;
        sb << args[0];
        return {sb.get_string()};
    }
};
struct Decode_Function : public Legacy_Function
{
    static const char* name() { return "decode"; }
    Decode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        String_Builder sb;
        At_Arg cx(*this, f);
        auto list = f[0].to<List>(cx);
        for (size_t i = 0; i < list->size(); ++i)
            sb << (char)(*list)[i].to_int(1, 127, At_Index(i,cx));
        return {sb.get_string()};
    }
};
struct Encode_Function : public Legacy_Function
{
    static const char* name() { return "encode"; }
    Encode_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        List_Builder lb;
        At_Arg cx(*this, f);
        auto str = f[0].to<String>(cx);
        for (size_t i = 0; i < str->size(); ++i)
            lb.push_back({(double)(int)str->at(i)});
        return {lb.get_list()};
    }
};

struct Match_Function : public Legacy_Function
{
    static const char* name() { return "match"; }
    Match_Function() : Legacy_Function(1,name()) {}
    Value call(Frame& f) override
    {
        At_Arg ctx0(*this, f);
        auto list = f[0].to<List>(ctx0);
        std::vector<Shared<Function>> cases;
        for (size_t i = 0; i < list->size(); ++i)
            cases.push_back(list->at(i).to<Function>(At_Index(i,ctx0)));
        auto mf = make<Piecewise_Function>(cases);
        mf->name_ = name_;
        mf->argpos_ = 1;
        return {mf};
    }
};

// The filename argument to "file", if it is a relative filename,
// is interpreted relative to the parent directory of the source file from
// which "file" is called.
//
// Because "file" has this hidden parameter (the name of the source file from
// which it is called), it is not a pure function. For this reason, it isn't
// a function value at all, it's a metafunction.
struct File_Expr : public Just_Expression
{
    Shared<Operation> arg_;
    File_Expr(Shared<const Call_Phrase> src, Shared<Operation> arg)
    :
        Just_Expression(std::move(src)),
        arg_(std::move(arg))
    {}
    virtual Value eval(Frame& f) const override
    {
        // Metafunction calls do not automatically get a new stack frame
        // allocated. But I want the call to `file pathname` to appear
        // in stack traces, so I need a Frame.
        auto& callphrase = dynamic_cast<const Call_Phrase&>(*syntax_);
        std::unique_ptr<Frame> f2 =
            Frame::make(0, f.system_, &f, &callphrase, nullptr);
        At_Metacall_With_Call_Frame cx("file", 0, *f2);

        // construct file pathname from argument
        Value arg = arg_->eval(f);
        auto argstr = arg.to<String>(cx);
        namespace fs = boost::filesystem;
        fs::path filepath;
        auto caller_filename = syntax_->location().source().name_;
        if (caller_filename->empty()) {
            filepath = fs::path(argstr->c_str());
        } else {
            filepath = fs::path(caller_filename->c_str()).parent_path()
                / fs::path(argstr->c_str());
        }

        return import(filepath, cx);
    }
};
struct File_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<File_Expr>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `print`, such as `print "foo"`.
struct Print_Action : public Operation
{
    Shared<Operation> arg_;
    Print_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        Value arg = arg_->eval(f);
        auto str = arg.dycast<String>();
        if (str == nullptr)
            str = stringify(arg);
        f.system_.print(str->c_str());
    }
};
/// The meaning of the phrase `print` in isolation.
struct Print_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Print_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Warning_Action : public Operation
{
    Shared<Operation> arg_;
    Warning_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        Value arg = arg_->eval(f);
        Shared<String> msg;
        if (auto str = arg.dycast<String>())
            msg = str;
        else
            msg = stringify(arg);
        Exception exc{At_Phrase(*syntax_, f), msg};
        f.system_.warning(exc);
    }
};
/// The meaning of the phrase `warning` in isolation.
struct Warning_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Warning_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

/// The meaning of a call to `error`, such as `error("foo")`.
struct Error_Operation : public Operation
{
    Shared<Operation> arg_;
    Error_Operation(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    [[noreturn]] void run(Frame& f) const
    {
        Value val = arg_->eval(f);
        Shared<const String> msg;
        if (auto s = val.dycast<String>())
            msg = s;
        else
            msg = stringify(val);
        throw Exception{At_Phrase(*syntax_, f), msg};
    }
    virtual void exec(Frame& f, Executor&) const override
    {
        run(f);
    }
    virtual Value eval(Frame& f) const override
    {
        run(f);
    }
};
/// The meaning of the phrase `error` in isolation.
struct Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Error_Operation>(share(ph), analyse_op(*ph.arg_, env));
    }
};

// exec(expr) -- a debug action that evaluates expr, then discards the result.
// It is used to call functions or source files for their side effects.
struct Exec_Action : public Operation
{
    Shared<Operation> arg_;
    Exec_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        arg_->eval(f);
    }
};
struct Exec_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        return make<Exec_Action>(share(ph), analyse_op(*ph.arg_, env));
    }
};

struct Assert_Action : public Operation
{
    Shared<Operation> arg_;
    Assert_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> arg)
    :
        Operation(std::move(syntax)),
        arg_(std::move(arg))
    {}
    virtual void exec(Frame& f, Executor&) const override
    {
        At_Metacall cx{"assert", 0, *arg_->syntax_, f};
        bool b = arg_->eval(f).to_bool(cx);
        if (!b)
            throw Exception(At_Phrase(*syntax_, f), "assertion failed");
    }
};
struct Assert_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        return make<Assert_Action>(share(ph), arg);
    }
};

struct Assert_Error_Action : public Operation
{
    Shared<Operation> expected_message_;
    Shared<const String> actual_message_;
    Shared<Operation> expr_;

    Assert_Error_Action(
        Shared<const Phrase> syntax,
        Shared<Operation> expected_message,
        Shared<const String> actual_message,
        Shared<Operation> expr)
    :
        Operation(std::move(syntax)),
        expected_message_(std::move(expected_message)),
        actual_message_(std::move(actual_message)),
        expr_(std::move(expr))
    {}

    virtual void exec(Frame& f, Executor&) const override
    {
        Value expected_msg_val = expected_message_->eval(f);
        auto expected_msg_str = expected_msg_val.to<const String>(
            At_Phrase(*expected_message_->syntax_, f));

        if (actual_message_ != nullptr) {
            if (*actual_message_ != *expected_msg_str)
                throw Exception(At_Phrase(*syntax_, f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        actual_message_,
                        "\""));
            return;
        }

        Value result;
        try {
            result = expr_->eval(f);
        } catch (Exception& e) {
            if (*e.shared_what() != *expected_msg_str) {
                throw Exception(At_Phrase(*syntax_, f),
                    stringify("assertion failed: expected error \"",
                        expected_msg_str,
                        "\", actual error \"",
                        e.shared_what(),
                        "\""));
            }
            return;
        }
        throw Exception(At_Phrase(*syntax_, f),
            stringify("assertion failed: expected error \"",
                expected_msg_str,
                "\", got value ", result));
    }
};
struct Assert_Error_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto parens = cast<Paren_Phrase>(ph.arg_);
        Shared<Comma_Phrase> commas = nullptr;
        if (parens) commas = cast<Comma_Phrase>(parens->body_);
        if (parens && commas && commas->args_.size() == 2) {
            auto msg = analyse_op(*commas->args_[0].expr_, env);
            Shared<Operation> expr = nullptr;
            Shared<const String> actual_msg = nullptr;
            try {
                expr = analyse_op(*commas->args_[1].expr_, env);
            } catch (Exception& e) {
                actual_msg = e.shared_what();
            }
            return make<Assert_Error_Action>(share(ph), msg, actual_msg, expr);
        } else {
            throw Exception(At_Phrase(ph, env),
                "assert_error: expecting 2 arguments");
        }
    }
};

struct Defined_Expression : public Just_Expression
{
    Shared<const Operation> expr_;
    Symbol_Expr selector_;

    Defined_Expression(
        Shared<const Phrase> syntax,
        Shared<const Operation> expr,
        Symbol_Expr selector)
    :
        Just_Expression(std::move(syntax)),
        expr_(std::move(expr)),
        selector_(std::move(selector))
    {
    }

    virtual Value eval(Frame& f) const override
    {
        auto val = expr_->eval(f);
        auto s = val.dycast<Record>();
        if (s) {
            auto id = selector_.eval(f);
            return {s->hasfield(id)};
        } else {
            return {false};
        }
    }
};
struct Defined_Metafunction : public Metafunction
{
    using Metafunction::Metafunction;
    virtual Shared<Meaning> call(const Call_Phrase& ph, Environ& env) override
    {
        auto arg = analyse_op(*ph.arg_, env);
        auto dot = cast<Dot_Expr>(arg);
        if (dot != nullptr)
            return make<Defined_Expression>(
                share(ph), dot->base_, dot->selector_);
        throw Exception(At_Phrase(*ph.arg_, env),
            "defined: argument must be `expression.identifier`");
    }
};

const Namespace&
builtin_namespace()
{
    #define FUNCTION(f) {f::name(), make<Builtin_Value>(Value{make<f>()})}

    static const Namespace names = {
    {"pi", make<Builtin_Value>(pi)},
    {"tau", make<Builtin_Value>(two_pi)},
    {"inf", make<Builtin_Value>(INFINITY)},
    {"null", make<Builtin_Value>(Value())},
    {"false", make<Builtin_Value>(Value(false))},
    {"true", make<Builtin_Value>(Value(true))},

    FUNCTION(Is_Null_Function),
    FUNCTION(Is_Bool_Function),
    FUNCTION(Is_Num_Function),
    FUNCTION(Is_String_Function),
    FUNCTION(Is_List_Function),
    FUNCTION(Is_Record_Function),
    FUNCTION(Is_Fun_Function),
    FUNCTION(Bit_Function),
    FUNCTION(Sqrt_Function),
    FUNCTION(Log_Function),
    FUNCTION(Abs_Function),
    FUNCTION(Floor_Function),
    FUNCTION(Ceil_Function),
    FUNCTION(Trunc_Function),
    FUNCTION(Round_Function),
    FUNCTION(Frac_Function),
    FUNCTION(Fhash_Function),
    FUNCTION(Sin_Function),
    FUNCTION(Cos_Function),
    FUNCTION(Tan_Function),
    FUNCTION(Asin_Function),
    FUNCTION(Acos_Function),
    FUNCTION(Atan_Function),
    FUNCTION(Atan2_Function),
    FUNCTION(Sinh_Function),
    FUNCTION(Cosh_Function),
    FUNCTION(Tanh_Function),
    FUNCTION(Asinh_Function),
    FUNCTION(Acosh_Function),
    FUNCTION(Atanh_Function),
    FUNCTION(Max_Function),
    FUNCTION(Min_Function),
    FUNCTION(Dot_Function),
    FUNCTION(Mag_Function),
    FUNCTION(Count_Function),
    FUNCTION(Fields_Function),
    FUNCTION(Strcat_Function),
    FUNCTION(Repr_Function),
    FUNCTION(Decode_Function),
    FUNCTION(Encode_Function),
    FUNCTION(Match_Function),

    {"file", make<Builtin_Meaning<File_Metafunction>>()},
    {"print", make<Builtin_Meaning<Print_Metafunction>>()},
    {"warning", make<Builtin_Meaning<Warning_Metafunction>>()},
    {"error", make<Builtin_Meaning<Error_Metafunction>>()},
    {"assert", make<Builtin_Meaning<Assert_Metafunction>>()},
    {"assert_error", make<Builtin_Meaning<Assert_Error_Metafunction>>()},
    {"exec", make<Builtin_Meaning<Exec_Metafunction>>()},
    {"defined", make<Builtin_Meaning<Defined_Metafunction>>()},
    };
    return names;
}

} // namespace curv


/* Copyright (c) 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <ArborX.hpp>
#include <ArborX_Ray.hpp>

#include <Kokkos_Core.hpp>

#include <boost/program_options.hpp>

#include <fstream>

template <typename MemorySpace>
struct TriangleBoundingVolume
{
  Kokkos::View<ArborX::Experimental::Triangle *, MemorySpace> triangles;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<TriangleBoundingVolume<MemorySpace>,
                            ArborX::PrimitivesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t
  size(const TriangleBoundingVolume<MemorySpace> &tbv)
  {
    return tbv.triangles.extent(0);
  }

  KOKKOS_FUNCTION static ArborX::Box
  get(TriangleBoundingVolume<MemorySpace> const &tbv, std::size_t const i)
  {
    auto const &triangle = tbv.triangles(i);
    float max_coord[3] = {triangle.a[0], triangle.a[1], triangle.a[2]};
    float min_coord[3] = {triangle.a[0], triangle.a[1], triangle.a[2]};
    for (int i = 0; i < 3; ++i)
    {
      // Max
      if (triangle.b[i] > max_coord[i])
        max_coord[i] = triangle.b[i];
      if (triangle.c[i] > max_coord[i])
        max_coord[i] = triangle.c[i];

      // Min
      if (triangle.b[i] < min_coord[i])
        min_coord[i] = triangle.b[i];
      if (triangle.c[i] < min_coord[i])
        min_coord[i] = triangle.c[i];
    }

    return {{min_coord[0], min_coord[1], min_coord[2]},
            {max_coord[0], max_coord[1], max_coord[2]}};
  }
};

template <typename MemorySpace>
struct Rays
{
  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> _rays;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<Rays<MemorySpace>, ArborX::PredicatesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t size(const Rays<MemorySpace> &rays)
  {
    return rays._rays.extent(0);
  }
  KOKKOS_FUNCTION static auto get(Rays<MemorySpace> const &rays, std::size_t i)
  {
    return attach(intersects(rays._rays(i)), (int)i);
  }
};

template <typename MemorySpace>
struct ProjectPointDistance
{
  Kokkos::View<ArborX::Experimental::Triangle *, MemorySpace> triangles;
  Kokkos::View<float *, MemorySpace> distance;

  template <typename Predicate>
  KOKKOS_FUNCTION void operator()(Predicate const &predicate,
                                  int const primitive_index) const
  {
    auto const &ray = ArborX::getGeometry(predicate);
    float t;
    float u;
    float v;
    int const i = getData(predicate);
    if (rayTriangleIntersect(ray, triangles(primitive_index), t, u, v))
    {
      distance(i) = t;
    }
  }
};

ArborX::Point read_point(char *facet)
{
  char f1[4] = {facet[0], facet[1], facet[2], facet[3]};
  char f2[4] = {facet[4], facet[5], facet[6], facet[7]};
  char f3[4] = {facet[8], facet[9], facet[10], facet[11]};

  ArborX::Point vertex;
  vertex[0] = *(reinterpret_cast<float *>(f1));
  vertex[1] = *(reinterpret_cast<float *>(f2));
  vertex[2] = *(reinterpret_cast<float *>(f3));

  return vertex;
}

// Algorithm to read STL is based on http://www.sgh1.net/posts/read-stl-file.md
template <typename MemorySpace>
Kokkos::View<ArborX::Experimental::Triangle *, MemorySpace>
read_stl(std::string const &filename)
{
  std::ifstream file(filename.c_str(), std::ios::binary);
  ARBORX_ASSERT(file.good());

  // read 80 byte header
  char header_info[80] = "";
  file.read(header_info, 80);

  // Read the number of Triangle
  unsigned long long int n_triangles = 0;
  {
    char n_tri[4];
    file.read(n_tri, 4);
    n_triangles = *(reinterpret_cast<unsigned long long *>(n_tri));
  }

  Kokkos::View<ArborX::Experimental::Triangle *, MemorySpace> triangles(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "triangles"),
      n_triangles);

  auto triangles_host = Kokkos::create_mirror_view(triangles);

  for (unsigned int i = 0; i < n_triangles; ++i)
  {
    char facet[50];

    // read one 50-byte triangle
    file.read(facet, 50);

    // populate each point of the triangle
    // facet + 12 skips the triangle's unit normal
    auto p1 = read_point(facet + 12);
    auto p2 = read_point(facet + 24);
    auto p3 = read_point(facet + 36);

    // add a new triangle to the View
    triangles_host(i) = {p1, p2, p3};
  }
  file.close();

  Kokkos::deep_copy(triangles, triangles_host);

  return triangles;
}

template <typename MemorySpace>
Kokkos::View<ArborX::Experimental::Ray *, MemorySpace>
read_ray_file(std::string const &filename)
{
  std::ifstream file(filename.c_str());
  ARBORX_ASSERT(file.good());

  int n_rays = 0;
  file >> n_rays;

  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> rays(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "rays"), n_rays);

  auto rays_host = Kokkos::create_mirror_view(rays);

  for (int i = 0; i < n_rays; ++i)
  {
    float x, y, z, dir_x, dir_y, dir_z;
    file >> x >> y >> z >> dir_x >> dir_y >> dir_z;
    rays_host(i) = {{x, y, z}, {dir_x, dir_y, dir_z}};
  }
  file.close();

  Kokkos::deep_copy(rays, rays_host);

  return rays;
}

void outputTXT(Kokkos::View<ArborX::Point *, Kokkos::HostSpace> point_cloud,
               std::ostream &file, std::string const &delimiter)
{
  int const n_points = point_cloud.extent(0);
  for (int i = 0; i < n_points; ++i)
  {
    file << point_cloud(i)[0] << delimiter << point_cloud(i)[1] << delimiter
         << point_cloud(i)[2] << "\n";
  }
}

void outputVTK(
    Kokkos::View<ArborX::Point *, Kokkos::HostSpace> full_point_cloud,
    std::ostream &file)
{
  // For the vtk output, we remove all the points that are at infinity
  std::vector<ArborX::Point> point_cloud;
  for (unsigned int i = 0; i < full_point_cloud.extent(0); ++i)
  {
    if (full_point_cloud(i)[0] < KokkosExt::ArithmeticTraits::max<float>::value)
    {
      point_cloud.emplace_back(full_point_cloud(i)[0], full_point_cloud(i)[1],
                               full_point_cloud(i)[2]);
    }
  }

  // Write the header
  file << "# vtk DataFile Version 2.0\n";
  file << "Ray tracing\n";
  file << "ASCII\n";
  file << "DATASET POLYDATA\n";

  int const n_points = point_cloud.size();
  file << "POINTS " << n_points << " float\n";
  for (int i = 0; i < n_points; ++i)
  {
    file << point_cloud[i][0] << " " << point_cloud[i][1] << " "
         << point_cloud[i][2] << "\n";
  }

  // We need to associate a value to each point. We arbitrarly choose 1.
  file << "POINT_DATA " << n_points << "\n";
  file << "SCALARS value float 1\n";
  file << "LOOKUP_TABLE table\n";
  for (int i = 0; i < n_points; ++i)
  {
    file << "1.0\n";
  }
}

int main(int argc, char *argv[])
{
  using ExecutionSpace = Kokkos::DefaultExecutionSpace;
  using MemorySpace = ExecutionSpace::memory_space;

  Kokkos::ScopeGuard guard(argc, argv);

  namespace bpo = boost::program_options;

  std::string stl_filename;
  std::string ray_filename;
  std::string output_filename;
  std::string output_type;
  int n_ray_files;

  bpo::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
    ("help,h", "help message" )
    ("stl_file,s", bpo::value<std::string>(&stl_filename), "name of the STL file")
    ("ray_files,r", bpo::value<std::string>(&ray_filename), "name of the ray file")
    ("n_ray_files,n", bpo::value<int>(&n_ray_files), "number of ray files")
    ("output_files,o", bpo::value<std::string>(&output_filename), "name of the output file")
    ("output_type,t", bpo::value<std::string>(&output_type),
       "type of the output: csv, numpy, or vtk")
    ;
  // clang-format on
  bpo::variables_map vm;
  bpo::store(bpo::command_line_parser(argc, argv).options(desc).run(), vm);
  bpo::notify(vm);

  if (vm.count("help") > 0)
  {
    std::cout << desc << '\n';
    return 1;
  }

  // Read the STL file
  Kokkos::Profiling::pushRegion("ArborX::read_stl");
  auto triangles = read_stl<MemorySpace>(stl_filename);
  Kokkos::Profiling::popRegion();

  // Build the BVH
  ExecutionSpace exec_space;
  ArborX::BVH<MemorySpace> bvh(exec_space,
                               TriangleBoundingVolume<MemorySpace>{triangles});

  for (int i = 0; i < n_ray_files; ++i)
  {
    // Read the ray file
    std::string current_ray_filename =
        ray_filename + "-" + std::to_string(i) + ".txt";

    Kokkos::Profiling::pushRegion("ArborX::read_ray_files");
    auto rays = read_ray_file<MemorySpace>(current_ray_filename);
    Kokkos::Profiling::popRegion();
    unsigned int n_rays = rays.extent(0);

    Kokkos::View<float *, MemorySpace> distance(
        Kokkos::view_alloc(Kokkos::WithoutInitializing, "distance"), n_rays);
    Kokkos::deep_copy(distance, -1.);
    bvh.query(exec_space, Rays<MemorySpace>{rays},
              ProjectPointDistance<MemorySpace>{triangles, distance});

    Kokkos::View<ArborX::Point *, MemorySpace> point_cloud("point_cloud",
                                                           n_rays);
    Kokkos::parallel_for(
        "project_points", Kokkos::RangePolicy<ExecutionSpace>(0, n_rays),
        KOKKOS_LAMBDA(int i) {
          if (distance(i) >= 0)
          {
            point_cloud(i) = {
                rays(i)._origin[0] + distance(i) * rays(i)._direction[0],
                rays(i)._origin[1] + distance(i) * rays(i)._direction[1],
                rays(i)._origin[2] + distance(i) * rays(i)._direction[2]};
          }
          else
          {
            float max_value = KokkosExt::ArithmeticTraits::max<float>::value;
            point_cloud(i) = {max_value, max_value, max_value};
          }
        });

    auto point_cloud_host =
        Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, point_cloud);

    // Write results to file
    Kokkos::Profiling::pushRegion("ArborX::write_results");
    if ((output_type == "csv") || (output_type == "numpy"))
    {
      std::string delimiter = " ";
      if (output_type == "csv")
        delimiter = ",";
      else
        delimiter = " ";
      std::ofstream file;
      file.open(output_filename + "-" + std::to_string(i) + ".txt");
      outputTXT(point_cloud_host, file, delimiter);
      file.close();
    }
    else
    {
      std::ofstream file;
      file.open(output_filename + "-" + std::to_string(i) + ".vtk");
      outputVTK(point_cloud_host, file);
      file.close();
    }
    Kokkos::Profiling::popRegion();
  }
}


#include "../src/streamingcc_include/hash.h"
#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <random>
#include <map>
#include <iostream>
#include <vector>
#include <cstdint>

inline int Sign(uint32_t num) {
  return (num % 2) * 2 - 1;
}

BOOST_AUTO_TEST_CASE(murmurhash_Test) {
  using streamingcc::murmurhash;
  BOOST_CHECK(murmurhash(10, 20) == murmurhash(10, 20));
  BOOST_CHECK(murmurhash(10, 201) == murmurhash(10, 201));
  BOOST_CHECK(murmurhash(11, 20) != murmurhash(10, 20));
  BOOST_CHECK(murmurhash(10, 21) != murmurhash(10, 20));
  // test if roughly give 50% +-1
  const int kN = 1000;
  const uint32_t kSeed = 1;
  int count_neg = 0;
  for (uint32_t i = 0; i < kN; ++i) {
    if (Sign(murmurhash(i, kSeed)) < 0) ++count_neg;
  }
  BOOST_CHECK_MESSAGE(count_neg * 2 < kN + 70 && count_neg * 2 > kN - 70,
                      "negative/total = " << count_neg << "/" << kN);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestScatter
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/scatter.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/constant_buffer_iterator.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(scatter_int)
{
    int input_data[] = { 1, 2, 3, 4, 5 };
    bc::vector<int> input(input_data, input_data + 5, queue);

    int map_data[] = { 0, 4, 1, 3, 2 };
    bc::vector<int> map(map_data, map_data + 5, queue);

    bc::vector<int> output(5, context);
    bc::scatter(input.begin(), input.end(), map.begin(), output.begin(), queue);
    CHECK_RANGE_EQUAL(int, 5, output, (1, 3, 5, 4, 2));
}

BOOST_AUTO_TEST_CASE(scatter_constant_indices)
{
    int input_data[] = { 1, 2, 3, 4, 5 };
    bc::vector<int> input(input_data, input_data + 5, queue);

    int map_data[] = { 0, 4, 1, 3, 2 };
    bc::buffer map_buffer(context,
                          5 * sizeof(int),
                          bc::buffer::read_only | bc::buffer::use_host_ptr,
                          map_data);

    bc::vector<int> output(5, context);
    bc::scatter(input.begin(),
                input.end(),
                bc::make_constant_buffer_iterator<int>(map_buffer, 0),
                output.begin(),
                queue);
    CHECK_RANGE_EQUAL(int, 5, output, (1, 3, 5, 4, 2));
}

BOOST_AUTO_TEST_SUITE_END()