#include <iostream>

#include <boost/thread.hpp>
#include <boost/assign/std/vector.hpp>

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

#include <maps/LocalMap.hpp>
#include <maps/MapWrapper.hpp>

#include <pointcloud_tools/pointcloud_vis.hpp>
#include <pointcloud_tools/filter_planes.hpp>

using namespace std;

#define verbose_txt 0
// 0 means publish nothing
// 1 means publish very important only [runs without issue]
// 2 means publish important
// 3 means publish all
#define verbose_lcm 3 // was 0

class Detector {
public:
  Detector(boost::shared_ptr<MapWrapper> iWrapper):
          null_poseT_(0, Eigen::Isometry3d::Identity()) {
    mWrapper = iWrapper;

  cloud_timestamp_ = 0;
  plane_fitter_id_ =1;
  publish_lcm_ = lcm_create(NULL);

  pc_vis_ = new pointcloud_vis(publish_lcm_);
  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Pose - Null",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Cloud - Laser Map"     ,1,1, 1000,1, {0,0,1}));

  // initial values of floor coeff which correspond to holding out in front:
  last_floor_coeff_.values = {-0.089914, -0.00691448, 0.995926, 0.769824} ;

  /*
  last_floor_coeff_.values.push_back(-0.089914);
  last_floor_coeff_.values.push_back(-0.00691448);
  last_floor_coeff_.values.push_back(0.995926);
  last_floor_coeff_.values.push_back(0.769824);
  */

  }

  void write_pcd(maptypes::PointCloud::Ptr &cloud,std::ofstream &fstream );

  void find_floor_with_aligned_cloud(pcl::PointCloud<PointXYZRGB>::Ptr &cloud_global);

  void find_planes (pcl::PointCloud<pcl::PointXYZRGB>::Ptr);

  void visualize_planes(vector<BasicPlane> &plane_stack );

  void operator ()() {
    while (true) {
      maptypes::PointCloud::Ptr cloud;
      mWrapper->lock();
      if (mWrapper->getMap() != NULL) {
        cloud = mWrapper->getMap()->getAsPointCloud();
      }
      mWrapper->unlock();

      cout << "Grabbed a point cloud of " << cloud->points.size() <<
        " points from octree" << endl;

      if (cloud->points.size() >1){
        ofstream outfile ("test.pcd");
        write_pcd( cloud,outfile);

        find_floor_with_aligned_cloud(cloud);

        //find_planes (cloud);

        cout << "done\n";
      }

      sleep(2);
    }
  }

private:
  int64_t cloud_timestamp_;
  int plane_fitter_id_; // id for visualizing plane fitting collections
  lcm_t * publish_lcm_;
  pointcloud_vis* pc_vis_;
  Isometry3dTime null_poseT_;

  pcl::ModelCoefficients last_floor_coeff_;
    // The precise adjustment of the pose required to aligne the floor in the cloud with horizontal:
    double precise_height_, precise_pitch_ , precise_roll_;

protected:
  boost::shared_ptr<MapWrapper> mWrapper;
};

void Detector::write_pcd(maptypes::PointCloud::Ptr &cloud,std::ofstream &fstream ){
  fstream << "# .PCD v.5 - Point Cloud Data file format" << endl;
  fstream << "FIELDS x y z" << endl;
  fstream << "SIZE 4 4 4" << endl;
  fstream << "TYPE F F F" << endl;
  fstream << "WIDTH " << cloud->points.size() << endl;
  fstream << "HEIGHT 1" << endl;
  fstream << "POINTS " << cloud->points.size() << endl;
  fstream << "DATA ascii" << endl;

  for (size_t i=0; i < cloud->points.size(); i++){
    pcl::PointXYZRGB lr = cloud->points[i];
    ostringstream temp0;
    temp0 << lr.x <<" " << lr.y<< " " << lr.z << endl;
    fstream << temp0.str();
  }
  fstream.close();
    cout << "finished writing points: "<< cloud->points.size() << endl;
}

void Detector::visualize_planes(vector<BasicPlane> &plane_stack ){
  // 3. Visualise normals:
  for (size_t i = 0; i< plane_stack.size() ; i++){
    BasicPlane plane = plane_stack[i];
    stringstream name_out;
    name_out << "local_polygons_major_" << plane.major << "_minor_" << plane.minor;
    int plane_id = 290+ i ;
    ptcld_cfg pcfg = ptcld_cfg(plane_id,    name_out.str()     ,3,1, 1000,1,{-1,-1,-1} );
    pc_vis_->ptcld_to_lcm(pcfg, (plane.cloud), null_poseT_.utime, null_poseT_.utime );
  }

  for (size_t i=0;i<plane_stack.size();i++){
    BasicPlane plane = plane_stack[i];
    if (plane.cloud.points.size() ==0 ){
      cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n";
      continue;
    }
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr normals_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    normals_cloud->points.push_back( plane.cloud.points[0]);

    pcl::PointXYZRGB pt;
    pt.x= plane.cloud.points[0].x + plane.coeffs.values[0];
    pt.y= plane.cloud.points[0].y + plane.coeffs.values[1];
    pt.z= plane.cloud.points[0].z + plane.coeffs.values[2];
    pt.r =0;      pt.g =255;      pt.b =0;
    normals_cloud->points.push_back( pt );

    stringstream name_out2;
    name_out2 << "global_normal_local_polygons_major_" << plane.major << "_minor_" << plane.minor;
    int plane_id2 = 590+ i  ;
    ptcld_cfg pcfg2 = ptcld_cfg(plane_id2,    name_out2.str()     ,3,1, 1000,1,{0.3,.8,0.1} );
    pc_vis_->ptcld_to_lcm(pcfg2, *normals_cloud, null_poseT_.utime, null_poseT_.utime );
  }
}

