/* cpuid_test.cc
   Jeremy Barnes, 21 February 2007
   Copyright (c) 2007 Jeremy Barnes.  All rights reserved.

   Test of the CPUID detection code.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/arch/simd_vector.h"
#include "jml/arch/demangle.h"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <vector>
#include <set>
#include <iostream>
#include <cmath>

using namespace ML;
using namespace std;

using boost::unit_test::test_suite;

BOOST_AUTO_TEST_CASE( test1 )
{
    BOOST_CHECK(cpuid_flags() != 0);
}

void vec_add_k_mixed_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals];
    float y[nvals];

    double k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_mixed_test )
{
    vec_add_k_mixed_test_case(1);
    vec_add_k_mixed_test_case(2);
    vec_add_k_mixed_test_case(3);
    vec_add_k_mixed_test_case(4);
    vec_add_k_mixed_test_case(5);
    vec_add_k_mixed_test_case(8);
    vec_add_k_mixed_test_case(9);
    vec_add_k_mixed_test_case(12);
    vec_add_k_mixed_test_case(16);
    vec_add_k_mixed_test_case(123);
}

void vec_add_k_double_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals], y[nvals];

    double k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_double_test )
{
    vec_add_k_double_test_case(1);
    vec_add_k_double_test_case(2);
    vec_add_k_double_test_case(3);
    vec_add_k_double_test_case(4);
    vec_add_k_double_test_case(5);
    vec_add_k_double_test_case(8);
    vec_add_k_double_test_case(9);
    vec_add_k_double_test_case(12);
    vec_add_k_double_test_case(16);
    vec_add_k_double_test_case(123);
}

void vec_add_k_float_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    float x[nvals], r[nvals], r2[nvals], y[nvals];

    float k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_add_k_float_test )
{
    vec_add_k_float_test_case(1);
    vec_add_k_float_test_case(2);
    vec_add_k_float_test_case(3);
    vec_add_k_float_test_case(4);
    vec_add_k_float_test_case(5);
    vec_add_k_float_test_case(8);
    vec_add_k_float_test_case(9);
    vec_add_k_float_test_case(12);
    vec_add_k_float_test_case(16);
    vec_add_k_float_test_case(123);
}

void vec_prod_double_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    double x[nvals], r[nvals], r2[nvals], y[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] * y[i];
    }

    SIMD::vec_prod(x, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_prod_double_test )
{
    vec_prod_double_test_case(1);
    vec_prod_double_test_case(2);
    vec_prod_double_test_case(3);
    vec_prod_double_test_case(4);
    vec_prod_double_test_case(5);
    vec_prod_double_test_case(8);
    vec_prod_double_test_case(9);
    vec_prod_double_test_case(12);
    vec_prod_double_test_case(16);
    vec_prod_double_test_case(123);
}

void vec_prod_float_test_case(int nvals)
{
    cerr << "testing " << nvals << endl;

    float x[nvals], r[nvals], r2[nvals], y[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] * y[i];
    }

    SIMD::vec_prod(x, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

BOOST_AUTO_TEST_CASE( vec_prod_float_test )
{
    vec_prod_float_test_case(1);
    vec_prod_float_test_case(2);
    vec_prod_float_test_case(3);
    vec_prod_float_test_case(4);
    vec_prod_float_test_case(5);
    vec_prod_float_test_case(8);
    vec_prod_float_test_case(9);
    vec_prod_float_test_case(12);
    vec_prod_float_test_case(16);
    vec_prod_float_test_case(123);
}

float get_eps(float)
{
    return 1e-7;
}

double get_eps(double)
{
    return 1e-10;
}

template<typename T>
void vec_dotprod_test_case(int nvals)
{
    cerr << "nvals = " << nvals << endl;

    T x[nvals], y[nvals], r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod(x, y, nvals);

    T eps = get_eps(T());
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

template<typename T>
void vec_dotprod_dp_test_case(int nvals)
{
    T x[nvals], y[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod_dp(x, y, nvals);

    T eps = get_eps(T());
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

void vec_dotprod_dp_mixed_test_case(int nvals)
{
    double x[nvals];
    float y[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r += x[i] * y[i];
    }

    r2 = SIMD::vec_dotprod_dp(x, y, nvals);

    double eps = get_eps(0.0);
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

BOOST_AUTO_TEST_CASE(vec_dotprod_test)
{
    cerr << "float" << endl;
    vec_dotprod_test_case<float>(1);
    vec_dotprod_test_case<float>(2);
    vec_dotprod_test_case<float>(3);
    vec_dotprod_test_case<float>(4);
    vec_dotprod_test_case<float>(5);
    vec_dotprod_test_case<float>(8);
    vec_dotprod_test_case<float>(9);
    vec_dotprod_test_case<float>(12);
    vec_dotprod_test_case<float>(16);
    vec_dotprod_test_case<float>(123);

    cerr << "float dp" << endl;
    vec_dotprod_dp_test_case<float>(1);
    vec_dotprod_dp_test_case<float>(2);
    vec_dotprod_dp_test_case<float>(3);
    vec_dotprod_dp_test_case<float>(4);
    vec_dotprod_dp_test_case<float>(5);
    vec_dotprod_dp_test_case<float>(8);
    vec_dotprod_dp_test_case<float>(9);
    vec_dotprod_dp_test_case<float>(12);
    vec_dotprod_dp_test_case<float>(16);
    vec_dotprod_dp_test_case<float>(123);

    cerr << "double" << endl;
    vec_dotprod_test_case<double>(1);
    vec_dotprod_test_case<double>(2);
    vec_dotprod_test_case<double>(3);
    vec_dotprod_test_case<double>(4);
    vec_dotprod_test_case<double>(5);
    vec_dotprod_test_case<double>(8);
    vec_dotprod_test_case<double>(9);
    vec_dotprod_test_case<double>(12);
    vec_dotprod_test_case<double>(16);
    vec_dotprod_test_case<double>(123);

    cerr << "mixed" << endl;
    vec_dotprod_dp_mixed_test_case(1);
    vec_dotprod_dp_mixed_test_case(2);
    vec_dotprod_dp_mixed_test_case(3);
    vec_dotprod_dp_mixed_test_case(4);
    vec_dotprod_dp_mixed_test_case(5);
    vec_dotprod_dp_mixed_test_case(8);
    vec_dotprod_dp_mixed_test_case(9);
    vec_dotprod_dp_mixed_test_case(12);
    vec_dotprod_dp_mixed_test_case(16);
    vec_dotprod_dp_mixed_test_case(123);
}

template<typename T1, typename T2>
void vec_accum_prod3_test_case(int nvals)
{
    cerr << "testing vec_accum_prod3 nvals " << nvals << " T1 "
         << demangle(typeid(T1).name())
         << " T2 " << demangle(typeid(T2).name())
         << endl;

    T1 x[nvals], y[nvals];
    T2 z[nvals];
    double r = 0.0, r2;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        z[i] = rand() / 16384.0;
        r += x[i] * y[i] * z[i];
    }

    r2 = SIMD::vec_accum_prod3(x, y, z, nvals);

    T1 eps = get_eps(T1());
    if (fabs(r - r2) / max(fabs(r), fabs(r2)) >= eps) {
        cerr << "difference = " << (abs(r - r2) / max(abs(r), abs(r2)))
             << endl;
        BOOST_CHECK_EQUAL(r, r2);
    }
    BOOST_CHECK(fabs(r - r2) / max(fabs(r), fabs(r2)) < eps);
}

template<typename Float1, typename Float2>
void vec_accum_prod3_test_cases()
{
    vec_accum_prod3_test_case<Float1, Float2>(1);
    vec_accum_prod3_test_case<Float1, Float2>(2);
    vec_accum_prod3_test_case<Float1, Float2>(3);
    vec_accum_prod3_test_case<Float1, Float2>(4);
    vec_accum_prod3_test_case<Float1, Float2>(5);
    vec_accum_prod3_test_case<Float1, Float2>(8);
    vec_accum_prod3_test_case<Float1, Float2>(9);
    vec_accum_prod3_test_case<Float1, Float2>(12);
    vec_accum_prod3_test_case<Float1, Float2>(16);
    vec_accum_prod3_test_case<Float1, Float2>(123);
}

BOOST_AUTO_TEST_CASE(vec_accum_prod3_test)
{
    vec_accum_prod3_test_cases<float, float>();
    vec_accum_prod3_test_cases<float, double>();
    vec_accum_prod3_test_cases<double, float>();
    vec_accum_prod3_test_cases<double, double>();
}

template<typename Float1, typename Float2>
void vec_add_sqr_test_case(int nvals)
{
    cerr << "testing vec_add_sqr nvals " << nvals << " float1 "
         << demangle(typeid(Float1).name())
         << " float2 " << demangle(typeid(Float2).name())
         << endl;

    Float1 x[nvals], r[nvals], r2[nvals];
    Float2 y[nvals];

    Float1 k = 3.0;

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        r2[i] = x[i] + k * (y[i] * y[i]);
    }

    SIMD::vec_add_sqr(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) ;

        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float1, typename Float2>
void vec_add_sqr_test_cases()
{
    vec_add_sqr_test_case<Float1, Float2>(1);
    vec_add_sqr_test_case<Float1, Float2>(2);
    vec_add_sqr_test_case<Float1, Float2>(3);
    vec_add_sqr_test_case<Float1, Float2>(4);
    vec_add_sqr_test_case<Float1, Float2>(5);
    vec_add_sqr_test_case<Float1, Float2>(8);
    vec_add_sqr_test_case<Float1, Float2>(9);
    vec_add_sqr_test_case<Float1, Float2>(12);
    vec_add_sqr_test_case<Float1, Float2>(16);
    vec_add_sqr_test_case<Float1, Float2>(123);
}

BOOST_AUTO_TEST_CASE( vec_add_sqr_test )
{
    vec_add_sqr_test_cases<float, float>();
    vec_add_sqr_test_cases<float, double>();
    vec_add_sqr_test_cases<double, float>();
    vec_add_sqr_test_cases<double, double>();
}

template<typename Float1, typename Float2, typename Float3>
void vec_add_3array_test_case(int nvals)
{
    cerr << "testing vec_add_sqr nvals " << nvals << " float1 "
         << demangle(typeid(Float1).name())
         << " float2 " << demangle(typeid(Float2).name())
         << " float3 " << demangle(typeid(Float3).name())
         << endl;

    Float1 x[nvals], r[nvals], r2[nvals];
    Float2 y[nvals];
    Float3 k[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0;
        y[i] = rand() / 16384.0;
        k[i] = rand() / 16384.0;
        r2[i] = x[i] + k[i] * y[i];
    }

    SIMD::vec_add(x, k, y, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) cerr << "difference on element " << i << " of "
                                << nvals << endl;;

        BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float1, typename Float2, typename Float3>
void vec_add_3array_test_cases()
{
    vec_add_3array_test_case<Float1, Float2, Float3>(1);
    vec_add_3array_test_case<Float1, Float2, Float3>(2);
    vec_add_3array_test_case<Float1, Float2, Float3>(3);
    vec_add_3array_test_case<Float1, Float2, Float3>(4);
    vec_add_3array_test_case<Float1, Float2, Float3>(5);
    vec_add_3array_test_case<Float1, Float2, Float3>(8);
    vec_add_3array_test_case<Float1, Float2, Float3>(9);
    vec_add_3array_test_case<Float1, Float2, Float3>(12);
    vec_add_3array_test_case<Float1, Float2, Float3>(15);
    vec_add_3array_test_case<Float1, Float2, Float3>(16);
    vec_add_3array_test_case<Float1, Float2, Float3>(17);
    vec_add_3array_test_case<Float1, Float2, Float3>(123);
}

BOOST_AUTO_TEST_CASE( vec_add_3array_test )
{
    vec_add_3array_test_cases<float, float, float>();
    vec_add_3array_test_cases<float, double, float>();
    vec_add_3array_test_cases<double, float, float>();
    vec_add_3array_test_cases<double, double, float>();
    vec_add_3array_test_cases<float, float, double>();
    vec_add_3array_test_cases<float, double, double>();
    vec_add_3array_test_cases<double, float, double>();
    vec_add_3array_test_cases<double, double, double>();
}

template<typename Float, typename Precision>
void vec_exp_test_case(int nvals)
{
    cerr << "testing vec_exp " << nvals << " float "
         << demangle(typeid(Float).name())
         << " precision " << demangle(typeid(Precision).name())
         << endl;

    Float x[nvals];
    Precision r[nvals], r2[nvals];

    for (unsigned i = 0; i < nvals;  ++i) {
        x[i] = rand() / 16384.0 / 1024.0;
        r2[i] = exp(Precision(x[i]));
    }

    SIMD::vec_exp(x, r, nvals);

    for (unsigned i = 0;  i < nvals;  ++i) {
        if (r[i] != r2[i]) cerr << "difference on element " << i << " of "
                                << nvals << endl;;

        // ./arch/testing/simd_vector_test.cc(515): error in "vec_exp_test": check r[i] == r2[i] failed [1.21850755e+20 != 1.21850746e+20]
        //BOOST_CHECK_EQUAL(r[i], r2[i]);
    }
}

template<typename Float, typename Precision>
void vec_exp_test_cases()
{
    vec_exp_test_case<Float, Precision>(1);
    vec_exp_test_case<Float, Precision>(2);
    vec_exp_test_case<Float, Precision>(3);
    vec_exp_test_case<Float, Precision>(4);
    vec_exp_test_case<Float, Precision>(5);
    vec_exp_test_case<Float, Precision>(8);
    vec_exp_test_case<Float, Precision>(9);
    vec_exp_test_case<Float, Precision>(12);
    vec_exp_test_case<Float, Precision>(15);
    vec_exp_test_case<Float, Precision>(16);
    vec_exp_test_case<Float, Precision>(17);
    vec_exp_test_case<Float, Precision>(123);
}

BOOST_AUTO_TEST_CASE( vec_exp_test )
{
    vec_exp_test_cases<float, float>();
    vec_exp_test_cases<float, double>();
    vec_exp_test_cases<double, double>();
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/replace_if.hpp>
#include <boost/simd/pack.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

namespace bs = boost::simd;

template < typename T>
struct p
{
  using p_t =  bs::pack<T>;
  p(const T & val):val_(val),  pval_(val){}
  T val_;
  p_t pval_;

  bool                  operator ()(T a)          const { return a < val_; }
  bs::as_logical_t<p_t> operator ()(const p_t& a) const { return a < pval_; }
};

STF_CASE_TPL( "Check unary simd::replace_if", STF_NUMERIC_TYPES )
{
  static const int N = bs::pack<T>::static_size;

  std::vector<T> values(2*N+3), ref(2*N+3);
  std::iota(values.begin(), values.end(),T(1));
  std::iota(ref.begin(), ref.end(),T(1));
  {
    std::replace_if(ref.begin(), ref.end(), [](T e){ return e < N; }, T(0));
    boost::simd::replace_if(values.data(), values.data()+2*N+3, p<T>(N), T(0));

    STF_EQUAL( values, ref );
  }
  {
    std::replace_if(ref.begin(), ref.end(), [](T e){ return e < 1; }, T(0));
    boost::simd::replace_if(values.data(), values.data()+2*N+3, p<T>(1), T(0));

    STF_EQUAL( values, ref );
  }
  {
    std::replace_if(ref.begin(), ref.end(),  [](T e){ return e < 2*N-1; }, T(0));
    boost::simd::replace_if(values.data(), values.data()+2*N+3, p<T>(2*N-1), T(0));

    STF_EQUAL( values, ref );
  }
}


#include "Scene_points_with_normal_item.h"
#include "Polyhedron_type.h"
#include <CGAL/Polygon_mesh_processing/compute_normal.h>

#include <CGAL/IO/read_ply_points.h>
#include <CGAL/IO/write_ply_points.h>
#include <CGAL/IO/read_off_points.h>
#include <CGAL/IO/write_off_points.h>
#include <CGAL/IO/read_xyz_points.h>
#include <CGAL/IO/write_xyz_points.h>
#include <CGAL/Timer.h>
#include <CGAL/Memory_sizer.h>

#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>

#include <QObject>
#include <QMenu>
#include <QGLViewer/manipulatedCameraFrame.h>

#include <set>
#include <stack>
#include <algorithm>
#include <boost/array.hpp>

Scene_points_with_normal_item::Scene_points_with_normal_item()
    : Scene_item(NbOfVbos,NbOfVaos),
    m_points(new Point_set),
    m_has_normals(false)
{
  setRenderingMode(Points);
    is_selected = true;
    nb_points = 0;
    nb_selected_points = 0;
    nb_lines = 0;
}

// Copy constructor
Scene_points_with_normal_item::Scene_points_with_normal_item(const Scene_points_with_normal_item& toCopy)
    :Scene_item(NbOfVbos,NbOfVaos), // do not call superclass' copy constructor
    m_points(new Point_set(*toCopy.m_points)),
    m_has_normals(toCopy.m_has_normals)
{
  if (m_has_normals)
    {
    setRenderingMode(PointsPlusNormals);
        is_selected = true;
    }
  else
    {
    setRenderingMode(Points);
        is_selected = true;
    }
    nb_points = 0;
    nb_selected_points = 0;
    nb_lines = 0;
    invalidateOpenGLBuffers();
}

// Converts polyhedron to point set
Scene_points_with_normal_item::Scene_points_with_normal_item(const Polyhedron& input_mesh)
    : Scene_item(NbOfVbos,NbOfVaos),
    m_points(new Point_set),
    m_has_normals(true)
{
  // Converts Polyhedron vertices to point set.
  // Computes vertices normal from connectivity.

  Polyhedron::Vertex_iterator v;
  for (v = const_cast<Polyhedron&>(input_mesh).vertices_begin();
       v != const_cast<Polyhedron&>(input_mesh).vertices_end(); v++)
  {
    const Kernel::Point_3& p = v->point();
    Kernel::Vector_3 n =
      CGAL::Polygon_mesh_processing::compute_vertex_normal(v, input_mesh);
    m_points->push_back(UI_point(p,n));
  }

  setRenderingMode(PointsPlusNormals);
  is_selected = true;
  nb_points = 0;
  nb_selected_points = 0;
  nb_lines = 0;
  invalidateOpenGLBuffers();
}

Scene_points_with_normal_item::~Scene_points_with_normal_item()
{
  Q_ASSERT(m_points != NULL);
  delete m_points; m_points = NULL;
}

void Scene_points_with_normal_item::initialize_buffers(CGAL::Three::Viewer_interface *viewer) const
{
    compute_normals_and_vertices();
    //vao for the edges
    {
        program = getShaderProgram(PROGRAM_NO_SELECTION, viewer);
        program->bind();

        vaos[Edges]->bind();
        buffers[Edges_vertices].bind();
        buffers[Edges_vertices].allocate(positions_lines.data(),
                            static_cast<int>(positions_lines.size()*sizeof(double)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_DOUBLE,0,3);
        buffers[Edges_vertices].release();

        vaos[Edges]->release();
        nb_lines = positions_lines.size();
        positions_lines.resize(0);
        std::vector<double>(positions_lines).swap(positions_lines);
        program->release();
    }
    //vao for the points
    {
        program = getShaderProgram(PROGRAM_NO_SELECTION, viewer);
        program->bind();

        vaos[ThePoints]->bind();
        buffers[Points_vertices].bind();
        buffers[Points_vertices].allocate(positions_points.data(),
                            static_cast<int>(positions_points.size()*sizeof(double)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_DOUBLE,0,3);
        buffers[Points_vertices].release();
        vaos[ThePoints]->release();
        nb_points = positions_points.size();
        positions_points.resize(0);
        std::vector<double>(positions_points).swap(positions_points);
        program->release();
    }
    //vao for the selected points
    {
        program = getShaderProgram(PROGRAM_NO_SELECTION, viewer);
        program->bind();

        vaos[Selected_points]->bind();
        buffers[Selected_points_vertices].bind();
        buffers[Selected_points_vertices].allocate(positions_selected_points.data(),
                            static_cast<int>(positions_selected_points.size()*sizeof(double)));
        program->enableAttributeArray("vertex");
        program->setAttributeBuffer("vertex",GL_DOUBLE,0,3);
        buffers[Selected_points_vertices].release();

        vaos[Selected_points]->release();
        nb_selected_points = positions_selected_points.size();
        positions_selected_points.resize(0);
        std::vector<double>(positions_selected_points).swap(positions_selected_points);
        program->release();
    }
    are_buffers_filled = true;
}

void Scene_points_with_normal_item::compute_normals_and_vertices() const
{
    positions_points.resize(0);
    positions_lines.resize(0);
    positions_selected_points.resize(0);
    normals.resize(0);

    positions_points.reserve(m_points->size() * 3);
    positions_lines.reserve(m_points->size() * 3 * 2);

    //Shuffle container to allow quick display random points
    Point_set_3<Kernel> points = *m_points;
    std::random_shuffle (points.begin(), points.end() - m_points->nb_selected_points());
    std::random_shuffle (points.end() - m_points->nb_selected_points(), points.end());

    //The points
    {
        // The *non-selected* points
      for (Point_set_3<Kernel>::const_iterator it = points.begin(); it != points.first_selected(); it++)
	{
	  const UI_point& p = *it;
	  positions_points.push_back(p.x());
	  positions_points.push_back(p.y());
	  positions_points.push_back(p.z());
	}

        // Draw *selected* points
      for (Point_set_3<Kernel>::const_iterator it = points.first_selected(); it != points.end(); it++)
	{
	  const UI_point& p = *it;
	  positions_selected_points.push_back(p.x());
	  positions_selected_points.push_back(p.y());
	  positions_selected_points.push_back(p.z());
	}
    }

    //The lines
    {
        // Stock normals
        Kernel::Sphere_3 region_of_interest = points.region_of_interest();
        float normal_length = (float)std::sqrt(region_of_interest.squared_radius() / 1000.);

	for (Point_set_3<Kernel>::const_iterator it = points.begin(); it != points.end(); it++)
	  {
	    const UI_point& p = *it;
	    const Point_set_3<Kernel>::Vector& n = p.normal();
	    Point_set_3<Kernel>::Point q = p + normal_length * n;
	    positions_lines.push_back(p.x());
	    positions_lines.push_back(p.y());
	    positions_lines.push_back(p.z());

	    positions_lines.push_back(q.x());
	    positions_lines.push_back(q.y());
	    positions_lines.push_back(q.z());
	  }
    }
}

// Duplicates scene item
Scene_points_with_normal_item*
Scene_points_with_normal_item::clone() const
{
  return new Scene_points_with_normal_item(*this);
}

// Is selection empty?
bool Scene_points_with_normal_item::isSelectionEmpty() const
{
  return (m_points->nb_selected_points() == 0);
}

// Delete selection
void Scene_points_with_normal_item::deleteSelection()
{
  CGAL::Timer task_timer; task_timer.start();
  std::cerr << "Delete " << m_points->nb_selected_points() << " points...";

  // Delete selected points
  m_points->delete_selection();

  std::size_t memory = CGAL::Memory_sizer().virtual_size();
  std::cerr << "done: " << task_timer.time() << " seconds, "
                        << (memory>>20) << " Mb allocated"
                        << std::endl;
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Invert selection
void Scene_points_with_normal_item::invertSelection()
{
  m_points->invert_selection();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Select everything
void Scene_points_with_normal_item::selectAll()
{
  m_points->select_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
// Reset selection mark
void Scene_points_with_normal_item::resetSelection()
{
  // Un-select all points
  m_points->unselect_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
  //Select duplicated points
void Scene_points_with_normal_item::selectDuplicates()
{
  std::set<Kernel::Point_3> unique_points;
  for (Point_set::iterator ptit=m_points->begin(); ptit!=m_points->end();++ptit )
    if ( !unique_points.insert(*ptit).second )
      m_points->select(ptit);
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Loads point set from .PLY file
bool Scene_points_with_normal_item::read_ply_point_set(std::istream& stream)
{
  Q_ASSERT(m_points != NULL);

  m_points->clear();
  bool ok = stream &&
            CGAL::read_ply_points_and_normals(stream,
                                              std::back_inserter(*m_points),
                                              CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type())) &&
            !isEmpty();
  if (ok)
    {
      for (Point_set::iterator it=m_points->begin(),
             end=m_points->end();it!=end; ++it)
        {
          if (it->normal() != CGAL::NULL_VECTOR)
            {
              m_has_normals=true;
              setRenderingMode(PointsPlusNormals);
              break;
            }
        }
    }
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .PLY file
bool Scene_points_with_normal_item::write_ply_point_set(std::ostream& stream) const
{
  Q_ASSERT(m_points != NULL);

  return stream &&
         CGAL::write_ply_points_and_normals(stream,
                                            m_points->begin(), m_points->end(),
                                            CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type()));
}

// Loads point set from .OFF file
bool Scene_points_with_normal_item::read_off_point_set(std::istream& stream)
{
  Q_ASSERT(m_points != NULL);

  m_points->clear();
  bool ok = stream &&
            CGAL::read_off_points_and_normals(stream,
                                              std::back_inserter(*m_points),
                                              CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type())) &&
            !isEmpty();
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .OFF file
bool Scene_points_with_normal_item::write_off_point_set(std::ostream& stream) const
{
  Q_ASSERT(m_points != NULL);

  return stream &&
         CGAL::write_off_points_and_normals(stream,
                                            m_points->begin(), m_points->end(),
                                            CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type()));
}

// Loads point set from .XYZ file
bool Scene_points_with_normal_item::read_xyz_point_set(std::istream& stream)
{
  Q_ASSERT(m_points != NULL);

  m_points->clear();
  bool ok = stream &&
            CGAL::read_xyz_points_and_normals(stream,
                                              std::back_inserter(*m_points),
                                              CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type())) &&
            !isEmpty();

  if (ok)
  {
    for (Point_set::iterator it=m_points->begin(),
                             end=m_points->end();it!=end; ++it)
    {
      if (it->normal() != CGAL::NULL_VECTOR)
      {
        m_has_normals=true;
        setRenderingMode(PointsPlusNormals);
        break;
      }
    }
  }
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .XYZ file
bool Scene_points_with_normal_item::write_xyz_point_set(std::ostream& stream) const
{
  Q_ASSERT(m_points != NULL);

  return stream &&
         CGAL::write_xyz_points_and_normals(stream,
                                            m_points->begin(), m_points->end(),
                                            CGAL::make_normal_of_point_with_normal_pmap(Point_set::value_type()));
}

QString
Scene_points_with_normal_item::toolTip() const
{
  Q_ASSERT(m_points != NULL);

  return QObject::tr("
%1 (color: %4)
"
                     "Point set
"
                     "
Number of points: %2
")
    .arg(name())
    .arg(m_points->size())
    .arg(color().name());
}

bool Scene_points_with_normal_item::supportsRenderingMode(RenderingMode m) const
{
    return m==Points ||
            ( has_normals() &&
              ( m==PointsPlusNormals || m==Splatting ) );
}

void Scene_points_with_normal_item::draw_splats(CGAL::Three::Viewer_interface* viewer) const
{
   // TODO add support for selection
   viewer->glBegin(GL_POINTS);
   for ( Point_set_3<Kernel>::const_iterator it = m_points->begin(); it != m_points->end(); it++)
   {
     const UI_point& p = *it;
     viewer->glNormal3dv(&p.normal().x());
     viewer->glMultiTexCoord1d(GL_TEXTURE2, p.radius());
     viewer->glVertex3dv(&p.x());

   }
   viewer->glEnd();

}

void Scene_points_with_normal_item::draw_edges(CGAL::Three::Viewer_interface* viewer) const
{
    double ratio_displayed = 1.0;
    if (viewer->inFastDrawing () &&
        (nb_lines/6 > 300000)) // arbitrary large value
      ratio_displayed = 6 * 300000. / (double)(nb_lines);

    if(!are_buffers_filled)
        initialize_buffers(viewer);
    vaos[Edges]->bind();
    program=getShaderProgram(PROGRAM_NO_SELECTION);
    attrib_buffers(viewer,PROGRAM_NO_SELECTION);
    program->bind();
    program->setAttributeValue("colors", this->color());
    viewer->glDrawArrays(GL_LINES, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * nb_lines)/3)));
    vaos[Edges]->release();
    program->release();
}
void Scene_points_with_normal_item::draw_points(CGAL::Three::Viewer_interface* viewer) const
{
    if(!are_buffers_filled)
        initialize_buffers(viewer);

    double ratio_displayed = 1.0;
    if (viewer->inFastDrawing () &&
        ((nb_points + nb_selected_points)/3 > 300000)) // arbitrary large value
      ratio_displayed = 3 * 300000. / (double)(nb_points + nb_selected_points);

    vaos[ThePoints]->bind();
    program=getShaderProgram(PROGRAM_NO_SELECTION);
    attrib_buffers(viewer,PROGRAM_NO_SELECTION);
    program->bind();
    program->setAttributeValue("colors", this->color());
    viewer->glDrawArrays(GL_POINTS, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * nb_points)/3)));
    vaos[ThePoints]->release();
    program->release();
    GLfloat point_size;
    viewer->glGetFloatv(GL_POINT_SIZE, &point_size);
    viewer->glPointSize(4.f);

    vaos[Selected_points]->bind();
    program=getShaderProgram(PROGRAM_NO_SELECTION);
    attrib_buffers(viewer,PROGRAM_NO_SELECTION);
    program->bind();
    program->setAttributeValue("colors", QColor(255,0,0));
    viewer->glDrawArrays(GL_POINTS, 0,
                         static_cast<GLsizei>(((std::size_t)(ratio_displayed * nb_selected_points)/3)));
    vaos[Selected_points]->release();
    program->release();
    viewer->glPointSize(point_size);
}
// Gets wrapped point set
Point_set* Scene_points_with_normal_item::point_set()
{
  Q_ASSERT(m_points != NULL);
  return m_points;
}
const Point_set* Scene_points_with_normal_item::point_set() const
{
  Q_ASSERT(m_points != NULL);
  return m_points;
}

bool
Scene_points_with_normal_item::isEmpty() const
{
  Q_ASSERT(m_points != NULL);
  return m_points->empty();
}

void
Scene_points_with_normal_item::compute_bbox() const
{
  Q_ASSERT(m_points != NULL);

  Kernel::Iso_cuboid_3 bbox = m_points->bounding_box();
  _bbox = Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
              bbox.xmax(),bbox.ymax(),bbox.zmax());
}

void Scene_points_with_normal_item::computes_local_spacing(int k)
{
  typedef Kernel Geom_traits;
  typedef CGAL::Search_traits_3<Geom_traits> TreeTraits;
  typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> Neighbor_search;
  typedef Neighbor_search::Tree Tree;

  Point_set::iterator end(m_points->end());

  // build kdtree
  Tree tree(m_points->begin(), end);

  // Compute the radius of each point = (distance max to k nearest neighbors)/2.
  {
    int i=0;
    for (Point_set::iterator it=m_points->begin(); it!=end; ++it, ++i)
    {
      Neighbor_search search(tree, *it, k+1);
      double maxdist2 = (--search.end())->second; // squared distance to furthest neighbor
      it->radius() = sqrt(maxdist2)/2.;
    }
  }

  m_points->set_radii_uptodate(true);
}

QMenu* Scene_points_with_normal_item::contextMenu()
{
    const char* prop_name = "Menu modified by Scene_points_with_normal_item.";

    QMenu* menu = Scene_item::contextMenu();

    // Use dynamic properties:
    // http://doc.qt.io/qt-5/qobject.html#property
    bool menuChanged = menu->property(prop_name).toBool();

    if(!menuChanged) {
        actionDeleteSelection = menu->addAction(tr("Delete Selection"));
        actionDeleteSelection->setObjectName("actionDeleteSelection");
        connect(actionDeleteSelection, SIGNAL(triggered()),this, SLOT(deleteSelection()));

        actionResetSelection = menu->addAction(tr("Reset Selection"));
        actionResetSelection->setObjectName("actionResetSelection");
        connect(actionResetSelection, SIGNAL(triggered()),this, SLOT(resetSelection()));

        actionSelectDuplicatedPoints = menu->addAction(tr("Select duplicated points"));
        actionSelectDuplicatedPoints->setObjectName("actionSelectDuplicatedPoints");
        connect(actionSelectDuplicatedPoints, SIGNAL(triggered()),this, SLOT(selectDuplicates()));

        menu->setProperty(prop_name, true);
    }

    if (isSelectionEmpty())
    {
        actionDeleteSelection->setDisabled(true);
        actionResetSelection->setDisabled(true);
    }
    else
    {
        actionDeleteSelection->setDisabled(false);
        actionResetSelection->setDisabled(false);
    }

    return menu;
}

void Scene_points_with_normal_item::setRenderingMode(RenderingMode m)
{
    Scene_item::setRenderingMode(m);
    if (rendering_mode==Splatting && (!m_points->are_radii_uptodate()))
    {
        computes_local_spacing(6); // default value = small
    }
}

bool Scene_points_with_normal_item::has_normals() const { return m_has_normals; }

void Scene_points_with_normal_item::set_has_normals(bool b) {
  if (b!=m_has_normals){
    m_has_normals=b;
    //reset the context menu
    delete this->defaultContextMenu;
    this->defaultContextMenu = 0;
  }
}

void Scene_points_with_normal_item::invalidateOpenGLBuffers()
{
    are_buffers_filled = false;
    compute_bbox();
}


// Copyright (c) 2012-2013 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "mruset.h"

#include "util.h"

#include <set>

#include <boost/test/unit_test.hpp>

#define NUM_TESTS 16
#define MAX_SIZE 100

using namespace std;

class mrutester {
 public:
	mrutester() {
		m_nMru.max_size(MAX_SIZE);
	}

	int size() const {
		return m_nSet.size();
	}

	void insert(int n) {
		m_nMru.insert(n);
		m_nSet.insert(n);
		BOOST_CHECK(m_nMru == m_nSet);
	}

 private:
	mruset<int> m_nMru;
	std::set<int> m_nSet;
};

BOOST_AUTO_TEST_SUITE(mruset_tests)

// Test that an mruset behaves like a set, as long as no more than MAX_SIZE elements are in it
BOOST_AUTO_TEST_CASE(mruset_like_set) {
	for (int nTest = 0; nTest < NUM_TESTS; nTest++) {
		mrutester cTester;
		while (cTester.size() < MAX_SIZE) {
			cTester.insert(GetRandInt(2 * MAX_SIZE));
		}
	}
}

// Test that an mruset's size never exceeds its max_size
BOOST_AUTO_TEST_CASE(mruset_limited_size) {
	for (int nTest = 0; nTest < NUM_TESTS; nTest++) {
		mruset<int> mru(MAX_SIZE);
		for (int nAction = 0; nAction < 3 * MAX_SIZE; nAction++) {
			int n = GetRandInt(2 * MAX_SIZE);
			mru.insert(n);
			BOOST_CHECK(mru.size() <= MAX_SIZE);
		}
	}
}

// 16-bit permutation function
int static permute(int n) {
	// hexadecimals of pi; verified to be linearly independent
	static const int sarrnTable[16] = {
			0x243F, 0x6A88, 0x85A3, 0x08D3, 0x1319, 0x8A2E, 0x0370, 0x7344,
			0xA409, 0x3822,	0x299F, 0x31D0, 0x082E, 0xFA98, 0xEC4E, 0x6C89 };

	int nRet = 0;
	for (int bit = 0; bit < 16; bit++) {
		if (n & (1 << bit)) {
			nRet ^= sarrnTable[bit];
		}
	}
	return nRet;
}

// Test that an mruset acts like a moving window, if no duplicate elements are added
BOOST_AUTO_TEST_CASE(mruset_window) {
	mruset<int> mru(MAX_SIZE);
	for (int n = 0; n < 10 * MAX_SIZE; n++) {
		mru.insert(permute(n));
		set<int> tester;
		for (int m = max(0, n - MAX_SIZE + 1); m <= n; m++)
			tester.insert(permute(m));

		BOOST_CHECK(mru == tester);
	}
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ROR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ROR_HPP_INCLUDED

#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( ror_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::unsigned_<A0> >
                          , bd::scalar_< bd::unsigned_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "ror : rotation is out of range");

      static const A0 width = sizeof(A0)*CHAR_BIT-1;
      A0 n = A0(a1);
      return (a0 >>  n) | (a0 << (-n&width));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ror_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const
    {
      using i_t = bd::as_integer_t<A0, unsigned>;
      return bitwise_cast<A0>( ror (bitwise_cast<i_t>(a0), i_t(a1)) );
    }
  };

} } }

#endif


#include <iostream>
#include <vector>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

auto in = [](const auto &min, const auto &max, char const * const opt_name){
    return [opt_name, min, max](const auto &v){
        if(v < min || v > max){
            throw po::validation_error
                    (po::validation_error::invalid_option_value,
                     opt_name, std::to_string(v));
        }
    };
};

/**
 * Returns true if counters are reached the max
 *
 */
