// Copyright (c) 2014 Stefan Walk
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: LicenseRef-RFL
// License notice in Installation/LICENSE.RFL
//
// Author(s)     : Stefan Walk

// Modifications from original library:
//  * changed inclusion protection tag
//  * moved to namespace CGAL::internal::
//  * fix computation of node_dist[label] so that results are always <= 1.0
//  * change serialization functions to avoid a bug with boost and some
//    compilers (that leads to dereferencing a null pointer)
//  * add a method to get feature usage

#ifndef CGAL_INTERNAL_LIBLEARNING_RANDOMFORESTS_NODE_H
#define CGAL_INTERNAL_LIBLEARNING_RANDOMFORESTS_NODE_H
#include "../dataview.h"
#include "common-libraries.hpp"

#if defined(CGAL_LINKED_WITH_BOOST_IOSTREAMS) && defined(CGAL_LINKED_WITH_BOOST_SERIALIZATION)
#include <boost/serialization/scoped_ptr.hpp>
#include <boost/serialization/vector.hpp>
#else
#include <boost/scoped_ptr.hpp>
#include <vector>
#endif

#if VERBOSE_NODE_LEARNING
#include <cstdio>
#endif

namespace CGAL { namespace internal {

namespace liblearning {
namespace RandomForest {

template <typename Derived, typename ParamT, typename Splitter>
class Node {
public:
    typedef typename Splitter::FeatureType FeatureType;
    bool is_leaf;
    size_t n_samples;
    size_t depth;
    typedef ParamT ParamType;
    ParamType const* params;
    Splitter splitter;

    boost::scoped_ptr<Derived> left;
    boost::scoped_ptr<Derived> right;
    std::vector<float> node_dist;

    Node() : is_leaf(true), n_samples(0), depth(-1), params(0) {}
    Node(size_t depth, ParamType const* params) :
        is_leaf(true), n_samples(0), depth(depth), params(params)
    {}

    bool pure(DataView2D<int> labels, int* sample_idxes) const {
        if (n_samples < 2)
            return true; // an empty node is by definition pure
        int first_sample_idx = sample_idxes[0];
        int seen_class = labels(first_sample_idx, 0);
        // check if all classes are equal to the first class
        for (size_t i_sample = 1; i_sample < n_samples; ++i_sample) {
            int sample_idx = sample_idxes[i_sample];
            if (labels(sample_idx, 0) != seen_class)
                return false;
        }
        return true;
    }

    float const* votes() const {
        return (float const*)&node_dist[0];
    }

    int partition_samples(DataView2D<FeatureType> samples, int* sample_idxes) {
        // sort samples in bag so that left-samples precede right-samples
        // works like std::partition
        int low  = 0;
        int high = n_samples;

        while (true) {
            while (true) {
                if (low == high) {
                    return low;
                } else if (!splitter.classify_sample(samples.row_pointer(sample_idxes[low]))) {
                    ++low;
                } else {
                    break;
                }
            }
            --high;
            while (true) {
                if (low == high) {
                    return low;
                } else if (splitter.classify_sample(samples.row_pointer(sample_idxes[high]))) {
                    --high;
                } else {
                    break;
                }
            }
            std::swap(sample_idxes[low], sample_idxes[high]);
            ++low;
        }
    }

    Derived const* split (FeatureType const* sample) const {
        if (splitter.classify_sample(sample)) {
            return right.get();
        } else {
            return left.get();
        }
    }

    typedef std::list<Derived const*> NodeList;

    NodeList get_all_childs() {
        NodeList ret;
        ret.push_back(this);
        if (!is_leaf) {
            NodeList left_childs  = left->get_all_childs();
            ret.splice(ret.end(), left_childs);
            NodeList right_childs = right->get_all_childs();
            ret.splice(ret.end(), right_childs);
        }
        return ret;
    }

    template<typename SplitGenerator>
    void determine_best_split(DataView2D<FeatureType> samples,
                              DataView2D<int>         labels,
                              int*                  sample_idxes,
                              SplitGenerator        split_generator,
                              RandomGen&            gen
                              )
    {
        typename Splitter::FeatureClassData data_points;
        init_feature_class_data(data_points, params->n_classes, n_samples);
        float best_loss = std::numeric_limits<float>::infinity();

        std::vector<uint64_t> classes_l;
        std::vector<uint64_t> classes_r;

        // pass information about data to split generator
        split_generator.init(samples,
                             labels,
                             sample_idxes,
                             n_samples,
                             params->n_classes,
                             gen);

        size_t n_proposals = split_generator.num_proposals();

        std::pair<FeatureType, float> results; // (threshold, loss)

        for (size_t i_proposal = 0; i_proposal < n_proposals; ++i_proposal) {
            // generate proposal
            Splitter split = split_generator.gen_proposal(gen);
            // map samples to numbers using proposal
            split.map_points(samples, labels, sample_idxes, n_samples, data_points);
            // check best loss using this proposal
            results = static_cast<Derived*>(this)->determine_best_threshold(data_points, classes_l, classes_r, gen);
            if (results.second < best_loss) {
                // Proposal resulted into new optimum
                best_loss = results.second;
                split.set_threshold(results.first);
                splitter = split;
            }
        }
    }

    template<typename SplitGenerator>
    void train(DataView2D<FeatureType> samples,
               DataView2D<int> labels,
               int* sample_idxes,
               size_t n_samples_,
               SplitGenerator const& split_generator,
               RandomGen& gen
               )
    {
        n_samples = n_samples_;
        node_dist.resize(params->n_classes, 0.0f);
        for (size_t i_sample = 0; i_sample < n_samples; ++i_sample) {
            int label = labels(sample_idxes[i_sample], 0);
            node_dist[label] += 1.0f;
        }

        if (n_samples != 0)
          for (std::size_t i = 0; i < node_dist.size(); ++ i)
            node_dist[i] /= n_samples;

        bool do_split = // Only split if ...
            (n_samples >= params->min_samples_per_node) && // enough samples are available
            !pure(labels, sample_idxes) && // this node is not already pure
            (depth < params->max_depth); // we did not reach max depth
        if (!do_split) {
            splitter.threshold = 0.0;
            return;
        }

        is_leaf = false;

#if VERBOSE_NODE_LEARNING
        std::printf("Determining the best split at depth %zu/%zu\n", depth, params->max_depth);
#endif
        determine_best_split(samples, labels, sample_idxes, split_generator, gen);

        left.reset(new Derived(depth + 1, params));
        right.reset(new Derived(depth + 1, params));

        // sort samples in bag so that left-samples precede right-samples
        int low = partition_samples(samples, sample_idxes);
        int n_samples_left  = low;
        int n_samples_right = n_samples - low;
        int offset_left     = 0;
        int offset_right    = low;
#ifdef TREE_GRAPHVIZ_STREAM
        if (depth <= TREE_GRAPHVIZ_MAX_DEPTH) {
            TREE_GRAPHVIZ_STREAM << "p" << std::hex << (unsigned long)this
                << " -> "
                << "p" << std::hex << (unsigned long)left.get()
                << std::dec << " [label=\"" << n_samples_left << "\"];" <<  std::endl;
            TREE_GRAPHVIZ_STREAM << "p" << std::hex << (unsigned long)this
                << " -> "
                << "p" << std::hex << (unsigned long)right.get()
                << std::dec << " [label=\"" << n_samples_right << "\"];" << std::endl;
        }
#endif
        // train left and right side of split
        left->train (samples, labels, sample_idxes + offset_left,  n_samples_left,  split_generator, gen);
        right->train(samples, labels, sample_idxes + offset_right, n_samples_right, split_generator, gen);
    }

#if defined(CGAL_LINKED_WITH_BOOST_IOSTREAMS) && defined(CGAL_LINKED_WITH_BOOST_SERIALIZATION)
    template <typename Archive>
    void serialize(Archive& ar, unsigned /*version*/)
    {
        ar & BOOST_SERIALIZATION_NVP(is_leaf);
        ar & BOOST_SERIALIZATION_NVP(n_samples);
        ar & BOOST_SERIALIZATION_NVP(depth);
        ar & BOOST_SERIALIZATION_NVP(params);
        ar & BOOST_SERIALIZATION_NVP(splitter);
        ar & BOOST_SERIALIZATION_NVP(node_dist);
        if (!is_leaf)
        {
          ar & BOOST_SERIALIZATION_NVP(left);
          ar & BOOST_SERIALIZATION_NVP(right);
        }
    }
#endif