void Detector::find_floor_with_aligned_cloud(pcl::PointCloud<PointXYZRGB>::Ptr &cloud_filtered){
  // MODUS OPERANDUS:
  // given previous estimate of ground and accurate RPY

  std::cerr << "[IN] : " << cloud_filtered->points.size()<< " data points." << std::endl;
  null_poseT_.utime = cloud_timestamp_;
  pc_vis_->pose_to_lcm_from_list(1000, null_poseT_);
  // visualize raw input cloud:
  //pc_vis_->ptcld_to_lcm_from_list(1001, *cloud_filtered, cloud_timestamp_, cloud_timestamp_);

  // 1. Find Planes in cloud:
  FilterPlanes filtp;
  filtp.setInputCloud(cloud_filtered);
  filtp.setPoseIDs(plane_fitter_id_,cloud_timestamp_);
  filtp.setLCM(publish_lcm_,0); // 0 means no publish to lcm, higher does
  filtp.setDistanceThreshold(0.04);
  filtp.setStopProportion(0.60);
  vector<BasicPlane> plane_stack;
  filtp.filterPlanes(plane_stack);
  std::cout << "[OUT] number of planes extracted: " << plane_stack.size() << "\n";
  // visualize plane stack:
  //visualize_planes(plane_stack);

  // 2. Find the ground plane in the plane set:
  int ground_id = -1;
  for (size_t  i=0 ; i <plane_stack.size() ; i ++ ){
    pcl::ModelCoefficients coeff = plane_stack[i].coeffs;
    double pitch = atan(coeff.values[0]/coeff.values[2]);
    double roll =- atan(coeff.values[1]/coeff.values[2]);
    double coeff_norm = sqrt(pow(coeff.values[0],2) +
    pow(coeff.values[1],2) + pow(coeff.values[2],2));
    double height = (coeff.values[2]*coeff.values[3]) / coeff_norm;

    //  cout  <<  "\nRANSAC Floor Coefficients: " << coeff.values[0]
    //    << " " << coeff.values[1] << " "  << coeff.values[2] << " " << coeff.values[3] << endl;
    //  cout << "Pitch: " << pitch << " (" << (pitch*180/M_PI) << "d). positive nose down\n";
    //  cout << "Roll : " << roll << " (" << (roll*180/M_PI) << "d). positive right side down\n";
    //  cout << "Height : " << height << " of device off ground [m]\n";

    // quit if we find a plane fitting the model

    // 0.1 rads or 5.7 degrees is pretty tight for a walking platform but usually works on fixed
    //if ((fabs(pitch) < 0.1) && (fabs(roll) < 0.1)){
    if ((fabs(pitch) < 0.2) && (fabs(roll) < 0.2)){
      cout << "GROUND PLANE FOUND\n";
      ground_id = (int) i;
      break;
    }
  }

  pcl::ModelCoefficients floor_coeff;
  if (ground_id<0){
    cout << "NO GROUND PLANE FOUND - using last Height|P|R\n";
    floor_coeff = last_floor_coeff_;
  }else{
    // 1. Using precise height pitch and roll, align the cloud
    floor_coeff = plane_stack[ground_id].coeffs;
  }

  precise_pitch_ = atan(floor_coeff.values[0]/floor_coeff.values[2]);
  precise_roll_ =- atan(floor_coeff.values[1]/floor_coeff.values[2]);
  double coeff_norm = sqrt(pow(floor_coeff.values[0],2) + pow(floor_coeff.values[1],2) + pow(floor_coeff.values[2],2));
  precise_height_ = (floor_coeff.values[2]*floor_coeff.values[3]) / coeff_norm;
  cout << "Pitch: " << precise_pitch_ << " (" << (precise_pitch_*180/M_PI) << "d). positive nose down\n";
  cout << "Roll : " << precise_roll_ << " (" << (precise_roll_*180/M_PI) << "d). positive right side down\n";
  cout << "Height : " << precise_height_ << " of device off ground [m]\n";
  last_floor_coeff_ = floor_coeff;

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_global_x (new pcl::PointCloud<pcl::PointXYZRGB> ());
  //  Eigen::Vector3f world_trans(0 , 0 ,  precise_height_ );
  //  Eigen::Quaternionf world_rot = euler_to_quat_f(0, -precise_pitch_, -precise_roll_); // heading
  //  pcl::transformPointCloud (*cloud_filtered, *cloud_global_x,world_trans, world_rot);
  cloud_global_x = cloud_filtered ;

  // 2. Extract only the obstacles by excluding the floor:
  // TODO use pcl filter for this instead
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  for (size_t i=0;i < cloud_global_x->points.size() ; i ++){

    // any thing  withing this height is consider to be floor, else obstacle
    double height_thres =0.125; // was 0.025

    double dist_to_ground = pcl::pointToPlaneDistance    (  cloud_global_x->points[i],
                     floor_coeff.values[0], floor_coeff.values[1], floor_coeff.values[2], floor_coeff.values[3]) ;

    if ( dist_to_ground  >  height_thres){
      // alternative: pcl::pointToPlaneDistance(pt,a,b,c,d)
      inliers->indices.push_back(i);
    }
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr occupied_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr floor_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());

  pcl::ExtractIndices<PointXYZRGB> extract;
  extract.setInputCloud (cloud_global_x);
  extract.setIndices (inliers);
  extract.setNegative (false);
  extract.filter (*occupied_cloud);
  extract.setNegative (true);
  extract.filter (*floor_cloud);

  if (verbose_lcm > 1){ // quite important

    ptcld_cfg pcfg1 = ptcld_cfg(50112,    "Occupied w/o floor [Global Frame]" ,1,1, 1000,1,{1.0,0.0,0.0} );
    pc_vis_->ptcld_to_lcm(pcfg1, *occupied_cloud, null_poseT_.utime, null_poseT_.utime );

    ptcld_cfg pcfg2 = ptcld_cfg(50113,    "Floor w/o occupied [Global Frame]" ,1,1, 1000,1,{0.0,1.0,0.0} );
    pc_vis_->ptcld_to_lcm(pcfg2, *floor_cloud, null_poseT_.utime, null_poseT_.utime );

    /*
    floorcoll_cfg.id =50112;
    floorcoll_cfg.name.assign("Occupied w/o floor [Global Frame]");
    floorcoll_cfg.element_id = cloud_utime;
    floorcoll_cfg.collection = 3001;
    floorcoll_cfg.npoints = occupied_cloud->points.size();
    pcdXYZRGB_to_lcm(publish_lcm_,floorcoll_cfg, *occupied_cloud);

    floorcoll_cfg.id =51110113;
    floorcoll_cfg.name.assign("Floor w/o occupied [Global Frame]");
    floorcoll_cfg.element_id = cloud_utime;
    floorcoll_cfg.collection = 3001;
    floorcoll_cfg.npoints = floor_cloud->points.size();
    pcdXYZRGB_to_lcm(publish_lcm_,floorcoll_cfg, *floor_cloud);*/
  }

}