bool vectorIncrement(std::vector<int> &counters, std::vector<int> &lb, std::vector<int> &up, std::vector<int> &inc) {

    for (int i=counters.size() - 1; i >= 0; i--) {
        counters.at(i) = counters.at(i) + inc.at(i);
        if (counters.at(i) > up.at(i)) {
            if (i == 0) {
                return true; // done
            }
            counters.at(i) = lb.at(i);
        }  else {
            break;
        }
    }
    return false;
}

void computeJobSizes(int num_cluster_nodes, int num_sizes, int space_to_leave) {
    std::vector<int> lb, ub, inc; // lower bound, upper bound, increment
    std::vector<int> candidates;

    int num_found = 0;
    int last_seen_value = -1;

    // Set up lb
    lb.push_back(2);
    for (int i=1; i < num_sizes; i++) {
        lb.push_back(1);
    }

    // Set up up
    for (int i=0; i < num_sizes; i++) {
        if (i < 2) {
            //ub.push_back(num_cluster_nodes/4);
            ub.push_back(num_cluster_nodes);
        } else if (i < 4) {
            //ub.push_back(num_cluster_nodes/2);
            ub.push_back(num_cluster_nodes);
        } else {
            ub.push_back(num_cluster_nodes);
        }
    }

    // Set up inc
    for (int i=0; i < num_sizes; i++) {
        //inc.push_back(1 + i % 2);
        inc.push_back(1);
    }

    // Initialize candidate
    for (int i=0; i < num_sizes; i++) {
        candidates.push_back(lb.at(i));
    }

    // Two helper vectors
    std::vector<int> zeros;
    std::vector<int> ones;
    std::vector<int> maxes;
    for (int i=0; i < num_sizes; i++) {
        zeros.push_back(0);
        ones.push_back(1);
        maxes.push_back(num_cluster_nodes);
    }

//    std::cerr << "LOWER BOUNDS: ";
//    for (auto const s : lb) {
//        std::cerr << s << " ";
//    }
//    std::cerr << "\n";
//    std::cerr << "UPPER BOUNDS: ";
//    for (auto const s : ub) {
//        std::cerr << s << " ";
//    }
//    std::cerr << "\n";

    // Do it
    while (true) {
          if (candidates.at(0) != last_seen_value) {
              last_seen_value = candidates.at(0);
              std::cerr << "progress: " << last_seen_value << "/" << num_cluster_nodes << "\n";
          }

//        std::cerr << "CANDIDATE: ";
//        for (auto const s : candidates) {
//                std::cerr << s << " ";
//        }
//        std::cerr << "\n";

        // If minimum is > half capacity, nevermind (that would mean only one job at a time!)
        if (*(std::min_element(candidates.begin(), candidates.end())) < num_cluster_nodes / 3) {

            // Check that space is always left
            std::vector<int> num_jobs;
            num_jobs.reserve(num_sizes);
            for (int i=0; i < num_sizes; i++) {
                num_jobs.push_back(0);
            }
            bool enough_space_left = true;
            while(true) {
//                std::cerr << "   NUM JOBS: ";
//                for (auto const s : num_jobs) {
//                        std::cerr << s << " ";
//                }
//                std::cerr << "  ";
                int occupied_space = 0;
                for (int i=0; i < num_sizes; i++) {
                    occupied_space += num_jobs.at(i) * candidates.at(i);
                }
//                std::cerr << "occupied space: " << occupied_space << "\n";
                if (occupied_space <= num_cluster_nodes) {
                    if (occupied_space > num_cluster_nodes - space_to_leave) {
                        enough_space_left = false;
                        break;
                    }
                }
                if (vectorIncrement(num_jobs, zeros, maxes, ones)) {
                    break;
                }
            }

            // If enough space is left, we found an option
            if (enough_space_left) {
                // Sanity check it
                bool all_unique = true;
                for (int i=0; i < candidates.size(); i++) {
                    for (int j=i+1; j < candidates.size(); j++) {
                        if (candidates.at(i) == candidates.at(j)) {
                            all_unique = false;
                            break;
                        }
                    }
                }
                if (all_unique) {
                    std::cout << "FOUND ONE: ";
                    for (auto const s : candidates) {
                        std::cout << s << ", ";
                    }
                    std::cout << "\n";
                    num_found++;
                }
            }
        }

        // Increment candidate
        if (vectorIncrement(candidates, lb, ub, inc)) {
            break;
        }
    }
    std::cout << "NUM FOUND: " << num_found << "\n";

}