    void get_feature_usage (std::vector<std::size_t>& count) const
    {
      if (!is_leaf)
      {
        count[std::size_t(splitter.feature)] ++;
        left->get_feature_usage(count);
        right->get_feature_usage(count);
      }
    }
};

}
}

}} // namespace CGAL::internal::

#endif


#include "Fuser.h"

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

#include <boost/foreach.hpp>

#include "Found_NMS.h"

#include <time.h>

void Fuser::Load_Kernel(std::string input, const int in, const int out, float* &h_kernel, float* &d_kernel)
{
  std::string input_path;
  _nh.getParam(input, input_path);
  std::vector<std::vector<float> > vector_kernel = Populate_Weights(input_path);
  h_kernel = Columate_Matrix(vector_kernel);
  d_kernel = cublas_Set_Matrix(h_kernel, in, out);
}

void Fuser::Load_Bias(std::string input, const int out, float* &h_bias, float* &d_bias)
{
  std::string input_path;
  _nh.getParam(input, input_path);
  std::vector<float> vector_bias = Populate_Biases(input_path);
  h_bias = Vectorize(vector_bias);
  d_bias = cublas_Set_Vector(h_bias, out);
}

void Fuser::Load_Layer(std::string input, const int in, const int out, float* &h_kernel, float* &d_kernel, float* &h_bias, float* &d_bias)
{
  std::string bias_input, kernel_input;

  bias_input = input + "_bias";
  kernel_input = input + "_weight";

  Load_Kernel(kernel_input, in, out, h_kernel, d_kernel);
  Load_Bias(bias_input, out, h_bias, d_bias);
}

Fuser::Fuser()
{
/*
  Input(s): N/A
  Output(s): N/A
  Function: Construct the Fuser object
*/
  std::string input_path;

  // Populate the transform matrices to be used
  _nh.getParam("transforms", input_path);
  _Tr_velo_to_cam = Populate_Transform(input_path, "Tr_velo_to_cam: ");
  _P = Populate_Transform(input_path, "P2: ");
  _R = Populate_Transform(input_path, "R0_rect: ");

  // Populate the class layer in GPU memory
  Load_Layer("class", _layer1_size, _class_size, _h_class_kernel, _d_class_kernel, _h_class_bias, _d_class_bias);

  // Populate the length layer in GPU memory
  Load_Layer("length", _layer1_size, _length_size, _h_length_kernel, _d_length_kernel, _h_length_bias, _d_length_bias);

  // Populate the z layer in GPU memory
  Load_Layer("z", _layer1_size, _z_size, _h_z_kernel, _d_z_kernel, _h_z_bias, _d_z_bias);

  // Populate the hidden layer
  Load_Layer("layer", _input_size, _layer1_size, _h_layer_kernel, _d_layer_kernel, _h_layer_bias, _d_layer_bias);

  // Populate the rotation layer
  Load_Layer("rotation", _layer1_size, _rotation_size, _h_rotation_kernel, _d_rotation_kernel, _h_rotation_bias, _d_rotation_bias);

  // Create the CUBLAS handle used in the CUBLAS API
  cublasCreate(&_cublas_handle);

  // Preallocate space for the outputs of the MLP
  _d_input = Allocate_MLP(_input_size);
  _d_layer1_ouput = Allocate_MLP(_layer1_size);
  _d_class_output = Allocate_MLP(_class_size);
  _d_z_output = Allocate_MLP(_z_size);
  _d_length_output = Allocate_MLP(_length_size);
  _d_rotation_output = Allocate_MLP(_rotation_size);

  _h_input = (float*)malloc(_input_size * sizeof(float));
}

Fuser::~Fuser()
{
/*
  Input(s): N/A
  Output(s): N/A
  Function: Destroy the Fuser object
*/
  // Destroy the CUBLAS handle
  cublasDestroy(_cublas_handle);

  // Destroy GPU memory
  destroy_memory(_d_class_kernel);
  destroy_memory(_d_length_kernel);
  destroy_memory(_d_z_kernel);
  destroy_memory(_d_layer_kernel);
  destroy_memory(_d_class_bias);
  destroy_memory(_d_length_bias);
  destroy_memory(_d_z_bias);
  destroy_memory(_d_layer_bias);
  destroy_memory(_d_input);
  destroy_memory(_d_layer1_ouput);
  destroy_memory(_d_class_output);
  destroy_memory(_d_z_output);
  destroy_memory(_d_length_output);
  destroy_memory(_d_rotation_output);

  free(_h_input);
}

void Fuser::Publish_Cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
/*
  Input(s): Point Cloud colored according to clusters
  Output(s): N/A
  Function: Publish a point cloud in ROS
*/
  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg(*cloud, output);

  output.header.frame_id = "map";

  _cloud_pub.publish(output);
}

void Fuser::debug_pub(std::vector<Fuser::Sample> samples)
{
/*
  Input(s): Vector of potential samples found in the world
  Output(s): N/A
  Function: Combine all clusters from each sample in a single Point Cloud and publish it using ROS
*/
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

  int r, g, b;
  int N = 0;
  for (int i = 0; i < samples.size(); i++)
  {
    r = rand() % 255;
    g = rand() % 255;
    b = rand() % 255;

    for (int j = 0; j < samples[i].cluster.size(); j++)
    {
      pcl::PointXYZRGB point;
      point.x = samples[i].cluster[j].x;
      point.y = samples[i].cluster[j].y;
      point.z = samples[i].cluster[j].z;
      point.r = r;
      point.g = g;
      point.b = b;
      cloud->points.push_back(point);
      N++;
    }
  }
  cloud->height = 1;
  cloud->width = N;

  Publish_Cloud(cloud);
}

void Fuser::_publish_image(std::vector<detection_fusion::Detection3D> detections)
{
/*
  Input(s): Vector of detections from the output of the fusion algorithm
  Output(s): N/A
  Function: Publish an annotated image of the final fused detections from the algorithm
*/
  cv::Point p1, p2;
  cv::Scalar Blue(255, 0, 0);
  cv::Scalar Green(0, 255, 0);
  cv::Scalar Red(0, 0, 255);

  int x1, x2, y1, y2;

  for (int i = 0; i < detections.size(); i++)
  {
    x1 = detections[i].bbox.x1;
    x2 = detections[i].bbox.x2;
    y1 = detections[i].bbox.y1;
    y2 = detections[i].bbox.y2;

    p1.x = x1;
    p1.y = y1;

    p2.x = x2;
    p2.y = y2;

    if (detections[i].bbox.obj_class == 1) { cv::rectangle(_cv_ptr->image, p1, p2, Blue, 3); } //Car
    else if (detections[i].bbox.obj_class == 2) { cv::rectangle(_cv_ptr->image, p1, p2, Green, 3); } //Pedestrian
    else if (detections[i].bbox.obj_class == 3) { cv::rectangle(_cv_ptr->image, p1, p2, Red, 3); } //Cyclist
  }

  cv_bridge::CvImage img_bridge;
  img_bridge = cv_bridge::CvImage(_cv_ptr->header, _cv_ptr->encoding, _cv_ptr->image);

  sensor_msgs::Image ros_image;
  img_bridge.toImageMsg(ros_image);

  _image_pub.publish(ros_image);
}