void Detector::find_planes (pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered){
  // 1. Read input:
  std::cerr << "[IN] : " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
  null_poseT_.utime = cloud_timestamp_;
  pc_vis_->pose_to_lcm_from_list(1000, null_poseT_);
  pc_vis_->ptcld_to_lcm_from_list(1001, *cloud_filtered, cloud_timestamp_, cloud_timestamp_);

  // 2. Extract the major planes and send them to lcm:
  FilterPlanes filtp;
  filtp.setInputCloud(cloud_filtered);
  filtp.setPoseIDs(plane_fitter_id_,cloud_timestamp_);
  filtp.setLCM(publish_lcm_);
  filtp.setDistanceThreshold(0.04);
  filtp.setStopProportion(0.10);
  vector<BasicPlane> plane_stack;
  filtp.filterPlanes(plane_stack);
  std::cout << "[OUT] number of planes extracted: " << plane_stack.size() << "\n";
  visualize_planes(plane_stack);

}

int main(const int iArgc, const char** iArgv) {
  boost::shared_ptr<lcm::LCM> theLcm(new lcm::LCM());
  if (!theLcm->good()) {
    cerr << "Cannot create lcm instance." << endl;
    return -1;
  }

  boost::shared_ptr<MapWrapper> wrapper(new MapWrapper());
  wrapper->setLcm(theLcm);
  wrapper->setMapChannel("LOCAL_MAP");
  wrapper->start();

  Detector action(wrapper);
  boost::thread thread(boost::ref(action));

  while (0 == theLcm->handle());

  wrapper->stop();

  return 0;
}


#ifndef PERIOR_TREE_POINT_OPERATIONS
#define PERIOR_TREE_POINT_OPERATIONS
#include <periortree/point_traits.hpp>
#include <boost/utility/enable_if.hpp>

namespace perior
{
namespace ops
{

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
operator+(const T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval[i] = lhs[i] + rhs[i];
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type&
operator+=(T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        lhs[i] += rhs[i];
    }
    return lhs;
}

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
operator-(const T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval[i] = lhs[i] - rhs[i];
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type&
operator-=(T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        lhs[i] -= rhs[i];
    }
    return lhs;
}

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
operator*(const T& lhs, const typename traits::scalar_type_of<T>::type& rhs)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval[i] = lhs[i] * rhs;
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
operator*(const typename traits::scalar_type_of<T>::type& lhs, const T& rhs)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval[i] = lhs * rhs[i];
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type&
operator*=(T& lhs, const typename traits::scalar_type_of<T>::type& rhs)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        lhs[i] *= rhs;
    }
    return lhs;
}

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
operator/(const T& lhs, const typename traits::scalar_type_of<T>::type& rhs)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval[i] = lhs[i] / rhs;
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type&
operator/=(T& lhs, const typename traits::scalar_type_of<T>::type& rhs)
    BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        lhs[i] /= rhs;
    }
    return lhs;
}

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, bool>::type
operator==(const T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        if(lhs[i] != rhs[i]) {return false;}
    }
    return true;
}
template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, bool>::type
operator!=(const T& lhs, const T& rhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    return !(lhs == rhs);
}

template<typename T>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<T>,
         typename traits::scalar_type_of<T>::type>::type
length_sq(const T& lhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    typename traits::scalar_type_of<T>::type retval(0);
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval += lhs[i] * lhs[i];
    }
    return retval;
}
template<typename T>
BOOST_FORCEINLINE
typename boost::enable_if<traits::is_point<T>,
         typename traits::scalar_type_of<T>::type>::type
length(const T& lhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    return std::sqrt(length_sq(lhs));
}

template<typename T>
BOOST_FORCEINLINE typename boost::enable_if<traits::is_point<T>, T>::type
abs(const T& lhs) BOOST_NOEXCEPT_OR_NOTHROW
{
    T retval;
    for(std::size_t i=0; i<traits::dimension<T>::value; ++i)
    {
        retval += std::abs(lhs[i]);
    }
    return retval;
}

}// ops
}// perior
#endif// PERIOR_TREE_POINT_OPERATIONS


#pragma once

#include "cho_util/core/geometry/geometry_base.hpp"

#include <Eigen/Core>

namespace cho {
namespace core {

template <typename _Scalar, int N>
class PointCloud : public GeometryBase<PointCloud<_Scalar, N>> {
  using Scalar = _Scalar;

 public:
  explicit constexpr PointCloud() {}
  PointCloud(std::initializer_list<Scalar> args) : data_(args) {}

  const Scalar* GetPtr() const { return data_.data(); }
  Scalar* GetPtr() { return data_.data(); }
  int GetSize() const { return N * data_.cols(); }

  auto& GetData() { return data_; }
  const auto& GetData() const { return data_; }

  int GetNumPoints() const { return data_.cols(); }
  void SetNumPoints(const int n) { data_.resize(N, n); }

  auto GetPoint(const int i) { return data_.col(i); }
  const auto GetPoint(const int i) const { return data_.col(i); }

  bool IsEmpty() const { return GetNumPoints() <= 0; }

 private:
  // layout: row=ndim, col=npoints
  Eigen::Matrix<Scalar, N, Eigen::Dynamic> data_;

 public:
  friend class GeometryBase<PointCloud>;
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename _Scalar, int N>
struct GeometryTraits<PointCloud<_Scalar, N>> {
  using Scalar = _Scalar;
};

}  // namespace core
}  // namespace cho


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_DIAGONAL_INCLUDE
#define MTL_DIAGONAL_INCLUDE

#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    namespace vec {