int main(int argc, char **argv) {
    int num_cluster_nodes;
    int num_job_sizes;
    int space_to_leave;

    // Parse command-line arguments
    po::options_description desc("Allowed options (warning, this program is brute-force/high-complexity");
    desc.add_options()
            ("help", "show help message")
            ("nodes", po::value<int>()->notifier(
                    in(1, INT_MAX, "nodes")), "number of compute nodes in the cluster")
            ("numsizes", po::value<int>()->notifier(
                    in(1, INT_MAX, "numsizes")), "number of different sizes needed")
            ("leftover", po::value<int>()->notifier(
                    in(1, INT_MAX, "leftover")), "guaranteed freed nodes")
            ;

    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);
    } catch (std::exception &e) {
        std::cerr << "Error: " << e.what() << "\n";
        return 1;
    }

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

    if (!vm.count("nodes") or !vm.count("numsizes") or !vm.count("leftover")) {
        std::cerr << "Missing arguments\n";
        exit(1);
    }

    num_cluster_nodes = vm["nodes"].as<int>();
    num_job_sizes = vm["numsizes"].as<int>();
    space_to_leave = vm["leftover"].as<int>();

    computeJobSizes(num_cluster_nodes, num_job_sizes, space_to_leave);
    std::cout << "\n";
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <Eigen/Dense>
#include <fstream>
#include <vector>
#include <vinecopulib/misc/nlohmann_json.hpp>
#include <vinecopulib/misc/triangular_array.hpp>