void Fuser::incoming_data_callback(const detection_fusion::Detections_Cloud::ConstPtr& msg)
{
/*
  Input(s): Message recieved from the image only CNN ROS node
  Output(s): N/A
  Function: Perform the point cloud processing on the data collected from the world
*/
  clock_t tStart = clock();

  // To get the detections from the msg data
  detection_fusion::Image_Detections detections = msg->detections;

  std::string filename = msg->filename;

  // Get the image from the msg
  try
  {
    _cv_ptr = cv_bridge::toCvCopy(msg->image, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  if (detections.detections.size() != 0)
  {
    // To populate a point cloud with the msg data
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(msg->pcloud,pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2,*cloud);

    double t1 = (double)(clock() - tStart)/CLOCKS_PER_SEC;

    // Only reason about points that are roughly in the same space as the image
    cloud = _mask_cloud(cloud);
    double t2 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t1;

    // Segment out the ground plane
    cloud = _segment_ground_plane(cloud);
    double t3 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t2;

    // Transform the point cloud from LiDAR coordinates to camera coordinates
    cloud = _transform_coords(cloud);
    double t4 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t3;

    // Seperate the cloud into clusters
    std::vector<std::vector<pcl::PointXYZ> > clusters = _cluster_incoming_cloud(cloud);
    double t5 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);//- t4;

    // Combine clusters and image based detections
    std::vector<Sample> samples = _filter_clusters(clusters, detections);
    double t6 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t5;

    // If any clusters and image detections matched up
    if (samples.size() != 0)
    {
      // Perform feature extraction on the clusters
      samples = _extract_features(samples);
      double t7 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t6;

      // Run the features through the MLP
      samples = _classify(samples);
      double t8 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t7;

      // Adjust the confidence of the image detections based upon the output of the LiDAR MLP
      std::vector<detection_fusion::Detection3D> combined_detections = _combine_detections(samples);
      double t9 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t8;

      // Perform  2D NMS on the combined detections
      std::vector<detection_fusion::Detection3D> fusion_output = _nms(combined_detections);
      double t10 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);// - t9;
      double t11 = ((double)(clock() - tStart)/CLOCKS_PER_SEC);

      // Publish the clustered cloud
      debug_pub(samples);

      // Publish the final annotated image
      _publish_image(fusion_output);

      // Publish the final output msg of the fusion algorithm of 3D detections
      if (fusion_output.size() != 0)
      {
        detection_fusion::Fusion output_msg;
        output_msg.detections = fusion_output;
        output_msg.filename = filename;
        output_msg.image_detections = detections;

        _fusion_pub.publish(output_msg);
      }
      //printf("t1=%.2f, t2=%.2f, t3=%.2f, t4=%.2f, t5=%.2f, t6=%.2f, t7=%.2f, t8=%.2f, t9=%.2f, t10=%.2f, t11=%.2f\n", t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11);
      //printf("t1=%.4f, t2=%.4f, t3=%.4f, t4=%.4f, t5=%.4f, t6=%.4f, t7=%.4f, t8=%.4f, t9=%.4f, t10=%.4f, t11=%.3f\n\n\n", t1, t2-t1, t3-t2, t4-t3, t5-t4, t6-t5, t7-t6, t8-t7, t9-t8, t10-t9, t11);
    }
  }
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Fuser::_mask_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
{
/*
  Input(s): Point cloud
  Output(s): Point cloud
  Function: Apply a mask to the point cloud that removes points roughly not in the cameras view
*/
  pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);
  float x, y, z, theta;
  const float theta_thresh = 3.14 / 4;
  const float theta_middle = 0;
  for (size_t i = 0; i < input->points.size(); i++)
  {
    x = input->points[i].x;
    y = input->points[i].y;
    z = input->points[i].z;
    theta = atan2(y, x);        //IF IN VELO COORDINATES
    //theta = atan2(x, z);          //IF IN CAMERA COORDINATES
    if ((theta >= theta_middle - theta_thresh) && (theta <= theta_middle + theta_thresh))
    {
      output->points.push_back(input->points[i]);
    }
  }
  output->width = output->points.size ();
  output->height = 1;
  output->is_dense = true;

  return output;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Fuser::_segment_ground_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
{
/*
  Input(s): Point cloud
  Output(s): Point cloud
  Function: Segment out the ground plane based on a planar RANSAC model

  http://pointclouds.org/documentation/tutorials/planar_segmentation.php
*/
  pcl::PointCloud<pcl::PointXYZ>::Ptr output (new pcl::PointCloud<pcl::PointXYZ>);

  // Do the plane fit
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.2);

  seg.setInputCloud (input);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
  }

  // Get all points not in the plane
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud (input);
  extract.setIndices (inliers);
  extract.setNegative (true);
  extract.filter (*output);

  return output;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Fuser::_transform_coords(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
{
/*
  Input(s): Point cloud
  Output(s): Point cloud
  Function: Transform the point cloud from LiDAR coordinate frame to camera coordinate frame
*/
  Eigen::VectorXd temp(4);

  for (size_t i = 0; i < input->points.size(); i++)
  {
    temp(0) = input->points[i].x;
    temp(1) = input->points[i].y;
    temp(2) = input->points[i].z;
    temp(3) = 1;

    temp = _Tr_velo_to_cam * temp;

    input->points[i].x = temp(0);
    input->points[i].y = temp(1);
    input->points[i].z = temp(2);
  }

  return input;
}

std::vector<std::vector<pcl::PointXYZ> > Fuser::_cluster_incoming_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input)
{
/*
  Input(s): Point cloud
  Output(s): Vector of clusters which are vectors of points
  Function: Seperate the cloud into clusters based upon Euclidean distance

  http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php
*/
  std::vector<std::vector<pcl::PointXYZ> > output;

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (input);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.25); // 25cm
  ec.setMinClusterSize (5);
  //ec.setMinClusterSize (25);
  //ec.setMaxClusterSize (2500);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (input);
  ec.extract (cluster_indices);

  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    std::vector<pcl::PointXYZ> temp_cluster;
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
      temp_cluster.push_back(input->points[*pit]);
    }
    output.push_back(temp_cluster);
  }

  return output;
}

void Fuser::Project_Cluster(float x_mean, float y_mean, float z_mean, int &u_centroid, int &v_centroid)
{
/*
  Input(s): The x, y, and z, values of the cluster centroid, the pixel space locations for the centroid
  Output(s): N/A
  Function: Find where the cluster would appear in image space
*/
  Eigen::VectorXd input(4);
  Eigen::VectorXd output(3);

  // This is for the shift between camera 0 and camera 2
  input(0) = x_mean - .06;
  input(1) = y_mean + .0003;
  input(2) = z_mean - .0025;
  input(3) = 1.;

  output = _P * input;
  //output = _P * _R * input;
  u_centroid = (int)(output(0) / output(2));
  v_centroid = (int)(output(1) / output(2));
}

std::vector<Fuser::Sample> Fuser::_filter_clusters(std::vector<std::vector<pcl::PointXYZ> > clusters, detection_fusion::Image_Detections detections)
{
/*
  Input(s): Point clusters, image detections
  Output(s): Combined detections of clusters that are close to the image detections
  Function: To match up possible image detections with point clusters based upon image space distance
*/
  std::vector<Sample> output;
  float x_mean, y_mean, z_mean;
  int u_center, v_center;
  float x_center, y_center;
  float dist;

  int u_centroid, v_centroid;

  // Compute the centroid of the cluster
  for (int i = 0; i < clusters.size(); i++)
  {
    x_mean = y_mean = z_mean = 0;

    for (int j = 0; j < clusters[i].size(); j++)
    {
      x_mean += clusters[i][j].x;
      y_mean += clusters[i][j].y;
      z_mean += clusters[i][j].z;
    }
    x_mean /= clusters[i].size();
    y_mean /= clusters[i].size();
    z_mean /= clusters[i].size();

    // Find the pixel location of the cluster's centroid
    Project_Cluster(x_mean, y_mean, z_mean, u_centroid, v_centroid);

    for (int j = 0; j < detections.detections.size(); j++)
    {
      u_center = (detections.detections[j].x2 + detections.detections[j].x1) / 2;
      v_center = (detections.detections[j].y2 + detections.detections[j].y1) / 2;

      // Determine the distance of the cluster centroid the the bounding box centroid
      dist = sqrt(pow(u_center - u_centroid, 2) + pow(v_center - v_centroid, 2));
      if (dist < _radial_thresh)
      {
        Sample iter;
        iter.cluster = clusters[i];
        iter.detection = detections.detections[j];
        output.push_back(iter);
      }
    }
  }

  return output;
}