	/// Transform a vector into a diagonal matrix
	template <typename Vector>
	mtl::mat::compressed2D<typename Collection<Vector>::value_type, mat::parameters<> >
	// typename mtl::traits::enable_if_vector<Vector, mtl::mat::compressed2D<typename Collection<Vector>::value_type> >::type
	inline diagonal(const Vector& v)
	{
	    vampir_trace<2016> tracer;
	    typedef mtl::mat::compressed2D<typename Collection<Vector>::value_type, mat::parameters<> > matrix_type;
	    matrix_type                           D(size(v), size(v));
	    D= 0;
	    mtl::mat::inserter<matrix_type>    ins(D, 1);
	    for (typename Collection<Vector>::size_type i= 0; i < size(v); ++i)
		ins[i][i] << v[i];

	    return D;
	}
    }

    namespace mat {

	/// Return the vector with the diagonal of the matrix
	template <typename Matrix>
	// typename mtl::traits::enable_if_matrix<Matrix, conj_view<Matrix> >::type
	mtl::vec::dense_vector<typename Collection<Matrix>::value_type, vec::parameters<> >
	inline diagonal(const Matrix& A)
	{
	    vampir_trace<3007> tracer;
	    using std::min;
	    typedef typename Collection<Matrix>::size_type size_type;
	    size_type n= min(num_rows(A), num_cols(A));
		mtl::vec::dense_vector<typename Collection<Matrix>::value_type, vec::parameters<> > v(n);

	    for (size_type i= 0; i < n; ++i)
		v[i]= A[i][i];
	    return v;
	}
    }

} // namespace mtl

#endif // MTL_DIAGONAL_INCLUDE


// Boost.Polygon library voronoi_builder_test.cpp file

//          Copyright Andrii Sydorchuk 2010-2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#include <ctime>
#include <limits>
#include <list>
#include <vector>

#define BOOST_TEST_MODULE voronoi_builder_test
#include <boost/mpl/list.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/test/test_case_template.hpp>

#include <boost/polygon/polygon.hpp>
#include <boost/polygon/voronoi.hpp>
#include "voronoi_test_helper.hpp"
using boost::polygon::voronoi_builder;
using boost::polygon::voronoi_diagram;

typedef boost::mpl::list<int> test_types;
typedef voronoi_diagram<double> vd_type;
typedef vd_type::coordinate_type coordinate_type;
typedef vd_type::edge_type voronoi_edge_type;
typedef vd_type::const_cell_iterator const_cell_iterator;
typedef vd_type::const_vertex_iterator const_vertex_iterator;

#define CHECK_OUTPUT_SIZE(output, cells, vertices, edges) \
    BOOST_CHECK_EQUAL(output.num_cells(), (std::size_t)cells); \
    BOOST_CHECK_EQUAL(output.num_vertices(), (std::size_t)vertices); \
    BOOST_CHECK_EQUAL(output.num_edges(), (std::size_t)edges)

#define VERIFY_OUTPUT(output) \
    BOOST_CHECK(voronoi_test_helper::verify_output(output, \
        voronoi_test_helper::CELL_CONVEXITY)); \
    BOOST_CHECK(voronoi_test_helper::verify_output(output, \
        voronoi_test_helper::INCIDENT_EDGES_CCW_ORDER)); \
    BOOST_CHECK(voronoi_test_helper::verify_output(output, \
        voronoi_test_helper::NO_HALF_EDGE_INTERSECTIONS))

#define VERIFY_NO_HALF_EDGE_INTERSECTIONS(output) \
    BOOST_CHECK(voronoi_test_helper::verify_output(output, \
        voronoi_test_helper::NO_HALF_EDGE_INTERSECTIONS))

// Sites: (0, 0).
BOOST_AUTO_TEST_CASE_TEMPLATE(single_site_test, T, test_types) {
  std::vector< point_data<T> > points;
  points.push_back(point_data<T>(0, 0));
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);

  BOOST_CHECK(test_output.cells().size() == 1);
  CHECK_OUTPUT_SIZE(test_output, 1, 0, 0);

  const_cell_iterator it = test_output.cells().begin();
  BOOST_CHECK(it->incident_edge() == NULL);
}

// Sites: (0, 0), (0, 1).
BOOST_AUTO_TEST_CASE_TEMPLATE(collinear_sites_test1, T, test_types) {
  std::vector< point_data<T> > points;
  points.push_back(point_data<T>(0, 0));
  points.push_back(point_data<T>(0, 1));
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);
  CHECK_OUTPUT_SIZE(test_output, 2, 0, 2);

  const_cell_iterator cell_it = test_output.cells().begin();
  cell_it++;

  const voronoi_edge_type* edge1_1 = cell_it->incident_edge();
  const voronoi_edge_type* edge1_2 = edge1_1->twin();

  BOOST_CHECK(edge1_1->twin() == edge1_2);
  BOOST_CHECK(edge1_2->twin() == edge1_1);

  BOOST_CHECK(edge1_1->next() == edge1_1);
  BOOST_CHECK(edge1_1->prev() == edge1_1);
  BOOST_CHECK(edge1_1->rot_next() == edge1_2);
  BOOST_CHECK(edge1_1->rot_prev() == edge1_2);

  BOOST_CHECK(edge1_2->next() == edge1_2);
  BOOST_CHECK(edge1_2->prev() == edge1_2);
  BOOST_CHECK(edge1_2->rot_next() == edge1_1);
  BOOST_CHECK(edge1_2->rot_prev() == edge1_1);
}