namespace vinecopulib {

namespace tools_serialization {

//! conversion from Eigen::Matrix to nlohmann::json
//!
//! @param matrix The Eigen::Matrix to convert.
//! @return the corresponding nlohmann::json.
template<class T>
inline nlohmann::json
matrix_to_json(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix)
{

  nlohmann::json output;
  output["shape"] = { matrix.rows(), matrix.cols() };
  nlohmann::json json_data;
  auto it = matrix.data();
  auto last = matrix.data() + matrix.size();
  for (; it != last; ++it) {
    json_data.push_back(*it);
  }
  output["data"] = json_data;

  return output;
}

//! conversion from vinecopulib::TriangularArray to nlohmann::json
//!
//! @param array The vinecopulib::TriangularArray to convert.
//! @return the corresponding nlohmann::json.
template<class T>
inline nlohmann::json
triangular_array_to_json(const TriangularArray<T>& array)
{
  nlohmann::json output;
  size_t d = array.get_dim();
  size_t trunc_lvl = array.get_trunc_lvl();
  output["d"] = d;
  output["t"] = trunc_lvl;

  nlohmann::json json_data;
  for (size_t i = 0; i < std::min(d - -1, trunc_lvl); i++) {
    nlohmann::json row;
    for (size_t j = 0; j < d - 1 - i; j++) {
      row.push_back(array(i, j));
    }
    json_data.push_back(row);
  }
  output["data"] = json_data;

  return output;
}

//! conversion from std::vector to nlohmann::json
//!
//! @param vec The std::vector to convert.
//! @return the corresponding nlohmann::json.
template<class T>
inline nlohmann::json
vector_to_json(const std::vector<T>& vec)
{
  nlohmann::json output = vec;
  return output;
}

//! conversion from nlohmann::json to Eigen::Matrix
//!
//! @param input The nlohmann::json to convert.
//! @return the corresponding Eigen::Matrix.
template<typename T>
inline Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
json_to_matrix(const nlohmann::json& input)
{

  size_t rows = input["shape"][0];
  size_t cols = input["shape"][1];
  std::vector<double> vec = input["data"];

  Eigen::MatrixXd matrix;
  matrix = Eigen::MatrixXd::Map(&vec[0], rows, cols);

  return matrix.cast<T>();
}

//! conversion from nlohmann::json to vinecopulib::TriangularArray
//!
//! @param input The nlohmann::json to convert.
//! @return the corresponding vinecopulib::TriangularArray
template<typename T>
inline TriangularArray<T>
json_to_triangular_array(const nlohmann::json& input)
{

  std::vector<std::vector<T>> vec = input["data"];
  return TriangularArray<T>(vec);
}

//! conversion from nlohmann::json to std::vector
//!
//! @param input The nlohmann::json to convert.
//! @return the corresponding std::vector.
template<typename T>
inline std::vector<T>
json_to_vector(const nlohmann::json& input)
{

  std::vector<T> res = input;
  return res;
}

inline nlohmann::json
file_to_json(const std::string& filename)
{
  nlohmann::json output;
  std::ifstream file(filename);
  file >> output;
  return output;
}

inline void
json_to_file(const std::string& filename, const nlohmann::json& json)
{
  std::ofstream file(filename);
  file << json << std::endl;
}
}
}


#include <ros/ros.h>
#include <iostream>
#include <fstream>
//#include <pcl/console/parse.h>
//#include <pcl/filters/extract_indices.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/sample_consensus/ransac.h>
//#include <pcl/sample_consensus/sac_model_plane.h>
//#include <pcl/sample_consensus/sac_model_sphere.h>
#include <algorithm>
#include <vector>
#include <string>
#include <numeric>
#include <cmath>
#include <time.h>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <dirent.h>

using namespace cv;
using namespace std;

clock_t t[100];

//read 3D points from pcd file
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadPCD(char* filePath)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ> (filePath, *cloud) == -1)
	{
		PCL_ERROR("Couldn't read file box_cloud.pcd \n");

	}
	return cloud;
}

//read 3D points from ply file
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadPLY(char* filePath)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PLYReader Reader;
	Reader.read(filePath, *cloud);
	return cloud;
}

//k nearest neighbor search
void PointsKNN(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int** indexKNN, double** distKNN)
{
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud (cloud);
	pcl::PointXYZ searchPoint;
	std::vector<int> pointIdxNKNSearch(maxK);
  	std::vector<float> pointNKNSquaredDistance(maxK);
	for(int i=0; i<pointNum; i++)
	{
		searchPoint.x=cloud->points[i].x;
		searchPoint.y=cloud->points[i].y;
		searchPoint.z=cloud->points[i].z;
		if(kdtree.nearestKSearch (searchPoint, maxK, pointIdxNKNSearch, pointNKNSquaredDistance)>0)
		{
			for(int j=0; j<maxK; j++)
			{
				indexKNN[i][j]=pointIdxNKNSearch[j];
				distKNN[i][j]=pointNKNSquaredDistance[j];
			}

		}
	}

	/*for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<indexKNN[i][j];
		}
		cout<<endl;
	}
	cout<<endl;
	for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<distKNN[i][j];
		}
		cout<<endl;
	}*/
}