std::vector<Fuser::Sample> Fuser::_extract_features(std::vector<Fuser::Sample> input)
{
/*
  Input(s): Combined clusters and detections
  Output(s): Combined clusters and detections
  Function: Extract features from each of the clusters
*/
  float x_mean, y_mean, z_mean;
  float x_sqr, y_sqr, z_sqr;
  float x_std, y_std, z_std;
  float x_range, y_range, z_range;
  float x_y, y_x, x_z, z_x, y_z, z_y;

  float x_min, x_max, y_min, y_max, z_min, z_max;

  int N;

  std::vector<float> cluster_features;
  for (int i = 0; i < input.size(); i++)
  {
    N = input[i].cluster.size();
    x_mean = y_mean = z_mean = 0;
    x_sqr = y_sqr = z_sqr = 0;
    x_std = y_std = z_std = 0;
    x_range = y_range = z_range = 0;
    x_y = y_x = x_z = z_x = y_z = z_y = 0;
    x_min = y_min = z_min = 1000000;
    x_max = y_max = z_max = -1000000;

    for (int j = 0; j < N; j++)
    {
      // X Points
      x_mean += input[i].cluster[j].x;
      x_sqr += pow(input[i].cluster[j].x, 2);
      if (input[i].cluster[j].x < x_min)
      {
        x_min = input[i].cluster[j].x;
      }
      else if (input[i].cluster[j].x > x_max)
      {
        x_max = input[i].cluster[j].x;
      }

      // Y Points
      y_mean += input[i].cluster[j].y;
      y_sqr += pow(input[i].cluster[j].y, 2);
      if (input[i].cluster[j].y < y_min)
      {
        y_min = input[i].cluster[j].y;
      }
      else if (input[i].cluster[j].y > y_max)
      {
        y_max = input[i].cluster[j].y;
      }

      // Z Points
      z_mean += input[i].cluster[j].z;
      z_sqr += pow(input[i].cluster[j].z, 2);
      if (input[i].cluster[j].z < z_min)
      {
        z_min = input[i].cluster[j].z;
      }
      else if (input[i].cluster[j].z > z_max)
      {
        z_max = input[i].cluster[j].z;
      }
    }
    x_mean /= N;
    y_mean /= N;
    z_mean /= N;

    x_sqr /= N;
    y_sqr /= N;
    z_sqr /= N;

    x_std = x_sqr - pow(x_mean, 2);
    y_std = y_sqr - pow(y_mean, 2);
    z_std = z_sqr - pow(z_mean, 2);

    x_range = x_max - x_min;
    y_range = y_max - y_min;
    z_range = z_max - z_min;

    x_y = x_range / y_range;
    y_x = y_range / x_range;
    x_z = x_range / z_range;
    z_x = z_range / x_range;
    y_z = y_range / z_range;
    z_y = z_range / y_range;

    cluster_features.push_back(x_mean);
    cluster_features.push_back(y_mean);
    cluster_features.push_back(z_mean);

    cluster_features.push_back(x_std);
    cluster_features.push_back(y_std);
    cluster_features.push_back(z_std);

    cluster_features.push_back(x_range);
    cluster_features.push_back(y_range);
    cluster_features.push_back(z_range);

    cluster_features.push_back(x_y);
    cluster_features.push_back(y_x);
    cluster_features.push_back(x_z);
    cluster_features.push_back(z_x);
    cluster_features.push_back(y_z);
    cluster_features.push_back(z_y);

    input[i].feature = cluster_features;
    cluster_features.clear();
  }
  return input;
}

std::vector<Fuser::Sample> Fuser::_classify(std::vector<Fuser::Sample> input)
{
/*
  Input(s): Combined clusters and detections
  Output(s): Combined clusters and detections
  Function: Push each cluster feature vector through the MLP and populate the output
*/
  std::vector<float> temp;
  for (int i = 0; i < _input_size; i++)
  {
    temp.push_back((float)i);
  }

  for (int i = 0; i < input.size(); i++)
  {
    cublas_Set_Input(_h_input, input[i].feature, _d_input, _input_size);

    blas_layer(_d_input, _d_layer_kernel, _d_layer_bias, _d_layer1_ouput, _layer1_size, _input_size, _cublas_handle);
    RELU(_d_layer1_ouput, _layer1_size);

    blas_layer(_d_layer1_ouput, _d_class_kernel, _d_class_bias, _d_class_output, _class_size, _layer1_size, _cublas_handle);
    Softmax(_d_class_output, _class_size);

    blas_layer(_d_layer1_ouput, _d_length_kernel, _d_length_bias, _d_length_output, _length_size, _layer1_size, _cublas_handle);
    Sigmoid(_d_length_output, _length_size);

    blas_layer(_d_layer1_ouput, _d_z_kernel, _d_z_bias, _d_z_output, _z_size, _layer1_size, _cublas_handle);
    Sigmoid(_d_z_output, _z_size);

    blas_layer(_d_layer1_ouput, _d_rotation_kernel, _d_rotation_bias, _d_rotation_output, _rotation_size, _layer1_size, _cublas_handle);

    classification_output iter_output = populate_output(_d_class_output, _d_length_output, _d_z_output, _d_rotation_output);
    input[i].output = iter_output;
  }

  return input;
}

std::vector<detection_fusion::Detection3D> Fuser::_combine_detections(std::vector<Fuser::Sample> input)
{
/*
  Input(s): Combined clusters and detections
  Output(s): 3D detections
  Function: Filter out detections and clusters that do not agree and adjust confidence of those that do
*/
  std::vector<detection_fusion::Detection3D> output;

  detection_fusion::BBox2D camera_detection;
  classification_output lidar_detection;
  std::vector<float> feature;

  int camera_class, lidar_class;
  float camera_prob, lidar_prob;

  detection_fusion::Detection3D iter;

  for (int i = 0; i < input.size(); i++)
  {
    camera_detection = input[i].detection;
    lidar_detection = input[i].output;
    feature = input[i].feature;

    camera_class = camera_detection.obj_class;
    lidar_class = lidar_detection.obj_class;

    camera_prob = camera_detection.probability;
    lidar_prob = lidar_detection.probability;

    if (camera_class == lidar_class)
    {
      iter.bbox = camera_detection;
      iter.x = feature[0];
      iter.y = feature[1];
      iter.z = lidar_detection.z * _scale;
      iter.w = feature[6];
      iter.h = feature[7];
      iter.l = lidar_detection.l * _scale;
      iter.a = lidar_detection.a * _rotation_scale;

      iter.bbox.probability += .5;
      if (iter.bbox.probability > 1.) iter.bbox.probability = 1.0;

      output.push_back(iter);
    }
    else
    { continue; }
  }

  return output;
}

std::vector<detection_fusion::Detection3D> Fuser::_nms(std::vector<detection_fusion::Detection3D> temp_input)
{
/*
  Input(s): 3D detections
  Output(s): Final 3D detections
  Function: Convert box forms to run the NMS algorithm on the detections
*/
  std::vector<detection_fusion::Detection3D> output;
  for (int i = 0; i < temp_input.size(); i ++)
  {
    if (temp_input[i].bbox.probability > .6)
    {
      output.push_back(temp_input[i]);
    }
  }

  std::vector<std::vector<float> > temp_boxes;
  std::vector<float> temp;
  float x1, x2, y1, y2;
  for (int i = 0; i < output.size(); i++)
  {
    x1 = (float)output[i].bbox.x1;
    x2 = (float)output[i].bbox.x2;
    y1 = (float)output[i].bbox.y1;
    y2 = (float)output[i].bbox.y2;

    temp.push_back(x1);
    temp.push_back(y1);
    temp.push_back(x2);
    temp.push_back(y2);
    temp_boxes.push_back(temp);
    temp.clear();
  }
  std::vector<cv::Rect> reducedRectangle = nms(temp_boxes, .3);

  int nms_x1, nms_x2, nms_y1, nms_y2, box_x1, box_x2, box_y1, box_y2;
  std::vector<detection_fusion::Detection3D> real_output;
  for (int i = 0; i < reducedRectangle.size(); i++)
  {
    for (int j = 0; j < output.size(); j++)
    {
      nms_x1 = (int)reducedRectangle[i].x;
      nms_x2 = (int)reducedRectangle[i].width + nms_x1;
      nms_y1 = (int)reducedRectangle[i].y;
      nms_y2 = (int)reducedRectangle[i].height + nms_y1;
      box_x1 = output[j].bbox.x1;
      box_x2 = output[j].bbox.x2;
      box_y1 = output[j].bbox.y1;
      box_y2 = output[j].bbox.y2;
      if (box_x1 == nms_x1 && box_y1 == nms_y1 && box_x2 == nms_x2 && box_y2 == nms_y2)
      {
        real_output.push_back(output[j]);
      }
    }
  }

  return real_output;
}


//
// Copyright (c) 2013-2017 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BEAST_WEBSOCKET_DETAIL_MASK_HPP
#define BEAST_WEBSOCKET_DETAIL_MASK_HPP

#include <boost/asio/buffer.hpp>
#include <array>
#include <climits>
#include <cstdint>
#include <random>
#include <type_traits>

namespace beast {
namespace websocket {
namespace detail {

// Pseudo-random source of mask keys
//
template<class Generator>
class maskgen_t
{
    Generator g_;

public:
    using result_type =
        typename Generator::result_type;