// Sites: (0, 0), (1, 1), (2, 2).
BOOST_AUTO_TEST_CASE_TEMPLATE(collinear_sites_test2, T, test_types) {
  std::vector< point_data<T> > points;
  points.push_back(point_data<T>(0, 0));
  points.push_back(point_data<T>(1, 1));
  points.push_back(point_data<T>(2, 2));
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);
  CHECK_OUTPUT_SIZE(test_output, 3, 0, 4);

  const_cell_iterator cell_it = test_output.cells().begin();
  const voronoi_edge_type* edge1_1 = cell_it->incident_edge();
  const voronoi_edge_type* edge1_2 = edge1_1->twin();
  cell_it++;
  cell_it++;
  const voronoi_edge_type* edge2_2 = cell_it->incident_edge();
  const voronoi_edge_type* edge2_1 = edge2_2->twin();

  BOOST_CHECK(edge1_1->twin() == edge1_2 && edge1_2->twin() == edge1_1);
  BOOST_CHECK(edge2_1->twin() == edge2_2 && edge2_2->twin() == edge2_1);

  BOOST_CHECK(edge1_1->next() == edge1_1 && edge1_1->prev() == edge1_1);
  BOOST_CHECK(edge1_2->next() == edge2_1 && edge1_2->prev() == edge2_1);
  BOOST_CHECK(edge2_1->next() == edge1_2 && edge2_1->prev() == edge1_2);
  BOOST_CHECK(edge2_2->next() == edge2_2 && edge2_2->prev() == edge2_2);

  BOOST_CHECK(edge1_1->rot_next() == edge1_2 && edge1_1->rot_prev() == edge2_1);
  BOOST_CHECK(edge1_2->rot_next() == edge2_2 && edge1_2->rot_prev() == edge1_1);
  BOOST_CHECK(edge2_1->rot_next() == edge1_1 && edge2_1->rot_prev() == edge2_2);
  BOOST_CHECK(edge2_2->rot_next() == edge2_1 && edge2_2->rot_prev() == edge1_2);

  BOOST_CHECK(edge1_2->next() == edge2_1 && edge1_2->prev() == edge2_1);
  BOOST_CHECK(edge2_1->next() == edge1_2 && edge2_1->prev() == edge1_2);
}

// Sites: (0, 0), (0, 4), (2, 1).
BOOST_AUTO_TEST_CASE_TEMPLATE(triangle_test1, T, test_types) {
  point_data<T> point1(0, 0);
  point_data<T> point2(0, 4);
  point_data<T> point3(2, 1);
  std::vector< point_data<T> > points;
  points.push_back(point1);
  points.push_back(point2);
  points.push_back(point3);
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);
  CHECK_OUTPUT_SIZE(test_output, 3, 1, 6);

  const_vertex_iterator it = test_output.vertices().begin();
  BOOST_CHECK_EQUAL(it->x(), 0.25);
  BOOST_CHECK_EQUAL(it->y(), 2.0);

  const voronoi_edge_type* edge1_1 = it->incident_edge();
  const voronoi_edge_type* edge1_2 = edge1_1->twin();
  BOOST_CHECK(edge1_1->cell()->source_index() == 1);
  BOOST_CHECK(edge1_2->cell()->source_index() == 2);

  const voronoi_edge_type* edge2_1 = edge1_1->rot_prev();
  const voronoi_edge_type* edge2_2 = edge2_1->twin();
  BOOST_CHECK(edge2_1->cell()->source_index() == 2);
  BOOST_CHECK(edge2_2->cell()->source_index() == 0);

  const voronoi_edge_type* edge3_1 = edge2_1->rot_prev();
  const voronoi_edge_type* edge3_2 = edge3_1->twin();
  BOOST_CHECK(edge3_1->cell()->source_index() == 0);
  BOOST_CHECK(edge3_2->cell()->source_index() == 1);

  BOOST_CHECK(edge1_2->twin() == edge1_1);
  BOOST_CHECK(edge2_2->twin() == edge2_1);
  BOOST_CHECK(edge3_2->twin() == edge3_1);

  BOOST_CHECK(edge1_1->prev() == edge3_2 && edge1_1->next() == edge3_2);
  BOOST_CHECK(edge2_1->prev() == edge1_2 && edge2_1->next() == edge1_2);
  BOOST_CHECK(edge3_1->prev() == edge2_2 && edge3_1->next() == edge2_2);

  BOOST_CHECK(edge1_2->next() == edge2_1 && edge1_2->prev() == edge2_1);
  BOOST_CHECK(edge2_2->next() == edge3_1 && edge2_2->prev() == edge3_1);
  BOOST_CHECK(edge3_2->next() == edge1_1 && edge3_2->prev() == edge1_1);

  BOOST_CHECK(edge1_1->rot_next() == edge3_1);
  BOOST_CHECK(edge3_1->rot_next() == edge2_1);
  BOOST_CHECK(edge2_1->rot_next() == edge1_1);

  BOOST_CHECK(edge1_2->rot_next() == edge2_2);
  BOOST_CHECK(edge2_2->rot_next() == edge3_2);
  BOOST_CHECK(edge3_2->rot_next() == edge1_2);
}

// Sites: (0, 1), (2, 0), (2, 4).
BOOST_AUTO_TEST_CASE_TEMPLATE(triangle_test2, T, test_types) {
  point_data<T> point1(0, 1);
  point_data<T> point2(2, 0);
  point_data<T> point3(2, 4);
  std::vector< point_data<T> > points;
  points.push_back(point1);
  points.push_back(point2);
  points.push_back(point3);
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);
  CHECK_OUTPUT_SIZE(test_output, 3, 1, 6);

  const_vertex_iterator it = test_output.vertices().begin();
  BOOST_CHECK_EQUAL(it->x(), 1.75);
  BOOST_CHECK_EQUAL(it->y(), 2.0);

  const voronoi_edge_type* edge1_1 = it->incident_edge();
  const voronoi_edge_type* edge1_2 = edge1_1->twin();
  BOOST_CHECK(edge1_1->cell()->source_index() == 2);
  BOOST_CHECK(edge1_2->cell()->source_index() == 1);

  const voronoi_edge_type* edge2_1 = edge1_1->rot_prev();
  const voronoi_edge_type* edge2_2 = edge2_1->twin();
  BOOST_CHECK(edge2_1->cell()->source_index() == 1);
  BOOST_CHECK(edge2_2->cell()->source_index() == 0);

  const voronoi_edge_type* edge3_1 = edge2_1->rot_prev();
  const voronoi_edge_type* edge3_2 = edge3_1->twin();
  BOOST_CHECK(edge3_1->cell()->source_index() == 0);
  BOOST_CHECK(edge3_2->cell()->source_index() == 2);

  BOOST_CHECK(edge1_2->twin() == edge1_1);
  BOOST_CHECK(edge2_2->twin() == edge2_1);
  BOOST_CHECK(edge3_2->twin() == edge3_1);

  BOOST_CHECK(edge1_1->prev() == edge3_2 && edge1_1->next() == edge3_2);
  BOOST_CHECK(edge2_1->prev() == edge1_2 && edge2_1->next() == edge1_2);
  BOOST_CHECK(edge3_1->prev() == edge2_2 && edge3_1->next() == edge2_2);

  BOOST_CHECK(edge1_2->next() == edge2_1 && edge1_2->prev() == edge2_1);
  BOOST_CHECK(edge2_2->next() == edge3_1 && edge2_2->prev() == edge3_1);
  BOOST_CHECK(edge3_2->next() == edge1_1 && edge3_2->prev() == edge1_1);

  BOOST_CHECK(edge1_1->rot_next() == edge3_1);
  BOOST_CHECK(edge3_1->rot_next() == edge2_1);
  BOOST_CHECK(edge2_1->rot_next() == edge1_1);

  BOOST_CHECK(edge1_2->rot_next() == edge2_2);
  BOOST_CHECK(edge2_2->rot_next() == edge3_2);
  BOOST_CHECK(edge3_2->rot_next() == edge1_2);
}