//compute sigma and NWR matrix, then get the normals of the points
cv::Mat NormalEstimation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int** indexKNN, double** distKNN)
{
	cv::Mat diffM(3, 1, CV_64FC1);
	cv::Mat referP(3, 1, CV_64FC1);
	cv::Mat neighP(3, 1, CV_64FC1);
	cv::Mat nwrSample(3, 3, CV_64FC1);
	cv::Mat nwrMat(3, 3, CV_64FC1);
	cv::Mat normals(pointNum, 3, CV_64FC1);
	double sigma=0.2;
	//compute sigma
	//int nn;    //number of nonzero distance
	/*for(int i=0; i<pointNum; i++)
	{
		nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[i][j]!=0)
			{
				nn++;
			}
			if(nn==k)
			{
				sigma=distKNN[i][j]+sigma;
				break;
			}
		}
	}
	sigma=sigma/pointNum;*/
	//compute NWR Matrix
	t[10]=clock();
	//randomly pick some points
	//std::vector<int> randPoints;
	/*for(int i=0; i<pointNum; i++)
	{
		randPoints.push_back(i);
	}
	std::random_shuffle(randPoints.begin(), randPoints.end());
	pointNum=pointNum*randRate;*/
	//cout<<"randRate: "<<randRate<<endl;
	//cout<<"pointNum: "<<pointNum<<endl;
	for(int i=0; i<pointNum; i++)
	{
		referP.at<double>(0,0)=cloud->points[i].x;
		referP.at<double>(1,0)=cloud->points[i].y;
		referP.at<double>(2,0)=cloud->points[i].z;
		nwrMat=cv::Mat::zeros(cv::Size(3,3),CV_64FC1);
		//nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[i][j]!=0)
			{
				neighP.at<double>(0,0)=cloud->points[indexKNN[i][j]].x;
				neighP.at<double>(1,0)=cloud->points[indexKNN[i][j]].y;
				neighP.at<double>(2,0)=cloud->points[indexKNN[i][j]].z;
				diffM=neighP-referP;
				nwrSample=diffM*diffM.t();
				nwrSample=nwrSample/(distKNN[i][j]*distKNN[i][j]);
				nwrSample=nwrSample*exp(-(distKNN[i][j]*distKNN[i][j])/(2*sigma*sigma));
				nwrMat=nwrMat+nwrSample;
				//nn++;
			}
			/*if(nn==k)
			{
				break;
			}*/

		}
		//compute eigen value
		cv::Mat eigenvalue, eigenvector;
		cv::eigen(nwrMat, eigenvalue, eigenvector);
		//cout<<endl;
		//cout<<nwrMat<<endl;
		//cout<<endl;
		//cout<<eigenvalue<<endl;
		//cout<<endl;
		//cout<<eigenvector<<endl;
		normals.at<double>(i,0)=eigenvector.at<double>(2,0);
		normals.at<double>(i,1)=eigenvector.at<double>(2,1);
		normals.at<double>(i,2)=eigenvector.at<double>(2,2);
	}
	t[11]=clock();
	//printf("NormalEstimation: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
	return normals;
}

/*//Open 3D viewer and add point cloud
boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0, "global");
  viewer->initCameraParameters ();
  return (viewer);
}*/