    maskgen_t();

    result_type
    operator()() noexcept;

    void
    rekey();
};

template<class Generator>
maskgen_t<Generator>::maskgen_t()
{
    rekey();
}

template<class Generator>
auto
maskgen_t<Generator>::operator()() noexcept ->
    result_type
{
    for(;;)
        if(auto key = g_())
            return key;
}

template<class _>
void
maskgen_t<_>::rekey()
{
    std::random_device rng;
#if 0
    std::array<std::uint32_t, 32> e;
    for(auto& i : e)
        i = rng();
    // VFALCO This constructor causes
    //        address sanitizer to fail, no idea why.
    std::seed_seq ss(e.begin(), e.end());
    g_.seed(ss);
#else
    g_.seed(rng());
#endif
}

// VFALCO NOTE This generator has 5KB of state!
//using maskgen = maskgen_t<std::mt19937>;
using maskgen = maskgen_t<std::minstd_rand>;

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

using prepared_key =
    std::conditional<sizeof(void*) == 8,
        std::uint64_t, std::uint32_t>::type;

inline
void
prepare_key(std::uint32_t& prepared, std::uint32_t key)
{
    prepared = key;
}

inline
void
prepare_key(std::uint64_t& prepared, std::uint32_t key)
{
    prepared =
        (static_cast<std::uint64_t>(key) << 32) | key;
}

template<class T>
inline
typename std::enable_if<std::is_integral<T>::value, T>::type
ror(T t, unsigned n = 1)
{
    auto constexpr bits =
        static_cast<unsigned>(
            sizeof(T) * CHAR_BIT);
    n &= bits-1;
    return static_cast<T>((t << (bits - n)) | (
        static_cast<typename std::make_unsigned<T>::type>(t) >> n));
}

// 32-bit optimized
//
template<class = void>
void
mask_inplace_fast(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    if(n >= sizeof(key))
    {
        // Bring p to 4-byte alignment
        auto const i = reinterpret_cast<
            std::uintptr_t>(p) & (sizeof(key)-1);
        switch(i)
        {
        case 1: p[2] ^= static_cast<std::uint8_t>(key >> 16);
        case 2: p[1] ^= static_cast<std::uint8_t>(key >> 8);
        case 3: p[0] ^= static_cast<std::uint8_t>(key);
        {
            auto const d = static_cast<
                unsigned>(sizeof(key) - i);
            key = ror(key, 8*d);
            n -= d;
            p += d;
        }
        default:
            break;
        }
    }

    // Mask 4 bytes at a time
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint32_t*>(p) ^= key;
        p += sizeof(key);
    }

    // Leftovers
    n &= sizeof(key)-1;
    switch(n)
    {
    case 3: p[2] ^= static_cast<std::uint8_t>(key >> 16);
    case 2: p[1] ^= static_cast<std::uint8_t>(key >> 8);
    case 1: p[0] ^= static_cast<std::uint8_t>(key);
        key = ror(key, static_cast<unsigned>(8*n));
    default:
        break;
    }
}

// 64-bit optimized
//
template<class = void>
void
mask_inplace_fast(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    if(n >= sizeof(key))
    {
        // Bring p to 8-byte alignment
        auto const i = reinterpret_cast<
            std::uintptr_t>(p) & (sizeof(key)-1);
        switch(i)
        {
        case 1: p[6] ^= static_cast<std::uint8_t>(key >> 48);
        case 2: p[5] ^= static_cast<std::uint8_t>(key >> 40);
        case 3: p[4] ^= static_cast<std::uint8_t>(key >> 32);
        case 4: p[3] ^= static_cast<std::uint8_t>(key >> 24);
        case 5: p[2] ^= static_cast<std::uint8_t>(key >> 16);
        case 6: p[1] ^= static_cast<std::uint8_t>(key >> 8);
        case 7: p[0] ^= static_cast<std::uint8_t>(key);
        {
            auto const d = static_cast<
                unsigned>(sizeof(key) - i);
            key = ror(key, 8*d);
            n -= d;
            p += d;
        }
        default:
            break;
        }
    }

    // Mask 8 bytes at a time
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint64_t*>(p) ^= key;
        p += sizeof(key);
    }

    // Leftovers
    n &= sizeof(key)-1;
    switch(n)
    {
    case 7: p[6] ^= static_cast<std::uint8_t>(key >> 48);
    case 6: p[5] ^= static_cast<std::uint8_t>(key >> 40);
    case 5: p[4] ^= static_cast<std::uint8_t>(key >> 32);
    case 4: p[3] ^= static_cast<std::uint8_t>(key >> 24);
    case 3: p[2] ^= static_cast<std::uint8_t>(key >> 16);
    case 2: p[1] ^= static_cast<std::uint8_t>(key >> 8);
    case 1: p[0] ^= static_cast<std::uint8_t>(key);
        key = ror(key, static_cast<unsigned>(8*n));
    default:
        break;
    }
}

inline
void
mask_inplace(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    mask_inplace_fast(b, key);
}

inline
void
mask_inplace(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    mask_inplace_fast(b, key);
}

// Apply mask in place
//
template<class MutableBuffers, class KeyType>
void
mask_inplace(
    MutableBuffers const& bs, KeyType& key)
{
    for(auto const& b : bs)
        mask_inplace(b, key);
}

} // detail
} // websocket
} // beast

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ACCUMULATORS_BLOCK_HPP
#define CRYPTO3_ACCUMULATORS_BLOCK_HPP

#include <boost/container/static_vector.hpp>

#include <boost/parameter/value_type.hpp>

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>

#include <nil/crypto3/detail/make_array.hpp>
#include <nil/crypto3/detail/digest.hpp>
#include <nil/crypto3/detail/inject.hpp>

#include <nil/crypto3/block/accumulators/bits_count.hpp>

#include <nil/crypto3/block/accumulators/parameters/cipher.hpp>
#include <nil/crypto3/block/accumulators/parameters/bits.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <nil/crypto3/block/detail/cipher_modes.hpp>

#include <nil/crypto3/block/cipher.hpp>

namespace nil {
    namespace crypto3 {
        namespace accumulators {
            namespace impl {
                template<typename Mode>
                struct block_impl : boost::accumulators::accumulator_base {
                protected:
                    typedef Mode mode_type;
                    typedef typename Mode::cipher_type cipher_type;
                    typedef typename Mode::padding_type padding_type;

                    typedef typename mode_type::endian_type endian_type;

                    constexpr static const std::size_t word_bits = mode_type::word_bits;
                    typedef typename mode_type::word_type word_type;

                    constexpr static const std::size_t block_bits = mode_type::block_bits;
                    constexpr static const std::size_t block_words = mode_type::block_words;
                    typedef typename mode_type::block_type block_type;

                    constexpr static const std::size_t value_bits = sizeof(typename block_type::value_type) * CHAR_BIT;
                    constexpr static const std::size_t block_values = block_bits / value_bits;

                    typedef ::nil::crypto3::detail::injector<endian_type, value_bits, block_values, block_bits>
                        injector_type;

                public:
                    typedef digest<block_bits> result_type;

                    template<typename Args>
                    block_impl(const Args &args) :
                        total_seen(0), filled(false), mode(args[boost::accumulators::sample]) {
                    }

                    template<typename ArgumentPack>
                    inline void operator()(const ArgumentPack &args) {
                        resolve_type(args[boost::accumulators::sample],
                                     args[::nil::crypto3::accumulators::bits | std::size_t()]);
                    }

                    inline result_type result(boost::accumulators::dont_care) const {
                        using namespace ::nil::crypto3::detail;

                        result_type res = dgst;

                        block_type processed_block = mode.end_message(cache, total_seen);

                        res = ::nil::crypto3::resize<block_bits>(res, res.size() + block_values);

                        pack<endian_type, endian_type, value_bits, octet_bits>(
                            processed_block.begin(), processed_block.end(), res.end() - block_values);

                        return res;
                    }

                protected:
                    inline void resolve_type(const block_type &value, std::size_t bits) {
                        process(value, bits == 0 ? block_bits : bits);
                    }

                    inline void resolve_type(const word_type &value, std::size_t bits) {
                        process(value, bits == 0 ? word_bits : bits);
                    }