// Sites: (0, 0), (0, 1), (1, 0), (1, 1).
BOOST_AUTO_TEST_CASE_TEMPLATE(square_test1, T, test_types) {
  point_data<T> point1(0, 0);
  point_data<T> point2(0, 1);
  point_data<T> point3(1, 0);
  point_data<T> point4(1, 1);
  std::vector< point_data<T> > points;
  points.push_back(point1);
  points.push_back(point2);
  points.push_back(point3);
  points.push_back(point4);
  vd_type test_output;
  construct_voronoi(points.begin(), points.end(), &test_output);
  VERIFY_OUTPUT(test_output);
  CHECK_OUTPUT_SIZE(test_output, 4, 1, 8);

  // Check voronoi vertex.
  const_vertex_iterator it = test_output.vertices().begin();
  BOOST_CHECK_EQUAL(it->x(), 0.5);
  BOOST_CHECK_EQUAL(it->y(), 0.5);

  // Check voronoi edges.
  const voronoi_edge_type* edge1_1 = it->incident_edge();
  const voronoi_edge_type* edge1_2 = edge1_1->twin();
  BOOST_CHECK(edge1_1->cell()->source_index() == 3);
  BOOST_CHECK(edge1_2->cell()->source_index() == 2);

  const voronoi_edge_type* edge2_1 = edge1_1->rot_prev();
  const voronoi_edge_type* edge2_2 = edge2_1->twin();
  BOOST_CHECK(edge2_1->cell()->source_index() == 2);
  BOOST_CHECK(edge2_2->cell()->source_index() == 0);

  const voronoi_edge_type* edge3_1 = edge2_1->rot_prev();
  const voronoi_edge_type* edge3_2 = edge3_1->twin();
  BOOST_CHECK(edge3_1->cell()->source_index() == 0);
  BOOST_CHECK(edge3_2->cell()->source_index() == 1);

  const voronoi_edge_type* edge4_1 = edge3_1->rot_prev();
  const voronoi_edge_type* edge4_2 = edge4_1->twin();
  BOOST_CHECK(edge4_1->cell()->source_index() == 1);
  BOOST_CHECK(edge4_2->cell()->source_index() == 3);

  BOOST_CHECK(edge1_2->twin() == edge1_1);
  BOOST_CHECK(edge2_2->twin() == edge2_1);
  BOOST_CHECK(edge3_2->twin() == edge3_1);
  BOOST_CHECK(edge4_2->twin() == edge4_1);

  BOOST_CHECK(edge1_1->prev() == edge4_2 && edge1_1->next() == edge4_2);
  BOOST_CHECK(edge2_1->prev() == edge1_2 && edge2_1->next() == edge1_2);
  BOOST_CHECK(edge3_1->prev() == edge2_2 && edge3_1->next() == edge2_2);
  BOOST_CHECK(edge4_1->prev() == edge3_2 && edge4_1->next() == edge3_2);

  BOOST_CHECK(edge1_2->next() == edge2_1 && edge1_2->prev() == edge2_1);
  BOOST_CHECK(edge2_2->next() == edge3_1 && edge2_2->prev() == edge3_1);
  BOOST_CHECK(edge3_2->next() == edge4_1 && edge3_2->prev() == edge4_1);
  BOOST_CHECK(edge4_2->next() == edge1_1 && edge4_2->prev() == edge1_1);

  BOOST_CHECK(edge1_1->rot_next() == edge4_1);
  BOOST_CHECK(edge4_1->rot_next() == edge3_1);
  BOOST_CHECK(edge3_1->rot_next() == edge2_1);
  BOOST_CHECK(edge2_1->rot_next() == edge1_1);

  BOOST_CHECK(edge1_2->rot_next() == edge2_2);
  BOOST_CHECK(edge2_2->rot_next() == edge3_2);
  BOOST_CHECK(edge3_2->rot_next() == edge4_2);
  BOOST_CHECK(edge4_2->rot_next() == edge1_2);
}

#ifdef NDEBUG
BOOST_AUTO_TEST_CASE_TEMPLATE(grid_test, T, test_types) {
  vd_type test_output_small, test_output_large;
  std::vector< point_data<T> > point_vec_small, point_vec_large;
  int grid_size[] = {10, 33, 101};
  int max_value[] = {10, 33, 101};
  int array_length = sizeof(grid_size) / sizeof(int);
  for (int k = 0; k < array_length; k++) {
    test_output_small.clear();
    test_output_large.clear();
    point_vec_small.clear();
    point_vec_large.clear();
    int koef = (std::numeric_limits<int>::max)() / max_value[k];
    for (int i = 0; i < grid_size[k]; i++) {
      for (int j = 0; j < grid_size[k]; j++) {
        point_vec_small.push_back(point_data<T>(i, j));
        point_vec_large.push_back(point_data<T>(koef * i, koef * j));
      }
    }
    construct_voronoi(point_vec_small.begin(), point_vec_small.end(), &test_output_small);
    construct_voronoi(point_vec_large.begin(), point_vec_large.end(), &test_output_large);
    VERIFY_OUTPUT(test_output_small);
    VERIFY_OUTPUT(test_output_large);
    unsigned int num_cells = grid_size[k] * grid_size[k];
    unsigned int num_vertices = num_cells - 2 * grid_size[k] + 1;
    unsigned int num_edges = 4 * num_cells - 4 * grid_size[k];
    CHECK_OUTPUT_SIZE(test_output_small, num_cells, num_vertices, num_edges);
    CHECK_OUTPUT_SIZE(test_output_large, num_cells, num_vertices, num_edges);
  }
}
#endif