//implement one point ransac
cv::Mat OnePointRANSAC(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori, cv::Mat horiPoints, int maxIteration, double p, double disThreshold, int fitNum, std::vector<int> &best_inliers, std::vector<int> isExist)
{
	int iter=0;
	//plane which ransac find: planeModel is the index of the point on the plane and the plane normal
	cv::Mat planeModel = cv::Mat::zeros(2, 3, CV_64FC1); //The first row is the point on the plane, the second row is the normal of the plane
	int inNum=0;
	std::vector<int> inliers;
	int randInd;
	double e;
	while(iter<maxIteration)
	{
		//randomly select one point from horizontal points
		//t[12]=clock();
		randInd=rand()%cloud_hori->points.size();
		inliers.clear();
		for(int i=0; i<cloud_hori->points.size(); i++)
		{
			if(i==randInd || isExist[i]==0)
			{
				continue;
			}
			//compute the distance between every point to the plane
			double x=cloud_hori->points[i].x-cloud_hori->points[randInd].x;
			double y=cloud_hori->points[i].y-cloud_hori->points[randInd].y;
			double z=cloud_hori->points[i].z-cloud_hori->points[randInd].z;
			double xn=horiPoints.at<double>(randInd,0);
			double yn=horiPoints.at<double>(randInd,1);
			double zn=horiPoints.at<double>(randInd,2);
			double norm_n=sqrt(xn*xn+yn*yn+zn*zn);
			double dis=abs(x*xn+y*yn+z*zn)/norm_n;
			if (dis<disThreshold)
			{
				inliers.push_back(i);
			}
		}
		if (inliers.size()>fitNum)
		{
			inliers.push_back(randInd);
			//cout<<"inliers number: "<<inliers.size()<<endl;
			//e=1-(double(inliers.size())/cloud->points.size());
			//cout<<"e: "<<e<<endl;
			//maxIteration=log(1-p)/log(1-(1-e));
			if (inliers.size()>inNum)
			{
				inNum=inliers.size();
				best_inliers=inliers;
				cout<<"inliers number: "<<inliers.size()<<endl;
				e=1-(double(inliers.size())/cloud_hori->points.size());
				cout<<"e: "<<e<<endl;
				maxIteration=log(1-p)/log(1-(1-e));
			}
		}
		//t[13]=clock();
		//printf("OnePointRANSAC one iteration time: %lf s\n",(double)(t[13]-t[12])/CLOCKS_PER_SEC);
		iter++;
	}
	cout<<"iteration times: "<<iter<<endl;
	cout<<"maxIteration: "<<maxIteration<<endl;
	//if do not find any plane
	if (inNum==0)
	{
		return planeModel;
	}
	//re-estimate the plane using all the inliers points
	cv::Mat pointsInlier(best_inliers.size(), 3,  CV_64FC1);
	double colSum[3]={0,0,0};
	for(int i=0; i<best_inliers.size(); i++)
	{
		pointsInlier.at<double>(i,0)=cloud_hori->points[best_inliers[i]].x;
		pointsInlier.at<double>(i,1)=cloud_hori->points[best_inliers[i]].y;
		pointsInlier.at<double>(i,2)=cloud_hori->points[best_inliers[i]].z;
		colSum[0]+=cloud_hori->points[best_inliers[i]].x;
		colSum[1]+=cloud_hori->points[best_inliers[i]].y;
		colSum[2]+=cloud_hori->points[best_inliers[i]].z;
	}
	cv::Mat u,w,v;
	SVD::compute(pointsInlier, w, u, v);
	//cout<<"w: "<<endl<<w<<endl;
	//cout<<"u: "<<endl<<u<<endl;
	//cout<<"v: "<<endl<<v.row(2)<<endl;
	//cout<<"mean of v: "<<endl<<mean(v)[0]<<endl;
	planeModel.at<double>(1,0)=v.at<double>(2,0);
	planeModel.at<double>(1,1)=v.at<double>(2,1);
	planeModel.at<double>(1,2)=v.at<double>(2,2);

	for(int i=0; i<3; i++)
	{
		planeModel.at<double>(0,i)=colSum[i]/best_inliers.size();
	}
	return planeModel;
}
//read point cloud from inputFile and find horizontal planes, then write results to outputFile
void FindHorizontalPlanes(char* inputFile, char* outputFile)
{
	//srand((unsigned)time(NULL));
	t[1]=clock();
	//ros::init(argc, argv, "read_points");
	//ros::NodeHandle n;
	//read 3D points from pcd file
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
	//cloud1 = ReadPCD("/home/victor/project_ws/src/ground_plane/data/home_data_ascii/scene1_ascii.pcd");
    cloud1 = ReadPCD(inputFile);
	//cloud1 = ReadPLY(inputFile);
	t[2]=clock();
	printf("%lf s\n",(double)(t[2]-t[1])/CLOCKS_PER_SEC);
	/*//write all the points in cloud to ply file
	ofstream myfile2("/home/victor/project_ws/src/ground_plane/src/allPoints_origin.ply", ios::out);
	if(!myfile2)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile2<<"ply"<<endl;
		myfile2<<"format ascii 1.0"<<endl;
		myfile2<<"element vertex "<<cloud1->points.size()<<endl;
		myfile2<<"property double x"<<endl;
		myfile2<<"property double y"<<endl;
		myfile2<<"property double z"<<endl;
		myfile2<<"property uchar red"<<endl;
		myfile2<<"property uchar green"<<endl;
		myfile2<<"property uchar blue"<<endl;
		myfile2<<"element face 0"<<endl;
		myfile2<<"end_header"<<endl;
		for(int i=0; i<cloud1->points.size(); i++)
		{
			myfile2<<cloud1->points[i].x<<" "<<cloud1->points[i].y<<" "<<cloud1->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile2.close();
	}*/

	//delete the nan points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud = DeleteNAN(cloud1);
	t[3]=clock();
	printf("%lf s\n",(double)(t[3]-t[2])/CLOCKS_PER_SEC);
	int pointNum=cloud->points.size();
	//int pointNum=30;
	//cout the 3d points
	/*cout << "loaded " << cloud->width * cloud->height << " data points from box_cloud.pcd with the following fields: " << endl;
	for(int i=0; i<pointNum; i++)
	{
		cout<<" "<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<endl;
	}*/

	//write all the points in cloud to ply file
    char mkFile[100];
    sprintf(mkFile, "/home/victor/project_ws/src/ground_plane/src/%s/", outputFile);
    cout<<mkFile<<endl;
    int isCreate=mkdir(mkFile, S_IRWXU);
    if(!isCreate)
    {
        printf("Create Path:%s\n",mkFile);
    }
    else
    {
        printf("Create Path Failed!\n");
        return;
    }
    char outFile1[100];
    sprintf(outFile1, "/home/victor/project_ws/src/ground_plane/src/%s/allPoints.ply", outputFile);
	ofstream myfile1(outFile1, ios::out);
	if(!myfile1)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile1<<"ply"<<endl;
		myfile1<<"format ascii 1.0"<<endl;
		myfile1<<"element vertex "<<pointNum<<endl;
		myfile1<<"property double x"<<endl;
		myfile1<<"property double y"<<endl;
		myfile1<<"property double z"<<endl;
		myfile1<<"property uchar red"<<endl;
		myfile1<<"property uchar green"<<endl;
		myfile1<<"property uchar blue"<<endl;
		myfile1<<"element face 0"<<endl;
		myfile1<<"end_header"<<endl;
		for(int i=0; i<pointNum; i++)
		{
			myfile1<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile1.close();
	}

	t[4]=clock();
	printf("%lf s\n",(double)(t[4]-t[3])/CLOCKS_PER_SEC);
	//randomly pick some points from all the points
	/*std::vector<int> randPoints;
	double randRate=0.1;
	for(int i=0; i<pointNum; i++)
	{
		randPoints.push_back(i);
	}
	std::random_shuffle(randPoints.begin(), randPoints.end());
	pointNum=pointNum*randRate;*/
	//k nearest neighbor search
	int maxK=40;
  	int** indexKNN;
  	indexKNN=new int*[pointNum];
  	for(int i=0; i<pointNum; i++)
  	{
  		indexKNN[i]=new int[maxK];
	}
	double** distKNN;
	distKNN=new double*[pointNum];
	for(int i=0; i<pointNum; i++)
  	{
  		distKNN[i]=new double[maxK];
	}
	PointsKNN(cloud, pointNum, maxK, indexKNN, distKNN);

	t[5]=clock();
	printf("%lf s\n",(double)(t[5]-t[4])/CLOCKS_PER_SEC);

	/*cout<<endl;
	cout<<" "<<cloud->points[0].x<<" "<<cloud->points[0].y<<" "<<cloud->points[0].z<<endl;
	cout<<" "<<cloud->points[30000].x<<" "<<cloud->points[30000].y<<" "<<cloud->points[30000].z<<endl;
	cout<<" "<<cloud->points[10000].x<<" "<<cloud->points[10000].y<<" "<<cloud->points[10000].z<<endl;*/

	//normal estimation
	//cv::Mat normals(pointNum, 3, CV_64FC1);
	cv::Mat normals;
	normals=NormalEstimation(cloud, pointNum, maxK, indexKNN, distKNN);

	cout<<endl;
	cout<<"Number of normals: "<<normals.rows<<endl;
	cout<<"Number of points: "<<cloud->points.size()<<endl;

	t[6]=clock();
	printf("%lf s\n",(double)(t[6]-t[5])/CLOCKS_PER_SEC);

	//cout<<normals<<endl;

	//use kmeans to find the planes
	/*cv::Mat labels, centers;
	cv::Mat normals_32;
	normals.convertTo(normals_32, CV_32FC1);
	kmeans(normals_32, 3, labels, TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 1000, 0.1), 3, KMEANS_PP_CENTERS, centers);
	cout<<endl;
	cout<<labels<<endl;
	cout<<endl;
	cout<<centers<<endl;*/

	//find the points in horizontal plane
	//int normals_size=1;
	double ab, an, bn, cosr, interAngle;
	cv::Mat horiPoints;  //(1, 3, CV_64FC1); //the normals of the horizontal plane points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori (new pcl::PointCloud<pcl::PointXYZ>);
	//std::vector<int> indn;  //the index in normals
	std::vector<int> hori_ind; //the index in cloud for the horizontal points
	for(int i=0; i<normals.rows; i++)
	{
		//compute the angle between normal and yAxis direction (0,1,0)/zAxis direction (0,0,1)
		ab=normals.at<double>(i,2);
		an=1;
		bn=sqrt(normals.at<double>(i,0)*normals.at<double>(i,0)+normals.at<double>(i,1)*normals.at<double>(i,1)+normals.at<double>(i,2)*normals.at<double>(i,2));
		cosr=ab/(an*bn);
		//cout<<M_PI<<endl;
		//cout<<acos(cosr)*180/M_PI<<endl;
		interAngle=acos(cosr)*180/M_PI;
		/*if(isnan(interAngle))
		{
			cout<<"i="<<i<<endl;
			cout<<normals.at<double>(i,0)<<" "<<normals.at<double>(i,1)<<" "<<normals.at<double>(i,2)<<endl;
		}*/
		if(interAngle<=7 || interAngle>=173)
		{
			cv::Mat hpRow(1, 3, CV_64FC1);
			hpRow.at<double>(0,0)=normals.at<double>(i,0);
			hpRow.at<double>(0,1)=normals.at<double>(i,1);
			hpRow.at<double>(0,2)=normals.at<double>(i,2);
			cloud_hori->push_back(cloud->points[i]);
			hori_ind.push_back(i);
			//indn.push_back(i);
			//cout<<endl;
			//cout<<hpRow<<endl;
			horiPoints.push_back(hpRow);
		}
	}
	cout<<endl;
	cout<<horiPoints.rows<<endl;
	//cout<<horiPoints.row(0)<<endl;
	//cout<<horiPoints.row(1)<<endl;
	cout<<cloud_hori->points.size()<<endl;
	//cout<<indn.size()<<endl;
	//cout<<horiPoints<<endl;

	t[7]=clock();
	printf("%lf s\n",(double)(t[7]-t[6])/CLOCKS_PER_SEC);

	/*//write the normals into txt file
	char outFile3[100];
    sprintf(outFile3, "/home/victor/project_ws/src/ground_plane/src/%s/normals.txt", outputFile);
	ofstream myfile3(outFile3, ios::out);
	if(!myfile3)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		for(int i=0; i<horiPoints.rows; i++)
		{
			myfile3<<horiPoints.at<double>(i,0)<<" "<<horiPoints.at<double>(i,1)<<" "<<horiPoints.at<double>(i,2)<<endl;
		}
		myfile3.close();
	}*/

	//write the horizontal plane into ply file
    char outFile2[100];
    sprintf(outFile2, "/home/victor/project_ws/src/ground_plane/src/%s/horiPoints.ply", outputFile);
	ofstream myfile(outFile2, ios::out);
	if(!myfile)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile<<"ply"<<endl;
		myfile<<"format ascii 1.0"<<endl;
		myfile<<"element vertex "<<horiPoints.rows<<endl;
		myfile<<"property double x"<<endl;
		myfile<<"property double y"<<endl;
		myfile<<"property double z"<<endl;
		myfile<<"property uchar red"<<endl;
		myfile<<"property uchar green"<<endl;
		myfile<<"property uchar blue"<<endl;
		myfile<<"element face 0"<<endl;
		myfile<<"end_header"<<endl;
		for(int i=0; i<cloud_hori->points.size(); i++)
		{
			myfile<<cloud_hori->points[i].x<<" "<<cloud_hori->points[i].y<<" "<<cloud_hori->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile.close();
	}

	t[8]=clock();
	printf("%lf s\n",(double)(t[8]-t[7])/CLOCKS_PER_SEC);

	//use one point ransac to find horizontal plane
	std::vector<std::vector<int> > inliers;
	std::vector<cv::Mat> planeModel;
	int minNum=20;  //the minimum number of points in the plane
	double dis_thresh=0.01;  //threshold of the distance between point and plane or plane and plane
	std::vector<int> isExist(cloud_hori->points.size(), 1);
	//std::vector<int> isExist(cloud->points.size(), 1);

	while(accumulate(isExist.begin(), isExist.end(), 0)>minNum)
	{
		cv::Mat plane;  //first row is the point on the plane, second row is the normal
		std::vector<int> inl;
		//t[10]=clock();
		cout<<"isExist number: "<<endl<<accumulate(isExist.begin(), isExist.end(), 0)<<endl;
		//t[11]=clock();
		//printf("%lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		//t[10]=clock();
		plane=OnePointRANSAC(cloud_hori, horiPoints, horiPoints.rows, 0.99, dis_thresh, minNum, inl, isExist);
		//t[11]=clock();
		//printf("OnePointRANSAC time: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		if (countNonZero(plane)!=0)
		{
			planeModel.push_back(plane);
			inliers.push_back(inl);
			for(int i=0; i<inl.size(); i++)
			{
				isExist[inl[i]]=0;
			}
		}
		else
		{
			break;
		}
	}
	cout<<"planeModel size: "<<endl<<planeModel.size()<<endl;
	//aggregate the planes which should be the same one
	/*std::vector<std::vector<int> > inliers_fi;
	std::vector<cv::Mat> planeModel_fi;
	std::vector<int> indPlane;
	double disPlane;
	for(int i=0; i<planeModel.size(); i++)
	{
		indPlane.push_back(i);
	}
	for(int i=0; i<planeModel.size(); i++)
	{
		if (indPlane[i]!=i)
			continue;
		for(int j=i+1; j<planeModel.size(); j++)
		{
			if (indPlane[j]!=j)
				continue;
			//compute the offset of plane i
			double xn_i=planeModel[i].at<double>(1,0);
			double yn_i=planeModel[i].at<double>(1,1);
			double zn_i=planeModel[i].at<double>(1,2);
			double x_i=planeModel[i].at<double>(0,0);
			double y_i=planeModel[i].at<double>(0,1);
			double z_i=planeModel[i].at<double>(0,2);
			double os_i=-(xn_i*x_i+yn_i*y_i+zn_i*z_i);

			//compute the offset of plane j
			double xn_j=planeModel[j].at<double>(1,0);
			double yn_j=planeModel[j].at<double>(1,1);
			double zn_j=planeModel[j].at<double>(1,2);
			double x_j=planeModel[j].at<double>(0,0);
			double y_j=planeModel[j].at<double>(0,1);
			double z_j=planeModel[j].at<double>(0,2);
			double os_j=-(xn_j*x_j+yn_j*y_j+zn_j*z_j);

			disPlane=abs(planeModel[i].at<double>(0,2)-planeModel[j].at<double>(0,2));
			cout<<"i="<<i<<" j="<<j<<" distance: "<<disPlane<<endl;
			if (disPlane<dis_thresh)
			{
				indPlane[j]=indPlane[i];
			}
		}
	}
	for(int i=0; i<indPlane.size(); i++)
	{
		cout<<"indPlane["<<i<<"]= "<<indPlane[i]<<endl;
	}
	for(int i=0; i<indPlane.size(); i++)
	{
		if(indPlane[i]==-1)
		{
			continue;
		}
		std::vector<int> vecfi;
		vecfi.insert(vecfi.end(), inliers[i].begin(), inliers[i].end());
		cv::Mat pm(2, 3, CV_64FC1);
		pm.at<double>(0,0)=planeModel[i].at<double>(0,0)*inliers[i].size();
		pm.at<double>(0,1)=planeModel[i].at<double>(0,1)*inliers[i].size();
		pm.at<double>(0,2)=planeModel[i].at<double>(0,2)*inliers[i].size();
		int n=inliers[i].size();
		//pm.at<double>(1,0)=planeModel[i].at<double>(1,0);
		//pm.at<double>(1,1)=planeModel[i].at<double>(1,1);
		//pm.at<double>(1,2)=planeModel[i].at<double>(1,2);
		for(int j=i+1; j<indPlane.size(); j++)
		{
			if (indPlane[j]==indPlane[i])
			{
				pm.at<double>(0,0)=pm.at<double>(0,0)+planeModel[j].at<double>(0,0)*inliers[j].size();
				pm.at<double>(0,1)=pm.at<double>(0,1)+planeModel[j].at<double>(0,1)*inliers[j].size();
				pm.at<double>(0,2)=pm.at<double>(0,2)+planeModel[j].at<double>(0,2)*inliers[j].size();
				n+=inliers[j].size();
				vecfi.insert(vecfi.end(), inliers[j].begin(), inliers[j].end());
				indPlane[j]=-1;
			}
		}
		pm.at<double>(0,0)=pm.at<double>(0,0)/n;
		pm.at<double>(0,1)=pm.at<double>(0,1)/n;
		pm.at<double>(0,2)=pm.at<double>(0,2)/n;
		cv::Mat pointsIn(vecfi.size(), 3, CV_64FC1);
		for(int j=0; j<vecfi.size(); j++)
		{
			pointsIn.at<double>(j,0)=cloud->points[vecfi[j]].x;
			pointsIn.at<double>(j,1)=cloud->points[vecfi[j]].y;
			pointsIn.at<double>(j,2)=cloud->points[vecfi[j]].z;
		}
		cv::Mat u0,w0,v0;
		SVD::compute(pointsIn, w0, u0, v0);
		pm.at<double>(1,0)=v0.at<double>(2,0);
		pm.at<double>(1,1)=v0.at<double>(2,1);
		pm.at<double>(1,2)=v0.at<double>(2,2);
		planeModel_fi.push_back(pm);
		inliers_fi.push_back(vecfi);
		indPlane[i]=-1;
	}
	cout<<"After aggregate planeModel size: "<<endl<<planeModel_fi.size()<<endl;*/
	t[9]=clock();
	printf("%lf s\n", (double)(t[9]-t[8])/CLOCKS_PER_SEC);
	printf("Finding planes time: %lf s\n", (double)(t[9]-t[4])/CLOCKS_PER_SEC);

	//assign all the points in the point cloud to the nearest plane(distance less than threshold) and get final inliers
	//std::vector<std::vector<int> > inliers_final;
	//std::vector<cv::Mat> planeModel_final;
	std::vector<int> isUsed(cloud->points.size(), 0);
	for(int i=0; i<inliers.size(); i++)
	{
		for(int j=0; j<inliers[i].size(); j++)
		{
			inliers[i][j]=hori_ind[inliers[i][j]];//transfer the cloud_hori index to cloud index
			isUsed[inliers[i][j]]=1;
		}
	}

	for(int i=0; i<cloud->points.size(); i++)
	{
		if(isUsed[i]==1)
		{
			continue;
		}
		std::vector<double> dis_in(inliers.size(), 0);
		double min_dis;
		int min_ind;
		for(int j=0; j<inliers.size(); j++)
		{
			//compute the distance between every point to the inliers_fi planes
			//double x=cloud->points[i].x-planeModel_fi[j].at<double>(0,0);
			//double y=cloud->points[i].y-planeModel_fi[j].at<double>(0,1);
			//double z=cloud->points[i].z-planeModel_fi[j].at<double>(0,2);
			//double xn=planeModel_fi[j].at<double>(1,0);
			//double yn=planeModel_fi[j].at<double>(1,1);
			//double zn=planeModel_fi[j].at<double>(1,2);
			//double norm_n=sqrt(xn*xn+yn*yn+zn*zn);
			//dis_in[j]=abs(x*xn+y*yn+z*zn)/norm_n;
			dis_in[j]=abs(cloud->points[i].z-planeModel[j].at<double>(0,2));
			//cout<<"dis_in["<<j<<"]= "<<dis_in[j]<<endl;
			if (j==0)
			{
				min_dis=dis_in[j];
				min_ind=j;
			}
			else
			{
				if (dis_in[j]<min_dis)
				{
					min_dis=dis_in[j];
					min_ind=j;
				}
			}
		}
		if (min_dis<dis_thresh)
		{
			inliers[min_ind].push_back(i);
		}
		isUsed[i]=1;
	}
	//re-compute the normals and center point of the points in inliers_fi
	for(int i=0; i<inliers.size(); i++)
	{
		//cout<<"i= "<<i<<endl;
		cv::Mat pointsInlier(inliers[i].size(), 3,  CV_64FC1);
		double colSum[3]={0,0,0};
		for(int j=0; j<inliers[i].size(); j++)
		{
			//cout<<"j= "<<j<<"/"<<inliers_fi[i].size()<<endl;
			pointsInlier.at<double>(j,0)=cloud->points[inliers[i][j]].x;
			pointsInlier.at<double>(j,1)=cloud->points[inliers[i][j]].y;
			pointsInlier.at<double>(j,2)=cloud->points[inliers[i][j]].z;
			colSum[0]+=pointsInlier.at<double>(j,0);
			colSum[1]+=pointsInlier.at<double>(j,1);
			colSum[2]+=pointsInlier.at<double>(j,2);
		}
		cv::Mat u,w,v;
		SVD::compute(pointsInlier, w, u, v);
		planeModel[i].at<double>(1,0)=v.at<double>(2,0);
		planeModel[i].at<double>(1,1)=v.at<double>(2,1);
		planeModel[i].at<double>(1,2)=v.at<double>(2,2);

		for(int j=0; j<3; j++)
		{
			planeModel[i].at<double>(0,j)=colSum[j]/inliers[i].size();
		}
	}
	t[10]=clock();

	//write the horizontal plane into ply file and store the normals of each plane
	int num_allpoints=0;
	for(int i=0; i<inliers.size(); i++)
	{
		num_allpoints+=inliers[i].size();
		cout<<"inliers size: "<<endl<<inliers[i].size()<<endl;
		//write the horizontal plane into ply file
		char outPath[100];
		sprintf(outPath, "/home/victor/project_ws/src/ground_plane/src/%s/ransacPlane%d.ply", outputFile, i);
		ofstream myfile0(outPath, ios::out);
		if(!myfile0)
		{
			cout<<"error!"<<endl;
		}
		else
		{
			myfile0<<"ply"<<endl;
			myfile0<<"format ascii 1.0"<<endl;
			myfile0<<"element vertex "<<inliers[i].size()<<endl;
			myfile0<<"property double x"<<endl;
			myfile0<<"property double y"<<endl;
			myfile0<<"property double z"<<endl;
			myfile0<<"property uchar red"<<endl;
			myfile0<<"property uchar green"<<endl;
			myfile0<<"property uchar blue"<<endl;
			myfile0<<"element face 0"<<endl;
			myfile0<<"end_header"<<endl;
			for(int j=0; j<inliers[i].size(); j++)
			{
				myfile0<<cloud->points[inliers[i][j]].x<<" "<<cloud->points[inliers[i][j]].y<<" "<<cloud->points[inliers[i][j]].z<<" "<<"255 0 0"<<endl;
			}
			myfile0.close();

		}
		//store the normals and center point of each plane
		char outPath1[100];
		sprintf(outPath1, "/home/victor/project_ws/src/ground_plane/src/%s/normal%d.txt", outputFile, i);
		ofstream myfile2(outPath1, ios::out);
		if(!myfile2)
		{
			cout<<"error!"<<endl;
		}
		else
		{
			myfile2<<planeModel[i].at<double>(1,0)<<" "<<planeModel[i].at<double>(1,1)<<" "<<planeModel[i].at<double>(1,2)<<endl;
			myfile2<<planeModel[i].at<double>(0,0)<<" "<<planeModel[i].at<double>(0,1)<<" "<<planeModel[i].at<double>(0,2)<<endl;
		}
		myfile2.close();
	}

	//write all the horizontal planes into one ply file with different color
	/*int colorValue[3]={50, 150, 255};
	int RGBValue[27][3];
	int rgbn=0;
	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			for (int k=0; k<3; k++)
			{
				RGBValue[rgbn][0]=colorValue[i];
				RGBValue[rgbn][1]=colorValue[j];
				RGBValue[rgbn][2]=colorValue[k];
				rgbn++;
			}
		}
	}*/

	//read color from "rgb.txt" which is generated by jet in matlab
	int RGBValue[200][3];
	int rgbn=0;
    char inPath[100]="/home/victor/project_ws/src/ground_plane/src/rgb.txt";
	ifstream infile(inPath, ios::in);
	if(!infile)
	{
		cout<<"error reading rgb.txt!"<<endl;
	}
	else
	{
		for(int i=0; i<200; i++)
		{
			infile>>RGBValue[i][0]>>RGBValue[i][1]>>RGBValue[i][2];
		}
		infile.close();
	}

	//write the horizontal plane into ply file
	char outPath[100];
	sprintf(outPath, "/home/victor/project_ws/src/ground_plane/src/%s/ransacPlane(num:%d).ply", outputFile, int(inliers.size()));
	ofstream myfile0(outPath, ios::out);
	if(!myfile0)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile0<<"ply"<<endl;
		myfile0<<"format ascii 1.0"<<endl;
		myfile0<<"element vertex "<<num_allpoints<<endl;
		myfile0<<"property double x"<<endl;
		myfile0<<"property double y"<<endl;
		myfile0<<"property double z"<<endl;
		myfile0<<"property uchar red"<<endl;
		myfile0<<"property uchar green"<<endl;
		myfile0<<"property uchar blue"<<endl;
		myfile0<<"element face 0"<<endl;
		myfile0<<"end_header"<<endl;
		for (int i=0; i<inliers.size(); i++)
		{
			for(int j=0; j<inliers[i].size(); j++)
			{
				myfile0<<cloud->points[inliers[i][j]].x<<" "<<cloud->points[inliers[i][j]].y<<" "<<cloud->points[inliers[i][j]].z<<" "<<RGBValue[i][0]<<" "<<RGBValue[i][1]<<" "<<RGBValue[i][2]<<endl;
			}
		}
		myfile0.close();

	}

	//write the running time into txt file
	char outPath2[100];
	sprintf(outPath2, "/home/victor/project_ws/src/ground_plane/src/%s/time.txt", outputFile);
	ofstream myfile3(outPath2, ios::out);
	if(!myfile3)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile3<<(double)(t[9]-t[2])/CLOCKS_PER_SEC<<" "<<"s"<<endl;
		myfile3.close();

	}

	t[11]=clock();
	//printf("%lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
	printf("Find Horizontal Planes Time: %lf s\n", (double)(t[9]-t[2])/CLOCKS_PER_SEC);
	printf("Total Time: %lf s\n",(double)(t[11]-t[1])/CLOCKS_PER_SEC);

	/*//use ransac in pcl to find horizontal plane
	std::vector<int> inliers;
	pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud_hori));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
	ransac.setDistanceThreshold (.01);
	ransac.computeModel();
	ransac.getInliers(inliers);
	//copies all inliers of the model computed to another PointCloud
  	pcl::copyPointCloud<pcl::PointXYZ>(*cloud_hori, inliers, *final);
	//visualize the inliers
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = simpleVis(final);
	while (!viewer->wasStopped ())
  	{
    	viewer->spinOnce (100);
    	boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  	}*/
	//return 0;
}
//get all the files in path
void getFiles(char* dir_name, vector<string> &files)
{
    // check the parameter
    if(dir_name == NULL)
    {
        cout<<"dir_name is null !"<<endl;
        return;
    }

    // check if dir_name is a valid dir
    struct stat s;
    lstat(dir_name, &s);
    if(!S_ISDIR(s.st_mode))
    {
        cout<<"dir_name is not a valid directory!"<<endl;
        return;
    }

    struct dirent* filename;    // return value for readdir()
    DIR* dir;                   // return value for opendir()
    dir = opendir(dir_name);
    if(dir == NULL)
    {
        cout<<"Can not open dir "<<dir_name<<endl;
        return;
    }
    cout<<"Successfully opened the dir!"<<endl;

    //read all the files in the dir
    while((filename = readdir(dir)) != NULL)
    {
        // get rid of "." and ".."
        if( strcmp(filename->d_name, ".") == 0 ||
            strcmp(filename->d_name, "..") == 0 )
            continue;
        //cout<<filename->d_name<<endl;
        files.push_back(filename->d_name);
    }
}
int main(int argc, char **argv)
{
    srand((unsigned)time(NULL));
    ros::init(argc, argv, "read_points");
	ros::NodeHandle n;
    string folderName="home_data_ascii";
    //string folderName="office_data_ascii";
	//string folderName="rgbd-scenes-v2";
    char mkFile[100];
    sprintf(mkFile, "/home/victor/project_ws/src/ground_plane/src/%s/", folderName.c_str());
    cout<<mkFile<<endl;
    int isCreate=mkdir(mkFile, S_IRWXU);
    if(!isCreate)
    {
        printf("Create Path:%s\n",mkFile);
    }
    else
    {
        printf("Create Path Failed!\n");
        return 0;
    }
    char filePath[100];
    sprintf(filePath, "/home/victor/project_ws/src/ground_plane/data/%s", folderName.c_str());
    vector<string> files;
    getFiles(filePath, files);
    for(int i=0; i<files.size(); i++)
    {
        cout<<files[i].c_str()<<endl;
        char inPath[100];
        sprintf(inPath, "/home/victor/project_ws/src/ground_plane/data/%s/%s", folderName.c_str(), files[i].c_str());
        char outPath[100];
        string fname;
        fname.assign(files[i].begin(), files[i].end()-4);
        sprintf(outPath, "%s/%s", folderName.c_str(), fname.c_str());
        FindHorizontalPlanes(inPath, outPath);
    }
    return 0;
}