                    inline void process_block() {
                        using namespace ::nil::crypto3::detail;

                        block_type processed_block;
                        if (dgst.empty()) {
                            processed_block = mode.begin_message(cache, total_seen);
                        } else {
                            processed_block = mode.process_block(cache, total_seen);
                        }

                        dgst = ::nil::crypto3::resize<block_bits>(dgst, dgst.size() + block_values);

                        pack<endian_type, endian_type, value_bits, octet_bits>(
                            processed_block.begin(), processed_block.end(), dgst.end() - block_values);

                        filled = false;
                    }

                    inline void process(const block_type &value, std::size_t value_seen) {
                        using namespace ::nil::crypto3::detail;

                        if (filled) {
                            process_block();
                        }

                        std::size_t cached_bits = total_seen % block_bits;

                        if (cached_bits != 0) {
                            // If there are already any bits in the cache

                            std::size_t needed_to_fill_bits = block_bits - cached_bits;
                            std::size_t new_bits_to_append =
                                (needed_to_fill_bits > value_seen) ? value_seen : needed_to_fill_bits;

                            injector_type::inject(value, new_bits_to_append, cache, cached_bits);
                            total_seen += new_bits_to_append;

                            if (cached_bits == block_bits) {
                                // If there are enough bits in the incoming value to fill the block
                                filled = true;

                                if (value_seen > new_bits_to_append) {

                                    process_block();

                                    // If there are some remaining bits in the incoming value - put them into the cache,
                                    // which is now empty

                                    cached_bits = 0;

                                    injector_type::inject(value, value_seen - new_bits_to_append, cache, cached_bits,
                                                          new_bits_to_append);

                                    total_seen += value_seen - new_bits_to_append;
                                }
                            }

                        } else {

                            total_seen += value_seen;

                            // If there are no bits in the cache
                            if (value_seen == block_bits) {
                                // The incoming value is a full block
                                filled = true;

                                std::move(value.begin(), value.end(), cache.begin());

                            } else {
                                // The incoming value is not a full block
                                std::move(value.begin(),
                                          value.begin() + value_seen / word_bits + (value_seen % word_bits ? 1 : 0),
                                          cache.begin());
                            }
                        }
                    }

                    inline void process(const word_type &value, std::size_t value_seen) {
                        using namespace ::nil::crypto3::detail;

                        if (filled) {
                            process_block();
                        }

                        std::size_t cached_bits = total_seen % block_bits;

                        if (cached_bits % word_bits != 0) {
                            std::size_t needed_to_fill_bits = block_bits - cached_bits;
                            std::size_t new_bits_to_append =
                                (needed_to_fill_bits > value_seen) ? value_seen : needed_to_fill_bits;

                            injector_type::inject(value, new_bits_to_append, cache, cached_bits);
                            total_seen += new_bits_to_append;

                            if (cached_bits == block_bits) {
                                // If there are enough bits in the incoming value to fill the block

                                filled = true;

                                if (value_seen > new_bits_to_append) {

                                    process_block();

                                    // If there are some remaining bits in the incoming value - put them into the cache,
                                    // which is now empty
                                    cached_bits = 0;

                                    injector_type::inject(value, value_seen - new_bits_to_append, cache, cached_bits,
                                                          new_bits_to_append);

                                    total_seen += value_seen - new_bits_to_append;
                                }
                            }

                        } else {
                            cache[cached_bits / word_bits] = value;

                            total_seen += value_seen;
                        }
                    }

                    mode_type mode;

                    bool filled;
                    std::size_t total_seen;
                    block_type cache;
                    result_type dgst;
                };
            }    // namespace impl

            namespace tag {
                template<typename Mode>
                struct block : boost::accumulators::depends_on<bits_count> {
                    typedef Mode mode_type;

                    /// INTERNAL ONLY
                    ///

                    typedef boost::mpl::always<accumulators::impl::block_impl<mode_type>> impl;
                };
            }    // namespace tag