#ifdef NDEBUG
BOOST_AUTO_TEST_CASE_TEMPLATE(random_test, T, test_types) {
  boost::mt19937 gen(static_cast<unsigned int>(time(NULL)));
  vd_type test_output_small, test_output_large;
  std::vector< point_data<T> > point_vec_small, point_vec_large;
  int num_points[] = {10, 100, 1000, 10000};
  int num_runs[] = {1000, 100, 10, 1};
  int mod_koef[] = {10, 100, 100, 1000};
  int max_value[] = {5, 50, 50, 5000};
  int array_length = sizeof(num_points) / sizeof(int);
  for (int k = 0; k < array_length; k++) {
    int koef = (std::numeric_limits<int>::max)() / max_value[k];
    for (int i = 0; i < num_runs[k]; i++) {
      test_output_small.clear();
      test_output_large.clear();
      point_vec_small.clear();
      point_vec_large.clear();
      for (int j = 0; j < num_points[k]; j++) {
        T x = gen() % mod_koef[k] - mod_koef[k] / 2;
        T y = gen() % mod_koef[k] - mod_koef[k] / 2;
        point_vec_small.push_back(point_data<T>(x, y));
        point_vec_large.push_back(point_data<T>(koef * x, koef * y));
      }
      construct_voronoi(point_vec_small.begin(), point_vec_small.end(), &test_output_small);
      construct_voronoi(point_vec_large.begin(), point_vec_large.end(), &test_output_large);
      VERIFY_OUTPUT(test_output_small);
      VERIFY_OUTPUT(test_output_large);
      BOOST_CHECK_EQUAL(test_output_small.num_cells(), test_output_large.num_cells());
      BOOST_CHECK_EQUAL(test_output_small.num_vertices(), test_output_large.num_vertices());
      BOOST_CHECK_EQUAL(test_output_small.num_edges(), test_output_large.num_edges());
    }
  }
}
#endif

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_sites_test1, T, test_types) {
  vd_type test_output;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(1, 1);
  segments.push_back(segment_data<T>(point1, point2));
  construct_voronoi(segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 3, 0, 4);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_sites_test2, T, test_types) {
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(4, 4);
  point_data<T> point3(3, 1);
  point_data<T> point4(1, 3);
  segments.push_back(segment_data<T>(point1, point2));
  points.push_back(point3);
  points.push_back(point4);
  construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 5, 4, 16);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_sites_test3, T, test_types) {
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(4, 0);
  point_data<T> point2(0, 4);
  point_data<T> point3(3, 3);
  point_data<T> point4(1, 1);
  segments.push_back(segment_data<T>(point1, point2));
  points.push_back(point3);
  points.push_back(point4);
  construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 5, 4, 16);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_sites_test4, T, test_types) {
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(4, 0);
  point_data<T> point2(0, 4);
  point_data<T> point3(3, 2);
  point_data<T> point4(2, 3);
  segments.push_back(segment_data<T>(point1, point2));
  points.push_back(point3);
  points.push_back(point4);
  construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 5, 3, 14);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_site_test5, T, test_types) {
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(0, 8);
  point_data<T> point3(-2, -2);
  point_data<T> point4(-2, 4);
  point_data<T> point5(-2, 10);
  segments.push_back(segment_data<T>(point1, point2));
  points.push_back(point3);
  points.push_back(point4);
  points.push_back(point5);
  construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 6, 4, 18);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_site_test6, T, test_types) {
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(-1, 1);
  point_data<T> point2(1, 0);
  point_data<T> point3(1, 2);
  segments.push_back(segment_data<T>(point2, point3));
  points.push_back(point1);
  construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
  using boost::polygon::detail::int32;
  using boost::polygon::detail::int64;
  using boost::polygon::detail::fpt64;
  BOOST_CHECK_EQUAL(sizeof(detail::fpt64), 8U);
  BOOST_CHECK_EQUAL(sizeof(detail::int32), 4U);
  BOOST_CHECK_EQUAL(sizeof(detail::int64), 8U);
  vd_type::vertex_type v = test_output.vertices()[0];
  BOOST_CHECK_EQUAL(v.x(), -0.25);
  BOOST_CHECK_EQUAL(v.y(), 0.0);
  CHECK_OUTPUT_SIZE(test_output, 4, 2, 10);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_site_test7, T, test_types) {
  vd_type test_output;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(4, 0);
  point_data<T> point3(0, 4);
  point_data<T> point4(4, 4);
  segments.push_back(segment_data<T>(point1, point2));
  segments.push_back(segment_data<T>(point2, point3));
  segments.push_back(segment_data<T>(point3, point4));
  construct_voronoi(segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 7, 6, 24);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_site_test8, T, test_types) {
  vd_type test_output;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(4, 0);
  point_data<T> point3(4, 4);
  point_data<T> point4(0, 4);
  segments.push_back(segment_data<T>(point1, point2));
  segments.push_back(segment_data<T>(point2, point3));
  segments.push_back(segment_data<T>(point3, point4));
  segments.push_back(segment_data<T>(point4, point1));
  construct_voronoi(segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 8, 5, 24);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(segment_site_test9, T, test_types) {
  vd_type test_output;
  std::vector< segment_data<T> > segments;
  point_data<T> point1(0, 0);
  point_data<T> point2(2, 0);
  point_data<T> point3(4, 0);
  segments.push_back(segment_data<T>(point1, point2));
  segments.push_back(segment_data<T>(point2, point3));
  construct_voronoi(segments.begin(), segments.end(), &test_output);
  CHECK_OUTPUT_SIZE(test_output, 5, 0, 8);
  VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
}

#ifdef NDEBUG
BOOST_AUTO_TEST_CASE_TEMPLATE(segment_grid_test, T, test_types) {
  vd_type test_output_small, test_output_large;
  std::vector< segment_data<T> > segments_small, segments_large;
  int grid_size[] = {10, 27, 53};
  int max_value[] = {100, 330, 1000};
  int array_length = sizeof(grid_size) / sizeof(int);
  for (int k = 0; k < array_length; k++) {
    test_output_small.clear();
    test_output_large.clear();
    segments_small.clear();
    segments_large.clear();
    int cur_sz = grid_size[k];
    int koef = (std::numeric_limits<int>::max)() / max_value[k];
    for (int i = 0; i < cur_sz + 1; i++)
      for (int j = 0; j < cur_sz; j++) {
        point_data<T> point1_1(10 * i, 10 * j);
        point_data<T> point1_2(koef * 10 * i, koef * 10 * j);
        point_data<T> point2_1(10 * i, 10 * j + 10);
        point_data<T> point2_2(koef * 10 * i, koef * (10 * j + 10));
        segments_small.push_back(segment_data<T>(point1_1, point2_1));
        segments_large.push_back(segment_data<T>(point1_2, point2_2));
        point_data<T> point3_1(10 * j, 10 * i);
        point_data<T> point3_2(koef * 10 * j, koef * 10 * i);
        point_data<T> point4_1(10 * j + 10, 10 * i);
        point_data<T> point4_2(koef * (10 * j + 10), koef * 10 * i);
        segments_small.push_back(segment_data<T>(point3_1, point4_1));
        segments_large.push_back(segment_data<T>(point3_2, point4_2));
      }
    construct_voronoi(segments_small.begin(), segments_small.end(), &test_output_small);
    construct_voronoi(segments_large.begin(), segments_large.end(), &test_output_large);
    VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output_small);
    VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output_large);
    BOOST_CHECK_EQUAL(test_output_small.num_cells(), test_output_large.num_cells());
    BOOST_CHECK_EQUAL(test_output_small.num_vertices(), test_output_large.num_vertices());
    BOOST_CHECK_EQUAL(test_output_small.num_edges(), test_output_large.num_edges());
  }
}
#endif