#include <UnitTest++/UnitTest++.h>
#include "../pixel_array2d.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <algorithm>
#include <exception>
#include <boost/filesystem.hpp>
#include <typeinfo>

using namespace coela;
using namespace std;

extern string lucky_lib_test_resources_dir;

SUITE(pixel_array_pixel_access)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";

    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(null_constructor) {
        PixelArray2d<float> null_float_array;
        PixelArray2d<double> null_double_array;
    }

    const double test_array_init_value = 5.0;
    struct basic_float_pixel_array {
        basic_float_pixel_array() {
            test_array= PixelArray2d<float>(10,5, 5.0);
        }
        PixelArray2d<float> test_array;

    };

    TEST_FIXTURE(basic_float_pixel_array, SimpleConstructFromBits_float) {
        //check dims
        CHECK_EQUAL(10, test_array.range().x_dim());
        CHECK_EQUAL(5, test_array.range().y_dim());

        //check pixel values
        CHECK_EQUAL(test_array_init_value, test_array(5,1));

    }

    TEST_FIXTURE(basic_float_pixel_array, sum) {
        CHECK_CLOSE(test_array.range().n_pix()*test_array_init_value, test_array.sum() , 1e-6);
    }

    TEST(double_pixel_array_fractional_init) {
        PixelArray2d<double> pixarray(10,10, 1.0/9.0);
        double val = 1.0/9.0;
        CHECK_EQUAL(val, pixarray(1,1));
        CHECK_CLOSE(val*pixarray.range().n_pix(), pixarray.sum(), 1e-6);
    }

    TEST_FIXTURE(basic_float_pixel_array, pixel_iteration_for_float_array) {
        size_t n_its=0;
        for (PixelIterator pi(test_array.range()); pi!=pi.end; ++pi) {
            CHECK_EQUAL(5.0, test_array(pi));
            test_array(pi)-=3.0;
            n_its++;
        }

        CHECK_EQUAL(test_array.range().n_pix(), n_its);

        for (PixelIterator pi(test_array.range()); pi!=pi.end; ++pi) {
            CHECK_EQUAL(2.0,test_array(pi));
        }

    }

    TEST_FIXTURE(basic_float_pixel_array, copy_constructor_float_array) {
        PixelArray2d<float> array2(test_array);
        CHECK(test_array== array2);
    }

    TEST_FIXTURE(basic_float_pixel_array, float_array_assignment) {
        PixelArray2d<float> array2=test_array;
        CHECK(test_array==array2);
    }

    TEST_FIXTURE(basic_float_pixel_array, subarray_constructor_float_array) {
        float x=0.0;
        for (PixelIterator pi(test_array.range()); pi!=pi.end; ++pi) {
            test_array(pi)=x;
            x+=0.1;
        }
        PixelRange subarray_box(1,1, 5,5);
        PixelArray2d<float> subarray = PixelArray2d<float>::sub_array(test_array, subarray_box);

        CHECK(subarray_box.n_pix()==subarray.range().n_pix());

        for (PixelIterator pi_orig(subarray_box), pi_sub(subarray.range());
                pi_orig!=pi_orig.end;
                ++pi_orig, ++pi_sub) {
            CHECK(test_array(pi_orig)== subarray(pi_sub));
        }

    }

    TEST_FIXTURE(basic_float_pixel_array, factor_multiply) {
        double factor=2.0;
        test_array*=factor;
        PixelArray2d<float> twice_val(test_array.range().x_dim(),
                                      test_array.range().y_dim(),
                                      test_array_init_value*factor);

        CHECK(twice_val == test_array);
    }

    TEST_FIXTURE(basic_float_pixel_array, promotion_to_double_type) {
        PixelArray2d<double> promoted(test_array) ;
        for (PixelIterator it(promoted.range()); it!=it.end; ++it) {
            CHECK_CLOSE((double)test_array(it), promoted(it), 1e-6);
        }
    }

    TEST_FIXTURE(basic_float_pixel_array, conversion_between_types) {
        PixelArray2d<double> promoted(test_array);

        promoted.check_header_type_matches_array_type();
        PixelArray2d<int> i(5,5, 3);
//        PixelArray2d<int> f_to_i(test_array);
        PixelArray2d<float> i_to_f(i);

//        PixelArray2d<double> i_to_d(i);
//        PixelArray2d<short int> i_to_si(i);

        PixelArray2d<float> demoted(promoted);
//

//        for (PixelIterator it(promoted.range()); it!=it.end; ++it){
//            CHECK_CLOSE((double)test_array(it), demoted(it), 1e-6);
//        }
    }

    TEST(subtraction_of_double_From_array_of_int) {

        PixelArray2d<int> init(10,10, 5);

        PixelArray2d<double> debiased = init - 2.5;
        for (PixelIterator i(debiased.range()); i!=i.end; ++i) {
            CHECK_EQUAL(2.5, debiased(i));
        }

    }

    TEST_FIXTURE(basic_float_pixel_array, min_max_value_grabs) {
        PixelArray2d<float>& pix=test_array;

        Pixel<float> max(PixelIndex(pix.range().x_dim()/2, pix.range().y_dim()/2),
                         1.563465*1e5);
        Pixel<float> min(PixelIndex(pix.range().x_dim()/2 - 1, pix.range().y_dim()/2),
                         -1.343234*1e5);
        pix(max)=max.value;
        pix(min)=min.value;

        CHECK_EQUAL(PixelIndex(max), pix.max_PixelIndex());
        CHECK_EQUAL(PixelIndex(min), pix.min_PixelIndex());
        CHECK_EQUAL(min.value, pix.min_val());
        CHECK_EQUAL(max.value, pix.max_val());

    }

    //TO DO - test image arithmetic
}