            namespace extract {
                template<typename Mode, typename AccumulatorSet>
                typename boost::mpl::apply<AccumulatorSet, tag::block<Mode>>::type::result_type
                    block(const AccumulatorSet &acc) {
                    return boost::accumulators::extract_result<tag::block<Mode>>(acc);
                }
            }    // namespace extract
        }        // namespace accumulators
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ACCUMULATORS_BLOCK_HPP


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include <utility> // std::make_pair
#include "../../src/idle_box_multimap.hpp"
#include "spatial_test_types.hpp"

using namespace spatial;

BOOST_AUTO_TEST_CASE( test_idle_box_map_constructors )
{
  idle_box_multimap<2, int2, int> idle_boxes;
  idle_box_multimap<0, int2, int> runtime_idle_boxes;
}

BOOST_AUTO_TEST_CASE( test_idle_box_map_copy_assignment )
{
  idle_box_multimap<2, int2, int> idle_boxes;
  idle_boxes.insert(std::make_pair(zeros, 0));
  idle_boxes.insert(std::make_pair(ones, 1));
  idle_boxes.insert(std::make_pair(twos, 2));
  idle_box_multimap<2, int2, int> copy(idle_boxes);
  BOOST_CHECK_EQUAL(idle_boxes.size(), copy.size());
  BOOST_CHECK(*idle_boxes.begin() == *copy.begin());
  idle_boxes = copy;
  BOOST_CHECK_EQUAL(idle_boxes.size(), copy.size());
  BOOST_CHECK(*idle_boxes.begin() == *copy.begin());
}

BOOST_AUTO_TEST_CASE( test_zero_idle_box_map_copy_assignment )
{
  idle_box_multimap<0, int2, int> idle_boxes;
  idle_boxes.insert(std::make_pair(zeros, 0));
  idle_boxes.insert(std::make_pair(ones, 1));
  idle_boxes.insert(std::make_pair(twos, 2));
  idle_box_multimap<0, int2, int> copy(idle_boxes);
  BOOST_CHECK_EQUAL(idle_boxes.size(), copy.size());
  BOOST_CHECK(*idle_boxes.begin() == *copy.begin());
  idle_boxes = copy;
  BOOST_CHECK_EQUAL(idle_boxes.size(), copy.size());
  BOOST_CHECK(*idle_boxes.begin() == *copy.begin());
}


// TODO Rewrite these tests once the 'MultiReaderPointSource' can work with
// in-memory files

// #include "catch.hpp"

// #include "datastructures/SparseGrid.h"
// #include "io/BinaryPersistence.h"
// #include "io/MemoryPersistence.h"
// #include "process/Tiler.h"
// #include "tiling/OctreeAlgorithms.h"
// #include <debug/ProgressReporter.h>

// #include <boost/algorithm/string/predicate.hpp>
// #include <boost/format.hpp>
// #include <boost/functional/hash.hpp>
// #include <boost/scope_exit.hpp>
// #include <random>
// #include <unordered_set>

// namespace std {
// template<>
// struct hash<Vector3<double>>
// {
//   typedef Vector3<double> argument_type;
//   typedef std::size_t result_type;

//   result_type operator()(Vector3<double> const& s) const noexcept
//   {
//     size_t seed = std::hash<double>{}(s.x);
//     boost::hash_combine(seed, std::hash<double>{}(s.y));
//     boost::hash_combine(seed, std::hash<double>{}(s.z));
//     return seed;
//   }
// };
// } // namespace std

// static PointBuffer
// create_random_dataset(size_t num_points)
// {
//   std::default_random_engine rnd{ static_cast<uint32_t>(time(nullptr)) };
//   std::uniform_real_distribution<float> dist;

//   // Create num_points random points in [0;1]³
//   std::vector<Vector3<double>> positions;
//   positions.reserve(num_points);
//   std::generate_n(std::back_inserter(positions), num_points, [&]() ->
//   Vector3<double> {
//     return { dist(rnd), dist(rnd), dist(rnd) };
//   });

//   PointBuffer point_buffer{ num_points, std::move(positions) };
//   return point_buffer;
// }

// // static bool
// // points_obey_minimum_distance(const PointBuffer& points, const AABB&
// bounds,
// // float min_distance)
// // {
// //   SparseGrid grid{ bounds, min_distance };

// //   for (auto& pos : points.positions()) {
// //     if (!grid.add(pos))
// //       return false;
// //   }
// //   return true;
// // }

// static bool
// points_are_contained_in_bounds(const PointBuffer& points,
//                                const AABB& bounds,
//                                Vector3<double>* fail_point = nullptr)
// {
//   for (auto& pos : points.positions()) {
//     if (!bounds.isInside(pos)) {
//       if (fail_point) {
//         *fail_point = pos;
//       }
//       return false;
//     }
//   }
//   return true;
// }

// TEST_CASE("Tiler works", "[Tiler]")
// {
//   constexpr size_t NumPoints = 1'000'000;
//   constexpr size_t MaxPointsPerNode = 20'000;

//   const auto dataset = create_random_dataset(NumPoints);
//   AABB bounds{ { 0, 0, 0 }, { 1, 1, 1 } };
//   auto spacing_at_root = 0.1f;
//   const auto sampling_strategy =
//   make_sampling_strategy<RandomSortedGridSampling>(MaxPointsPerNode);
//   PointsPersistence persistence{ MemoryPersistence{} };

//   TilerMetaParameters tiler_meta_parameters;
//   tiler_meta_parameters.spacing_at_root = spacing_at_root;
//   tiler_meta_parameters.max_depth = 40;
//   tiler_meta_parameters.max_points_per_node = MaxPointsPerNode;
//   tiler_meta_parameters.internal_cache_size = 100'000; // Use small internal
//   cache size to guarantee
//                                                        // out-of-core
//                                                        processing

//   Tiler writer{ bounds, tiler_meta_parameters, sampling_strategy, nullptr,
//   persistence, "." };

//   writer.cache(dataset);
//   writer.index();
//   writer.close();

//   // Two properties have to hold:
//   //  1) The converter processes and stores ALL points (except when the tree
//   is
//   //  too shallow due to max_depth) 2) All points of each node must be
//   contained
//   //  within the bounds of their respective node

//   // There is also the spacing property (min distance), however this depends
//   on
//   // the sampling strategy used. The naive version of my algorithm uses the
//   // 'random sorted grid' approach, which can result in arbitrarily close
//   // points. Once other sampling strategies are implemented, there will be
//   tests
//   // for their correctness!

//   // For fast matching of processed points with existing points we use
//   // unordered_set
//   std::unordered_set<Vector3<double>> expected_points;
//   expected_points.reserve(NumPoints);
//   for (auto& pos : dataset.positions()) {
//     expected_points.insert(pos);
//   }

//   const auto processed_points =
//   persistence.get<MemoryPersistence>().get_points(); for (auto& kv :
//   processed_points) {
//     const auto& node_name = kv.first;
//     const auto& points = kv.second;

//     const auto node_key = from_string<21>(node_name);
//     const auto node_level = node_name.size() - 1; // Node level, root = -1
//     const auto node_bounds = get_bounds_from_morton_index(node_key, bounds,
//     node_level);
//     // const auto min_distance_at_node = spacing_at_root / std::pow(2,
//     // node_level + 1); REQUIRE(points_obey_minimum_distance(points,
//     // node_bounds, min_distance_at_node));
//     REQUIRE(points_are_contained_in_bounds(points, node_bounds));

//     for (auto& pos : points.positions()) {
//       const auto point_iter = expected_points.find(pos);
//       const auto point_exists_in_dataset = (expected_points.find(pos) !=
//       expected_points.end()); REQUIRE(point_exists_in_dataset);
//       expected_points.erase(point_iter); // Make sure we don't allow
//       duplicate points!
//     }
//   }

//   const auto num_processed_points = (dataset.count() -
//   expected_points.size()); REQUIRE(num_processed_points == NumPoints);
// }

// TEST_CASE("Tiler with deep tree works", "[Tiler]")
// {
//   constexpr size_t NumPoints = 1'000'000;
//   constexpr size_t MaxPointsPerNode = 20'000;

//   const auto dataset = create_random_dataset(NumPoints);
//   // Very large bounds will force the octree to become deeper than the 21
//   levels
//   // that MortonIndex64 can support. This should trigger the re-indexing
//   // mechanism, which is what we want to test with this test case! What we
//   don't
//   // want is dropped points, everything should still be indexed correctly!
//   AABB bounds{ { 0, 0, 0 }, { 1 << 20, 1 << 20, 1 << 20 } };
//   auto spacing_at_root = (1 << 20) / 10.f;
//   const auto sampling_strategy =
//   make_sampling_strategy<RandomSortedGridSampling>(MaxPointsPerNode);
//   PointsPersistence persistence{ MemoryPersistence{} };

//   TilerMetaParameters tiler_meta_parameters;
//   tiler_meta_parameters.spacing_at_root = spacing_at_root;
//   tiler_meta_parameters.max_depth = 40;
//   tiler_meta_parameters.max_points_per_node = MaxPointsPerNode;
//   tiler_meta_parameters.internal_cache_size = 100'000; // Use small internal
//   cache size to guarantee
//                                                        // out-of-core
//                                                        processing
//   Tiler writer{ bounds, tiler_meta_parameters, sampling_strategy, nullptr,
//   persistence, "." };

//   writer.cache(dataset);
//   writer.index();
//   writer.close();

//   // Two properties have to hold:
//   //  1) The converter processes and stores ALL points (except when the tree
//   is
//   //  too shallow due to max_depth) 2) All points of each node must be
//   contained
//   //  within the bounds of their respective node

//   // There is also the spacing property (min distance), however this depends
//   on
//   // the sampling strategy used. The naive version of my algorithm uses the
//   // 'random sorted grid' approach, which can result in arbitrarily close
//   // points. Once other sampling strategies are implemented, there will be
//   tests
//   // for their correctness!

//   // For fast matching of processed points with existing points we use
//   // unordered_set
//   std::unordered_set<Vector3<double>> expected_points;
//   expected_points.reserve(NumPoints);
//   for (auto& pos : dataset.positions()) {
//     expected_points.insert(pos);
//   }

//   const auto processed_points =
//   persistence.get<MemoryPersistence>().get_points(); for (auto& kv :
//   processed_points) {
//     const auto& node_name = kv.first;
//     const auto& points = kv.second;

//     const auto node_key = from_string<21>(node_name);
//     const auto node_level = node_name.size() - 1; // Node level, root = -1
//     const auto node_bounds = get_bounds_from_morton_index(node_key, bounds,
//     node_level);
//     // const auto min_distance_at_node = spacing_at_root / std::pow(2,
//     // node_level + 1); REQUIRE(points_obey_minimum_distance(points,
//     // node_bounds, min_distance_at_node));
//     REQUIRE(points_are_contained_in_bounds(points, node_bounds));

//     for (auto& pos : points.positions()) {
//       const auto point_iter = expected_points.find(pos);
//       const auto point_exists_in_dataset = (expected_points.find(pos) !=
//       expected_points.end()); REQUIRE(point_exists_in_dataset);
//       expected_points.erase(point_iter); // Make sure we don't allow
//       duplicate points!
//     }
//   }

//   const auto num_processed_points = (dataset.count() -
//   expected_points.size()); REQUIRE(num_processed_points == NumPoints);
// }

// TEST_CASE("Tiler with GridCenter sampling", "[Tiler]")
// {
//   constexpr size_t NumPoints = 1'000'000;
//   constexpr size_t MaxPointsPerNode = 20'000;

//   const auto dataset = create_random_dataset(NumPoints);
//   AABB bounds{ { 0, 0, 0 }, { 1, 1, 1 } };
//   auto spacing_at_root = 0.1f;
//   const auto sampling_strategy =
//   make_sampling_strategy<GridCenterSampling>(MaxPointsPerNode);
//   PointsPersistence persistence{ MemoryPersistence{} };

//   TilerMetaParameters tiler_meta_parameters;
//   tiler_meta_parameters.spacing_at_root = spacing_at_root;
//   tiler_meta_parameters.max_depth = 40;
//   tiler_meta_parameters.max_points_per_node = MaxPointsPerNode;
//   tiler_meta_parameters.internal_cache_size = 100'000; // Use small internal
//   cache size to guarantee
//                                                        // out-of-core
//                                                        processing
//   Tiler writer{ bounds, tiler_meta_parameters, sampling_strategy, nullptr,
//   persistence, "." };

//   writer.cache(dataset);
//   writer.index();
//   writer.close();

//   // Two properties have to hold:
//   //  1) The converter processes and stores ALL points (except when the tree
//   is
//   //  too shallow due to max_depth) 2) All points of each node must be
//   contained
//   //  within the bounds of their respective node

//   // For fast matching of processed points with existing points we use
//   // unordered_set
//   std::unordered_set<Vector3<double>> expected_points;
//   expected_points.reserve(NumPoints);
//   for (auto& pos : dataset.positions()) {
//     expected_points.insert(pos);
//   }

//   const auto processed_points =
//   persistence.get<MemoryPersistence>().get_points(); for (auto& kv :
//   processed_points) {
//     const auto& node_name = kv.first;
//     const auto& points = kv.second;

//     const auto node_key = from_string<21>(node_name);
//     const auto node_level = node_name.size() - 1; // Node level, root = -1
//     const auto node_bounds = get_bounds_from_morton_index(node_key, bounds,
//     node_level);
//     // const auto min_distance_at_node = spacing_at_root / std::pow(2,
//     // node_level + 1); REQUIRE(points_obey_minimum_distance(points,
//     // node_bounds, min_distance_at_node));
//     REQUIRE(points_are_contained_in_bounds(points, node_bounds));

//     for (auto& pos : points.positions()) {
//       const auto point_iter = expected_points.find(pos);
//       const auto point_exists_in_dataset = (expected_points.find(pos) !=
//       expected_points.end()); REQUIRE(point_exists_in_dataset);
//       expected_points.erase(point_iter); // Make sure we don't allow
//       duplicate points!
//     }
//   }

//   const auto num_processed_points = (dataset.count() -
//   expected_points.size()); REQUIRE(num_processed_points == NumPoints);
// }

// TEST_CASE("Tiler with PossionDisk sampling", "[Tiler]")
// {
//   constexpr size_t NumPoints = 1'000'000;
//   constexpr size_t MaxPointsPerNode = 20'000;

//   const auto dataset = create_random_dataset(NumPoints);
//   AABB bounds{ { 0, 0, 0 }, { 1, 1, 1 } };
//   auto spacing_at_root = 0.05f;
//   const auto sampling_strategy =
//   make_sampling_strategy<PoissonDiskSampling>(MaxPointsPerNode);
//   PointsPersistence persistence{ MemoryPersistence{} };

//   TilerMetaParameters tiler_meta_parameters;
//   tiler_meta_parameters.spacing_at_root = spacing_at_root;
//   tiler_meta_parameters.max_depth = 40;
//   tiler_meta_parameters.max_points_per_node = MaxPointsPerNode;
//   tiler_meta_parameters.internal_cache_size = 100'000; // Use small internal
//   cache size to guarantee
//                                                        // out-of-core
//                                                        processing

//   Tiler writer{ bounds, tiler_meta_parameters, sampling_strategy, nullptr,
//   persistence, "." };

//   writer.cache(dataset);
//   writer.index();
//   writer.close();

//   // Two properties have to hold:
//   //  1) The converter processes and stores ALL points (except when the tree
//   is
//   //  too shallow due to max_depth) 2) All points of each node must be
//   contained
//   //  within the bounds of their respective node

//   // For fast matching of processed points with existing points we use
//   // unordered_set
//   std::unordered_set<Vector3<double>> expected_points;
//   expected_points.reserve(NumPoints);
//   for (auto& pos : dataset.positions()) {
//     expected_points.insert(pos);
//   }

//   const auto points_obey_minimum_distance =
//     [](const auto& points, const auto& bounds, double min_distance) -> bool {
//     SparseGrid grid{ bounds, static_cast<float>(min_distance) };
//     for (auto& point : points) {
//       if (!grid.add(point))
//         return false;
//     }
//     return true;
//   };

//   const auto is_leaf_node = [](const std::string& node_name, const auto&
//   all_nodes) {
//     for (auto& kv : all_nodes) {
//       const auto& other_node_name = kv.first;
//       if (other_node_name.size() != (node_name.size() + 1))
//         continue;
//       if (boost::starts_with(other_node_name, node_name))
//         return false;
//     }
//     return true;
//   };

//   const auto processed_points =
//   persistence.get<MemoryPersistence>().get_points(); for (auto& kv :
//   processed_points) {
//     const auto& node_name = kv.first;
//     const auto& points = kv.second;

//     const auto node_key = from_string<21>(node_name);
//     const auto node_level = static_cast<int32_t>(node_name.size()) - 2; //
//     Node level, root = -1 const auto node_bounds =
//     get_bounds_from_morton_index(node_key, bounds, node_level + 1); const
//     auto min_distance_at_node = spacing_at_root / std::pow(2, node_level +
//     1);

//     const auto is_leaf = is_leaf_node(node_name, processed_points);
//     if (!is_leaf) {
//       REQUIRE(points_obey_minimum_distance(points.positions(), node_bounds,
//       min_distance_at_node));
//     }
//     Vector3<double> fail_point;
//     const auto points_all_contained =
//       points_are_contained_in_bounds(points, node_bounds, &fail_point);
//     if (!points_all_contained) {
//       UNSCOPED_INFO("Position " << fail_point.x << " " << fail_point.y << " "
//       << fail_point.z
//                                 << " not contained in bounds!");
//     }
//     REQUIRE(points_all_contained);

//     for (auto& pos : points.positions()) {
//       const auto point_iter = expected_points.find(pos);
//       const auto point_exists_in_dataset = (expected_points.find(pos) !=
//       expected_points.end()); REQUIRE(point_exists_in_dataset);
//       expected_points.erase(point_iter); // Make sure we don't allow
//       duplicate points!
//     }
//   }

//   const auto num_processed_points = (dataset.count() -
//   expected_points.size()); REQUIRE(num_processed_points == NumPoints);
// }

// TEST_CASE("Tiler with BinaryPersistence writer", "[Tiler]")
// {
//   constexpr size_t NumPoints = 1'000'000;
//   constexpr size_t MaxPointsPerNode = 20'000;

//   const auto dataset = create_random_dataset(NumPoints);
//   AABB bounds{ { 0, 0, 0 }, { 1, 1, 1 } };
//   auto spacing_at_root = 0.1f;
//   const auto sampling_strategy =
//   make_sampling_strategy<RandomSortedGridSampling>(MaxPointsPerNode);
//   // MemoryPersistence persistence;
//   PointAttributes attributes;
//   attributes.insert(PointAttribute::Position);

//   const auto tmp_directory =
//     (boost::format("./tmp_%1%") %
//      (std::chrono::high_resolution_clock::now().time_since_epoch().count()))
//       .str();
//   if (!fs::exists(tmp_directory)) {
//     if (!fs::create_directories(tmp_directory)) {
//       FAIL("Could not create temporary output directory");
//     }
//   }

//   BOOST_SCOPE_EXIT(&tmp_directory)
//   {
//     std::error_code ec;
//     fs::remove_all(tmp_directory, ec);
//     if (ec) {
//       UNSCOPED_INFO("Could not remove temporary output directory");
//     }
//   }
//   BOOST_SCOPE_EXIT_END

//   PointsPersistence persistence{ BinaryPersistence{ tmp_directory, attributes
//   } };

//   TilerMetaParameters tiler_meta_parameters;
//   tiler_meta_parameters.spacing_at_root = spacing_at_root;
//   tiler_meta_parameters.max_depth = 40;
//   tiler_meta_parameters.max_points_per_node = MaxPointsPerNode;
//   tiler_meta_parameters.internal_cache_size = 100'000; // Use small internal
//   cache size to guarantee
//                                                        // out-of-core
//                                                        processing

//   Tiler writer{ bounds, tiler_meta_parameters, sampling_strategy, nullptr,
//   persistence, "." };

//   writer.cache(dataset);
//   writer.index();
//   writer.close();
// }


#include "Connection.h"
#include <boost/range/algorithm.hpp>

#include "Angle.h"
#include "Quaternion.h"
#include "Part.h"
#include "Vector.h"

void Connection::connectTo(Connection *targetCon)
{
	Quaternion targetQuat(targetCon->angles());

	targetQuat = targetQuat * Quaternion{ 0, 0, 1, 0 };

	Angle newAngle = angles();

	auto rotmat = newAngle.calcRotation(targetQuat);

	parent->rotate(rotmat);
	Vector mov = Vector::diff(origin(), targetCon->origin());
	parent->move(mov);

	neighbour = targetCon;
	targetCon->neighbour = this;
}

bool Connection::isConnected() const
{
	return neighbour != nullptr;
}

Connection::Connection(Entity&& entity)
{
	this->solids = std::move(entity.solids);
	this->keyvals = std::move(entity.keyvals);
	if (keyvals.count("classname") > 0)
	{
		std::string val = keyvals["classname"];

		if (val.length() > 12)
			connectstr = val.substr(12);
	}
}