#ifdef NDEBUG
BOOST_AUTO_TEST_CASE_TEMPLATE(segment_random_test1, T, test_types) {
  boost::mt19937 gen(static_cast<unsigned int>(time(NULL)));
  vd_type test_output;
  std::vector< point_data<T> > points;
  std::vector< segment_data<T> > segments;
  int num_runs = 1000;
  int num_segments = 10;
  points.push_back(point_data<T>(-100, -100));
  points.push_back(point_data<T>(-100, 100));
  points.push_back(point_data<T>(100, -100));
  points.push_back(point_data<T>(100, 100));
  for (int i = 0; i < num_runs; i++) {
    test_output.clear();
    segments.clear();
    for (int j = 0; j < num_segments; j++) {
      T x1 = 0, y1 = 0, x2 = 0, y2 = 0;
      while (x1 == x2 && y1 == y2) {
        x1 = (gen() % 100) - 50;
        y1 = (gen() % 100) - 50;
        x2 = (gen() % 100) - 50;
        y2 = (gen() % 100) - 50;
      }
      point_data<T> point1(x1, y1);
      point_data<T> point2(x2, y2);
      segments.push_back(segment_data<T>(point1, point2));
    }
    voronoi_test_helper::clean_segment_set(segments);
    construct_voronoi(points.begin(), points.end(), segments.begin(), segments.end(), &test_output);
    VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output);
  }
}
#endif

#ifdef NDEBUG
BOOST_AUTO_TEST_CASE_TEMPLATE(segment_random_test2, T, test_types) {
  boost::mt19937 gen(static_cast<unsigned int>(time(NULL)));
  vd_type test_output_small, test_output_large;
  std::vector< segment_data<T> > segments_small, segments_large;
  int num_segments[] = {5, 25, 125, 625};
  int num_runs[] = {1000, 100, 10, 1};
  int mod_koef1[] = {10, 100, 200, 300};
  int mod_koef2[] = {10, 20, 50, 100};
  int max_value[] = {10, 60, 125, 200};
  int array_length = sizeof(num_segments) / sizeof(int);
  for (int k = 0; k < array_length; k++) {
    int koef = (std::numeric_limits<int>::max)() / max_value[k];
    for (int i = 0; i < num_runs[k]; i++) {
      test_output_small.clear();
      test_output_large.clear();
      segments_small.clear();
      segments_large.clear();
      for (int j = 0; j < num_segments[k]; j++) {
        T x1 = (gen() % mod_koef1[k]) - mod_koef1[k] / 2;
        T y1 = (gen() % mod_koef1[k]) - mod_koef1[k] / 2;
        T dx = 0, dy = 0;
        while (dx == 0 && dy == 0) {
          dx = (gen() % mod_koef2[k]) - mod_koef2[k] / 2;
          dy = (gen() % mod_koef2[k]) - mod_koef2[k] / 2;
        }
        T x2 = x1 + dx;
        T y2 = y1 + dy;
        point_data<T> point1_small(x1, y1);
        point_data<T> point2_small(x2, y2);
        segments_small.push_back(segment_data<T>(point1_small, point2_small));
      }
      voronoi_test_helper::clean_segment_set(segments_small);
      for (typename std::vector< segment_data<T> >::iterator it = segments_small.begin();
           it != segments_small.end(); ++it) {
        T x1 = it->low().x() * koef;
        T y1 = it->low().y() * koef;
        T x2 = it->high().x() * koef;
        T y2 = it->high().y() * koef;
        point_data<T> point1_large(x1, y1);
        point_data<T> point2_large(x2, y2);
        segments_large.push_back(segment_data<T>(point1_large, point2_large));
      }
      construct_voronoi(segments_small.begin(), segments_small.end(), &test_output_small);
      construct_voronoi(segments_large.begin(), segments_large.end(), &test_output_large);
      VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output_small);
      VERIFY_NO_HALF_EDGE_INTERSECTIONS(test_output_large);
      BOOST_CHECK_EQUAL(test_output_small.num_cells(), test_output_large.num_cells());
      BOOST_CHECK_EQUAL(test_output_small.num_vertices(), test_output_large.num_vertices());
      BOOST_CHECK_EQUAL(test_output_small.num_edges(), test_output_large.num_edges());
    }
  }
}
#endif