SUITE(restricted_functions_eg_stream_load_and_write)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";

    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    //Create a fixture to be tested:
    const double test_array_init_value = 5.0;
    struct basic_float_pixel_array {
        PixelArray2d<float> test_array;
        basic_float_pixel_array() {
            test_array= PixelArray2d<float>(10,5, 5.0);
        }
    };

    TEST_FIXTURE(basic_float_pixel_array, data_serialization_for_float_no_compression) {

        //NB fht serialization, PixelArrayHeader serialization, both unit tested elsewhere,

        ArrayCompressionInfo compression_inf = ArrayCompressionInfo::no_compression();
        stringstream ss;
        test_array.write_data_to_stream(ss, compression_inf);

        PixelArray2d<float> a2;

        a2.hdr = PixelArray2dHeader(test_array.bitpix(),
                                    test_array.range().x_dim(), test_array.range().y_dim());

        size_t byte_offset=0;
        a2.seek_and_load_data_from_stream(ss, byte_offset, compression_inf);
        CHECK(test_array==a2);
    }

    TEST_FIXTURE(basic_float_pixel_array, file_access_for_float) {
        string filename(test_suite_output_dir +"test_output_file.fits");

        test_array.write_to_file(filename);
        PixelArray2d<float> a2 = PixelArray2d<float>::load_from_file(filename);

        CHECK(test_array.range() == a2.range());
        CHECK(test_array== a2);

        FitsHeader fht(filename);
        CHECK(fht.get_key_value("BZERO")=="0.0");

    }

    TEST_FIXTURE(basic_float_pixel_array, buffered_file_access_for_float) {
        string filename(test_suite_output_dir +"test_output_file.fits");
        test_array.write_to_file(filename);

        PixelArray2d<float> reg_load=
            PixelArray2d<float>::load_from_file(filename);

        FileBuffer buf;
        buf.buffer_whole_file(filename);
        PixelArray2d<float> buf_load=
            PixelArray2d<float>::load_from_buffer(buf);

        CHECK(reg_load==buf_load);
    }

    TEST(file_access_for_short) {
        PixelArray2d<uint16_t> test_array(10,5,25.0);
        string filename(test_suite_output_dir +"test_output_short_file.fits");

        test_array.write_to_file(filename);

        PixelArray2d<uint16_t> a2= PixelArray2d<uint16_t>::load_from_file(filename);

        CHECK(test_array.range() == a2.range());

        CHECK(test_array==a2);

        CHECK_THROW(PixelArray2d<uint32_t>::load_from_file(filename), std::runtime_error);
        CHECK_THROW(PixelArray2d<double>::load_from_file(filename), std::runtime_error);
    }

    TEST(load_from_bitstream) {
        string bitstream_compressed_img_filename(lucky_lib_test_resources_dir+"bit_file.lcz");
        string float_img_filename(lucky_lib_test_resources_dir+"old_code_float_copy.fits");

        PixelArray2d<uint16_t> lcz_short_img =
            PixelArray2d<uint16_t>::load_from_file(bitstream_compressed_img_filename);
        PixelArray2d<uint32_t> lcz_int_img =
            PixelArray2d<uint32_t>::load_from_file(bitstream_compressed_img_filename);
        PixelArray2d<float> lcz_float_img =
            PixelArray2d<float>::load_from_file(bitstream_compressed_img_filename);
        PixelArray2d<double> lcz_double_img =
            PixelArray2d<double>::load_from_file(bitstream_compressed_img_filename);

        lcz_short_img.write_to_file(test_suite_output_dir +"decompressed_short_img.fits");
        lcz_int_img.write_to_file(test_suite_output_dir +"decompressed_int_img.fits");
        lcz_float_img.write_to_file(test_suite_output_dir +"decompressed_float_img.fits");
        lcz_double_img.write_to_file(test_suite_output_dir +"decompressed_double_img.fits");

        PixelArray2d<float> float_img =
            PixelArray2d<float>::load_from_file(float_img_filename);

        for (PixelIterator i(float_img.range()); i!=i.end; ++i) {
            CHECK_EQUAL(lcz_float_img(i), float_img(i));
        }
    }

    TEST(load_from_bitstream_buffer) {

        string bitstream_compressed_img_filename(lucky_lib_test_resources_dir+"bit_file.lcz");
        FileBuffer buf; buf.buffer_whole_file(bitstream_compressed_img_filename);

        PixelArray2d<uint16_t> from_file=
            PixelArray2d<uint16_t>::load_from_file(bitstream_compressed_img_filename);
        PixelArray2d<uint16_t> from_buf=
            PixelArray2d<uint16_t>::load_from_buffer(buf, 0);

        CHECK(from_file == from_buf);

    }

    TEST(load_from_lcc_bytestream) {
        string lcc_byte_stream_compressed_img_filename(
            lucky_lib_test_resources_dir+"lcc_file.lcc");
        string lcc_byte_stream_decompressed_img_filename(
            lucky_lib_test_resources_dir+"decompressed_lcc_short_file.fits");

        PixelArray2d<int16_t> lcc_short_img=
            PixelArray2d<int16_t>::load_from_file(lcc_byte_stream_compressed_img_filename);
        PixelArray2d<int32_t> lcc_uint_img=
            PixelArray2d<int32_t>::load_from_file(lcc_byte_stream_compressed_img_filename);
        for (PixelIterator i(lcc_short_img.range()); i!=i.end; ++i) {
            CHECK_EQUAL(lcc_short_img(i), lcc_uint_img(i));
        }
//        cout<<"Writing to file:"<<test_suite_output_dir +"decompress_lcc_to_short_img.fits"<<endl;
        lcc_short_img.write_to_file(test_suite_output_dir +"decompress_lcc_to_short_img.fits");
        lcc_uint_img.write_to_file(test_suite_output_dir +"decompress_lcc_to_uint_img.fits");

    }

    TEST(load_from_lcc_buffer) {
        string lcc_byte_stream_compressed_img_filename(
            lucky_lib_test_resources_dir+"lcc_file.lcc");

        FileBuffer buf; buf.buffer_whole_file(lcc_byte_stream_compressed_img_filename);

        PixelArray2d<uint16_t> from_file =
            PixelArray2d<uint16_t>::load_from_file(lcc_byte_stream_compressed_img_filename);
        PixelArray2d<uint16_t> from_buf=
            PixelArray2d<uint16_t>::load_from_buffer(buf, 0);

        CHECK(from_file == from_buf);

    }

}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIE_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIE_HPP_INCLUDED
#include <boost/math/special_functions.hpp>
#include <nt2/sdk/constant/eps_related.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/digits.hpp>

#include <nt2/include/functions/is_ltz.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/oneplus.hpp>
#include <nt2/include/functions/tan.hpp>
#include <nt2/include/functions/atan.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/include/functions/ellpe.hpp>
#include <nt2/include/functions/ellpk.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellie_, tag::cpu_,
                       (A0)(A1),
                       (arithmetic_<A0>)(arithmetic_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellie_(tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      return nt2::ellie(type(a0), type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellie_, tag::cpu_,
                       (A0)(A1),
                       (double_<A0>)(double_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellie_(tag::double_,tag::double_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      if (is_eqz(a1))  return type(a0);
      return boost::math::ellint_2(nt2::sqrt(type(a1)), type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellie_, tag::cpu_,
                       (A0)(A1),
                       (float_<A0>)(float_<A1>)
                      )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellie_(tag::float_,tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::floating(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      if (a1>One<A1>()||(is_ltz(a1))) return Nan<type>();
      else if (is_eqz(a1))
      return a0;
      else if (a1 == One<A1>())
      return nt2::sin(a0);
      else
      {
        type lphi = nt2::abs(a0);
        type m   =  a1;
        type a = One<type>();
        type b = nt2::sqrt(oneminus(m));
        type c = nt2::sqrt(m);
        int d = One<int>();
        type e = Zero<type>();
        type t = nt2::tan( lphi );
        int mod = (lphi + Pio_2<type>())/Pi<type>();
        while( nt2::abs(c) > Eps<type>()*nt2::abs(a) )
          {
            type temp = b/a;
            lphi = lphi + nt2::atan(t*temp) + mod * Pi < type>();
            mod = (lphi + Pio_2<type>())/Pi<type>();
            t *= oneplus(temp)/( oneminus(temp * sqr(t)));
            c = average(a,-b);
            temp = nt2::sqrt(a*b);
            a = nt2::average(a,b);
            b = temp;
            d += d;
            e += c*nt2::sin(lphi);
          }

        b = oneminus(m);
        type temp = nt2::ellpe(b)/nt2::ellpk(b);
        temp *= (nt2::atan(t) + mod * Pi < float>())/(d * a);
        temp += e;
        if(is_ltz(a0))  temp = -temp;
        return temp ;
      }
      }
  };
} }

#endif
// modified by jt the 26/12/2010