#include <pa/stages/particle_tracer.hpp>

#include <algorithm>
#include <limits>

#include <boost/serialization/vector.hpp>
#include <boost/mpi.hpp>
#include <tbb/tbb.h>

#include <pa/math/integrators.hpp>

#undef min
#undef max

namespace pa
{
particle_tracer::particle_tracer(partitioner* partitioner) : partitioner_(partitioner)
{

}

void                         particle_tracer::set_local_vector_field    (std::optional<vector_field>*                local_vector_field    )
{
  local_vector_field_     = local_vector_field    ;
}
void                         particle_tracer::set_neighbor_vector_fields(std::array<std::optional<vector_field>, 6>* neighbor_vector_fields)
{
  neighbor_vector_fields_ = neighbor_vector_fields;
}
void                         particle_tracer::set_integrator            (const variant_integrator&                   integrator            )
{
  integrator_    = integrator   ;
}
void                         particle_tracer::set_step_size             (const scalar                                step_size             )
{
  step_size_     = step_size    ;
}

std::vector<integral_curves> particle_tracer::trace                     (std::vector<particle>                       particles             )
{
  std::vector<integral_curves> integral_curves;

  while (!check_completion(particles))
  {
                      load_balance_distribute (particles                             );
    auto round_info = compute_round_info      (particles, integral_curves            );
                      allocate                (           integral_curves, round_info);
                      initialize              (particles, integral_curves, round_info);
                      trace                   (particles, integral_curves, round_info);
                      load_balance_collect    (                            round_info);
                      out_of_bounds_distribute(particles,                  round_info);
  }

  prune (integral_curves);
  return integral_curves;
}

void                         particle_tracer::load_balance_distribute   (      std::vector<particle>& particles                                                                                )
{
  // Send/receive particle counts.
  auto neighbors                = partitioner_->neighbor_rank_info();
  auto neighbor_particle_counts = std::array<std::size_t, 6> {};
  neighbor_particle_counts.fill(std::numeric_limits<std::size_t>::max());

  std::vector<boost::mpi::request> requests;
  for (auto i = 0; i < neighbors.size(); ++i)
    if (neighbors[i])
      requests.push_back(partitioner_->communicator()->isend(neighbors[i]->rank, 0, particles.size()));
  for (auto i = 0; i < neighbors.size(); ++i)
    if (neighbors[i])
      partitioner_->communicator()->recv (neighbors[i]->rank, 0, neighbor_particle_counts[i]);

  for (auto& request : requests)
    request.wait();

  /// Compute workload deficit.

  // Compute average of neighbors with more workload than this process.
  auto contributions     = std::array<std::size_t, 6> {};
  auto contributor_count = 0;
  contributions.fill(0);
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    if (!neighbors[i] || neighbor_particle_counts[i] < particles.size())
      continue;
    contributions[i] = neighbor_particle_counts[i];
    contributor_count++;
  }
  auto average = (particles.size() + std::accumulate(contributions.begin(), contributions.end(), 0ull)) / (contributor_count + 1);

  // Compute average of neighbors with more workload than the average until all contributing neighbors are above the average (i.e. only this process below the average).
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    contributions.fill(0);
    contributor_count = 0;
    for (auto i = 0; i < neighbors.size(); ++i)
    {
      if (!neighbors[i] || neighbor_particle_counts[i] < average)
        continue;
      contributions[i] = neighbor_particle_counts[i];
      contributor_count++;
    }

    const auto next_average = (particles.size() + std::accumulate(contributions.begin(), contributions.end(), 0ull)) / (contributor_count + 1);
    if (average == next_average)
      break;
    average = next_average;
  }

  const auto total_deficit       = average - particles.size();
  const auto total_contributions = std::accumulate(contributions.begin(), contributions.end(), 0ull);

  auto deficits        = std::array<std::size_t, 6> {};
  auto maximum_surplus = std::array<std::size_t, 6> {};
  deficits       .fill(0);
  maximum_surplus.fill(0);
  for (auto i = 0; i < neighbors.size(); ++i)
    if (neighbors[i] && total_contributions != 0)
      deficits[i] = total_deficit * (contributions[i] / total_contributions);

  // Send / receive partial deficits (as partial maximum surplus).
  requests.clear();
  for (auto i = 0; i < neighbors.size(); ++i)
    if (neighbors[i])
      requests.push_back(partitioner_->communicator()->isend(neighbors[i]->rank, 10, deficits[i]));
  for (auto i = 0; i < neighbors.size(); ++i)
    if (neighbors[i])
      partitioner_->communicator()->recv (neighbors[i]->rank, 10, maximum_surplus[i]);

  for (auto& request : requests)
    request.wait();

  /// Compute workload surplus.

  // Compute average of neighbors with less workload than this process.
  contributions     = std::array<std::size_t, 6> {};
  contributor_count = 0;
  contributions.fill(0);
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    if (!neighbors[i] || neighbor_particle_counts[i] > particles.size())
      continue;
    contributions[i] = neighbor_particle_counts[i];
    contributor_count++;
  }
  average = (particles.size() + std::accumulate(contributions.begin(), contributions.end(), 0ull)) / (1 + contributor_count);

  // Compute average of neighbors with less workload than the average until all contributing neighbors are below the average (i.e. only this process above the average).
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    contributions.fill(0);
    contributor_count = 0;
    for (auto i = 0; i < neighbors.size(); ++i)
    {
      if (!neighbors[i] || neighbor_particle_counts[i] > average)
        continue;
      contributions[i] = neighbor_particle_counts[i];
      contributor_count++;
    }

    const auto next_average = (particles.size() + std::accumulate(contributions.begin(), contributions.end(), 0ull)) / (1 + contributor_count);
    if (average == next_average)
      break;
    average = next_average;
  }

  // Compute surplus particles.
  auto surplus_particles = std::array<std::vector<particle>, 6> {};
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    if (!neighbors[i] || neighbor_particle_counts[i] > average)
      continue;

    const auto particle_count = std::min(maximum_surplus[i], std::size_t(average - neighbor_particle_counts[i]));

    if (particle_count > particles.size())
      continue;

    surplus_particles[i].insert(surplus_particles[i].end(), particles.end() - particle_count, particles.end());
    particles.erase(particles.end() - particle_count, particles.end());

    tbb::parallel_for(std::size_t(0), surplus_particles[i].size(), std::size_t(1), [&](const std::size_t index)
    {
      surplus_particles[i][index].vector_field_index = i % 2 == 0 ? i + 1 : i - 1; // This process' +X neighbor treats this process as its -X neighbor and so on.
    });
  }

  // Send/receive particles.
  requests.clear();
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    if (!neighbors[i])
      continue;

    requests.push_back(partitioner_->communicator()->isend(neighbors[i]->rank, 1, surplus_particles[i]));
  }
  for (auto i = 0; i < neighbors.size(); ++i)
  {
    if (!neighbors[i])
      continue;

    std::vector<particle> temporary;
    partitioner_->communicator()->recv (neighbors[i]->rank, 1, temporary);
    particles.insert(particles.end(), temporary.begin(), temporary.end());
  }

  for (auto& request : requests)
    request.wait();
}
particle_tracer::round_info  particle_tracer::compute_round_info        (const std::vector<particle>& particles, const std::vector<integral_curves>& integral_curves                              )
{
  round_info round_info;

  round_info.maximum_remaining_iterations        = std::max_element(particles.begin(), particles.end(), [ ] (const particle& lhs, const particle& rhs) { return lhs.remaining_iterations < rhs.remaining_iterations; })->remaining_iterations;

  const auto maximum_vertices_per_integral_curve = std::numeric_limits<integer>::max() / sizeof(vector4);
  const auto particles_per_integral_curve        = maximum_vertices_per_integral_curve / round_info.maximum_remaining_iterations;

  round_info.vertices_per_integral_curve         =                    particles_per_integral_curve * round_info.maximum_remaining_iterations;
  round_info.vertices_per_integral_curve_last    = particles.size() % particles_per_integral_curve * round_info.maximum_remaining_iterations;
  round_info.integral_curve_offset               = integral_curves.size();
  round_info.integral_curve_count                = particles.size() == 0 ? 0 : 1 + particles.size() / particles_per_integral_curve;

  for (auto& neighbor : partitioner_->neighbor_rank_info())
  {
    if (neighbor)
    {
      round_info.out_of_bounds_particles         .emplace(neighbor->rank, std::vector<particle>());
      round_info.neighbor_out_of_bounds_particles.emplace(neighbor->rank, std::vector<particle>());
    }
  }

  return     round_info;
}
void                         particle_tracer::allocate                  (                                              std::vector<integral_curves>& integral_curves, const round_info& round_info)
{
  integral_curves.resize(round_info.integral_curve_offset + round_info.integral_curve_count);
  tbb::parallel_for(round_info.integral_curve_offset, integral_curves.size(), std::size_t(1), [&] (const std::size_t index)
  {
    integral_curves[index].vertices.resize(index != integral_curves.size() - 1 ? round_info.vertices_per_integral_curve : round_info.vertices_per_integral_curve_last, invalid_vertex);
  });
}
void                         particle_tracer::initialize                (const std::vector<particle>& particles,       std::vector<integral_curves>& integral_curves, const round_info& round_info)
{
  tbb::parallel_for(std::size_t(0), particles.size(), std::size_t(1), [&] (const std::size_t index)
  {
    const auto absolute_vertex_index = index * round_info.maximum_remaining_iterations;
    const auto relative_vertex_index = absolute_vertex_index % round_info.vertices_per_integral_curve;
    const auto integral_curve_index  = absolute_vertex_index / round_info.vertices_per_integral_curve + round_info.integral_curve_offset;
    integral_curves[integral_curve_index].vertices[relative_vertex_index] = particles[index].position;
  });
}
void                         particle_tracer::trace                     (const std::vector<particle>& particles,       std::vector<integral_curves>& integral_curves,       round_info& round_info)
{
  auto& local     = partitioner_->local_rank_info   ();
  auto& neighbors = partitioner_->neighbor_rank_info();

  tbb::parallel_for(std::size_t(0), particles.size(), std::size_t(1), [&] (const std::size_t particle_index)
  {
    auto& particle      = particles[particle_index];
    auto& vector_field  = particle.vector_field_index == -1 ? local_vector_field_->value() : neighbor_vector_fields_->at(particle.vector_field_index).value();
    auto  minimum       = vector_field.offset;
    auto  maximum       = vector_field.offset + vector_field.size;
    auto  integrator    = integrator_;

    for (std::size_t iteration_index = 1; iteration_index < particle.remaining_iterations; ++iteration_index)
    {
      const auto  absolute_vertex_index = particle_index * round_info.maximum_remaining_iterations + iteration_index;
      const auto  relative_vertex_index = absolute_vertex_index % round_info.vertices_per_integral_curve;
      const auto  integral_curve_index  = absolute_vertex_index / round_info.vertices_per_integral_curve + round_info.integral_curve_offset;

      const auto& last_vertex           = integral_curves[integral_curve_index].vertices[relative_vertex_index - 1];
            auto& vertex                = integral_curves[integral_curve_index].vertices[relative_vertex_index    ];

      vertex = termination_vertex;

      if (iteration_index == particle.remaining_iterations - 1)
        break;

      if (!vector_field.contains(last_vertex))
      {
        pa::particle neighbor_particle {last_vertex, particle.remaining_iterations - iteration_index, -1};

        if (particle.vector_field_index == -1)
        {
          auto neighbor_rank = -1;

          if      (neighbor_particle.position[0] < minimum[0] && neighbors[0]) neighbor_rank = neighbors[0]->rank;
          else if (neighbor_particle.position[0] > maximum[0] && neighbors[1]) neighbor_rank = neighbors[1]->rank;
          else if (neighbor_particle.position[1] < minimum[1] && neighbors[2]) neighbor_rank = neighbors[2]->rank;
          else if (neighbor_particle.position[1] > maximum[1] && neighbors[3]) neighbor_rank = neighbors[3]->rank;
          else if (neighbor_particle.position[2] < minimum[2] && neighbors[4]) neighbor_rank = neighbors[4]->rank;
          else if (neighbor_particle.position[2] > maximum[2] && neighbors[5]) neighbor_rank = neighbors[5]->rank;

          round_info::particle_map::accessor accessor;
          if (round_info.out_of_bounds_particles.find(accessor, neighbor_rank))
            accessor->second.push_back(neighbor_particle);
        }
        else
        {
          auto neighbor_rank = neighbors[particle.vector_field_index]->rank;

          round_info::particle_map::accessor accessor;
          if (round_info.neighbor_out_of_bounds_particles.find(accessor, neighbor_rank))
            accessor->second.push_back(neighbor_particle);
        }

        break;
      }

      const auto vector = vector_field.interpolate(last_vertex);
      if (vector.isZero())
        break;

      const auto system = [&] (const vector4& x, vector4& dxdt, const float t)
      {
        dxdt = vector4(vector[0], vector[1], vector[2], scalar(0));
      };
      if      (std::holds_alternative<euler_integrator>                       (integrator))
        std::get<euler_integrator>                       (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<modified_midpoint_integrator>           (integrator))
        std::get<modified_midpoint_integrator>           (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<runge_kutta_4_integrator>               (integrator))
        std::get<runge_kutta_4_integrator>               (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<runge_kutta_cash_karp_54_integrator>    (integrator))
        std::get<runge_kutta_cash_karp_54_integrator>    (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<runge_kutta_dormand_prince_5_integrator>(integrator))
        std::get<runge_kutta_dormand_prince_5_integrator>(integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<runge_kutta_fehlberg_78_integrator>     (integrator))
        std::get<runge_kutta_fehlberg_78_integrator>     (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<adams_bashforth_2_integrator>           (integrator))
        std::get<adams_bashforth_2_integrator>           (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
      else if (std::holds_alternative<adams_bashforth_moulton_2_integrator>   (integrator))
        std::get<adams_bashforth_moulton_2_integrator>   (integrator).do_step(system, last_vertex, iteration_index * step_size_, vertex, step_size_);
    }
  });
}
void                         particle_tracer::load_balance_collect      (                                                                                round_info& round_info)
{
  auto& neighbors = partitioner_->neighbor_rank_info();
  auto  minimum   = local_vector_field_->value().offset;
  auto  maximum   = local_vector_field_->value().offset + local_vector_field_->value().size;

  std::vector<boost::mpi::request> requests;
  for (auto& neighbor : round_info.neighbor_out_of_bounds_particles)
    requests.push_back(partitioner_->communicator()->isend(neighbor.first, 2, neighbor.second));

  for (auto& neighbor : round_info.neighbor_out_of_bounds_particles)
  {
    std::vector<particle> temporary;
    partitioner_->communicator()->recv (neighbor.first, 2, temporary);

    tbb::parallel_for(std::size_t(0), temporary.size(), std::size_t(1), [&] (const std::size_t index)
    {
      auto& particle      = temporary[index];
      auto  neighbor_rank = -1;

      particle.vector_field_index = -1;

      if      (particle.position[0] < minimum[0] && neighbors[0]) neighbor_rank = neighbors[0]->rank;
      else if (particle.position[0] > maximum[0] && neighbors[1]) neighbor_rank = neighbors[1]->rank;
      else if (particle.position[1] < minimum[1] && neighbors[2]) neighbor_rank = neighbors[2]->rank;
      else if (particle.position[1] > maximum[1] && neighbors[3]) neighbor_rank = neighbors[3]->rank;
      else if (particle.position[2] < minimum[2] && neighbors[4]) neighbor_rank = neighbors[4]->rank;
      else if (particle.position[2] > maximum[2] && neighbors[5]) neighbor_rank = neighbors[5]->rank;

      round_info::particle_map::accessor accessor;
      if (round_info.out_of_bounds_particles.find(accessor, neighbor_rank))
        accessor->second.push_back(particle);
    });
  }

  for (auto& request : requests)
    request.wait();
}
void                         particle_tracer::out_of_bounds_distribute  (      std::vector<particle>& particles,                                                      const round_info& round_info)
{
  particles.clear();

  std::vector<boost::mpi::request> requests;
  for (auto& neighbor : round_info.out_of_bounds_particles)
    requests.push_back(partitioner_->communicator()->isend(neighbor.first, 3, neighbor.second));

  for (auto& neighbor : round_info.out_of_bounds_particles)
  {
    std::vector<particle> temporary;
    partitioner_->communicator()->recv (neighbor.first, 3, temporary);
    particles.insert(particles.end(), temporary.begin(), temporary.end());
  }

  for (auto& request : requests)
    request.wait();
}
bool                         particle_tracer::check_completion          (const std::vector<particle>& particles                                                                                )
{
  std::vector<std::size_t> particle_sizes;
  boost::mpi::gather   (*partitioner_->communicator(), particles.size(), particle_sizes, 0);
  auto   complete = std::all_of(particle_sizes.begin(), particle_sizes.end(), std::bind(std::equal_to<std::size_t>(), std::placeholders::_1, 0));
  boost::mpi::broadcast(*partitioner_->communicator(), complete, 0);
  return complete;
}
void                         particle_tracer::prune                     (                                              std::vector<integral_curves>& integral_curves                              )
{
  tbb::parallel_for(std::size_t(0), integral_curves.size(), std::size_t(1), [&] (const std::size_t index)
  {
    auto& vertices = integral_curves[index].vertices;
    vertices.erase(std::remove(vertices.begin(), vertices.end(), invalid_vertex), vertices.end());
  });
}
}


#include "find_initial_state.h"

#include "session_param.h"
#include "frames.h"
#include "vox_grid.h"

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <string>
#include <stdint.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <thread>
#include <chrono>
#include <algorithm>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>
#include <armadillo>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/PCLPointCloud2.h>

#include "focal_grid.h"

#define PI 3.14159265

using namespace std;

namespace p = boost::python;
namespace np = boost::python::numpy;

//-----------------------------------------------------------------------
//
// Find the initial state of the fly using minimum bounding boxes
//
//-----------------------------------------------------------------------

InitState::InitState() {
	// empty
}

void InitState::SetWeightXsi(double WXsi) {
	w_xsi = WXsi;
}

void InitState::SetWeightTheta(double WTheta) {
	w_theta = WTheta;
}

void InitState::SetWeightLength(double WLength) {
	w_length = WLength;
}

void InitState::SetWeightVolume(double WVolume) {
	w_volume = WVolume;
}

void InitState::SetConeAngle(double ConeAngle) {
	cone_angle = (ConeAngle/180.0)*PI;
}

void InitState::SetConeHeight(double ConeHeight) {
	cone_height = ConeHeight;
}

void InitState::ProjectSingleFrame(FocalGrid &fg, frames &frame_in, vox_grid &vox) {

	// Project a single segmented frame to a set of pointclouds:

	frame_in.single_frame_pcl.clear();

	vector<tuple<int,double,double,double,double,double,double>> pcl_out = fg.ProjectImage2Cloud(frame_in.single_seg_frame, vox);

	frame_in.single_frame_pcl = pcl_out;

}

void InitState::ProjectFrameBatch(FocalGrid &fg, frames &frame_in, vox_grid &vox) {

	// Project a segmented frame batch to pointclouds:

	frame_in.frame_batch_pcl.clear();

	int N_frames = frame_in.seg_frames[0].n_cols;

	int N_cam = frame_in.seg_frames.size();

	for (int i=0; i<N_frames; i++) {

		vector<arma::Col<int>> frame_now;

		for (int j=0; j<N_cam; j++) {
			frame_now.push_back(frame_in.seg_frames[j].col(i));
		}

		vector<tuple<int,double,double,double,double,double,double>> pcl_out = fg.ProjectImage2Cloud(frame_now, vox);

		frame_in.frame_batch_pcl.push_back(pcl_out);

	}

}

void InitState::FindInitialStateSingleFrame(frames &frame_in, model &mod) {

	frame_in.pcl_init_single_frame.clear();
	frame_in.M_init_single_frame.clear();

	double wing_length = mod.wing_length;

	vector<tuple<int,double,double,double,double,double,double>> pcl_in = frame_in.single_frame_pcl;

	arma::Mat<double> pcl_mat = InitState::ConvertVectorPCLArma(pcl_in);

	vector<tuple<struct bbox, arma::Mat<double>>> bbox_vector = InitState::FindBoundingBoxes(pcl_mat);

	vector<tuple<arma::Mat<double>,arma::Mat<double>>> pcl_vector = InitState::FindBodyandWing(bbox_vector,wing_length);

	frame_in.pcl_init_single_frame.push_back(get<0>(pcl_vector[0]));
	frame_in.pcl_init_single_frame.push_back(get<0>(pcl_vector[1]));
	frame_in.pcl_init_single_frame.push_back(get<0>(pcl_vector[2]));
	frame_in.M_init_single_frame.push_back(get<1>(pcl_vector[0]));
	frame_in.M_init_single_frame.push_back(get<1>(pcl_vector[1]));
	frame_in.M_init_single_frame.push_back(get<1>(pcl_vector[2]));

}

void InitState::FindInitialStateFrameBatch(frames &frame_in, model &mod) {

	frame_in.pcl_init_frame_batch.clear();
	frame_in.M_init_frame_batch.clear();

	double wing_length = mod.wing_length;

	int N_batch = frame_in.raw_frames[0].n_cols;

	frame_in.pcl_init_frame_batch.resize(N_batch);
	frame_in.M_init_frame_batch.resize(N_batch);

	for (int i=0; i<N_batch; i++) {

		vector<tuple<int,double,double,double,double,double,double>> pcl_in = frame_in.frame_batch_pcl[i];

		arma::Mat<double> pcl_mat = InitState::ConvertVectorPCLArma(pcl_in);

		vector<tuple<struct bbox, arma::Mat<double>>> bbox_vector = InitState::FindBoundingBoxes(pcl_mat);

		vector<tuple<arma::Mat<double>,arma::Mat<double>>> pcl_vector = InitState::FindBodyandWing(bbox_vector,wing_length);

		frame_in.pcl_init_frame_batch[i].push_back(get<0>(pcl_vector[0]));
		frame_in.pcl_init_frame_batch[i].push_back(get<0>(pcl_vector[1]));
		frame_in.pcl_init_frame_batch[i].push_back(get<0>(pcl_vector[2]));
		frame_in.M_init_frame_batch[i].push_back(get<1>(pcl_vector[0]));
		frame_in.M_init_frame_batch[i].push_back(get<1>(pcl_vector[1]));
		frame_in.M_init_frame_batch[i].push_back(get<1>(pcl_vector[2]));

	}

}

vector<tuple<arma::Mat<double>,arma::Mat<double>>> InitState::FindBodyandWing(vector<tuple<struct bbox, arma::Mat<double>>> &bbox_vector, double wing_length) {

	int N_segments = bbox_vector.size();

	arma::Mat<double> M_body;
	arma::Mat<double> M_wing_L;
	arma::Mat<double> M_wing_R;

	arma::Mat<double> body_pcl;
	arma::Mat<double> wing_L_pcl;
	arma::Mat<double> wing_R_pcl;

	if (N_segments>0) {

		struct bbox body_box = get<0>(bbox_vector[0]);
		body_pcl = get<1>(bbox_vector[0]);

		M_body = InitState::FindBodyRefFrame(body_box, body_pcl);

		if (N_segments>1) {

			vector<arma::Mat<double>> c_vector;
			vector<arma::Mat<double>> wing_pcl;

			for (int i=1; i<N_segments; i++) {
				arma::Mat<double> c_points;
				c_points = InitState::FindRootTip(M_body, get<0>(bbox_vector[i]));
				c_vector.push_back(c_points);
				wing_pcl.push_back(get<1>(bbox_vector[i]));
			}

			vector<arma::Mat<double>> wing_pcl_LR = InitState::FindWingPCL(wing_pcl, c_vector, wing_length);
			wing_L_pcl = wing_pcl_LR[0];
			wing_R_pcl = wing_pcl_LR[1];
		}
		else {
			wing_L_pcl.zeros(7,1);
			wing_R_pcl.zeros(7,1);
		}
	}
	else {
		body_pcl.zeros(7,1);
		wing_L_pcl.zeros(7,1);
		wing_R_pcl.zeros(7,1);
	}

	body_pcl.row(0).fill(1.0);
	wing_L_pcl.row(0).fill(2.0);
	wing_R_pcl.row(0).fill(3.0);

	vector<arma::Mat<double>> M_vector = InitState::FindWingRefFrame(M_body, wing_L_pcl, wing_R_pcl);

	//arma::Mat<double> wing_select_L = InitState::FindWingOrientation(M_vector[0], wing_L_pcl, 0);
	//arma::Mat<double> wing_select_R = InitState::FindWingOrientation(M_vector[1], wing_R_pcl, 1);

	vector<tuple<arma::Mat<double>,arma::Mat<double>>> vector_out;

	vector_out.push_back(make_tuple(body_pcl,M_body));
	vector_out.push_back(make_tuple(wing_L_pcl,M_vector[0]));
	vector_out.push_back(make_tuple(wing_R_pcl,M_vector[1]));

	//vector_out.push_back(make_tuple(body_pcl,M_body));
	//vector_out.push_back(make_tuple(wing_L_pcl,wing_select_L));
	//vector_out.push_back(make_tuple(wing_R_pcl,wing_select_R));

	return vector_out;
}

vector<arma::Mat<double>> InitState::FindWingPCL(vector<arma::Mat<double>> wing_pcls, vector<arma::Mat<double>> wing_prop, double wing_length) {

	int N_c = wing_prop.size(); // it is expected that there is 1 candidate or more

	vector<arma::Mat<double>> wing_pcl_LR;

	// Try to find a left and right wing candidate first:

	double score = 0.0;
	tuple<int, int> LR_pair;
	LR_pair = make_tuple(-1,-1);

	for (int i=0; i<N_c; i++) {
		for (int j=0; j<i; j++) {
			if ((wing_prop[i](3,9)>(0.8*wing_length)) && (wing_prop[i](3,9)<(1.5*wing_length))) {
				if ((wing_prop[j](3,9)>(0.8*wing_length)) && (wing_prop[j](3,9)<(1.5*wing_length))) {
					arma::Col<double> tip_i = {wing_prop[i](0,9),wing_prop[i](1,9),wing_prop[i](2,9)};
					arma::Col<double> tip_j = {wing_prop[j](0,9),wing_prop[j](1,9),wing_prop[j](2,9)};
					double d_xsi = arma::dot(tip_i,tip_j)/(arma::norm(tip_i,2.0)*arma::norm(tip_j,2.0));
					double delta_xsi = 0.0;
					if (d_xsi >= -1.0 && d_xsi <= 1.0) {
						delta_xsi = acos(d_xsi);
					}
					double xsi_i = wing_prop[i](0,10);
					double xsi_j = wing_prop[j](0,10);
					double theta_avg = sqrt(pow((wing_prop[i](1,10)+wing_prop[j](1,10))/2.0,2));
					double wing_span_avg = (wing_prop[i](2,10)+wing_prop[j](2,10))/2.0;
					double volume_avg = (wing_prop[i](3,10)+wing_prop[j](3,10));
					double score_now = w_xsi*(delta_xsi/PI)+w_length*(wing_span_avg/wing_length)+w_volume*volume_avg-w_theta*((2.0*theta_avg)/PI);
					if (score_now>score) {
						score = score_now;
						if (xsi_i > xsi_j) {
							LR_pair = make_tuple(i,j);
						}
						else if (xsi_i<xsi_j) {
							LR_pair = make_tuple(j,i);
						}
						else {
							LR_pair = make_tuple(i,j);
						}
					}
				}
			}
		}
	}

	cout << "left right pair" << endl;
	cout << get<0>(LR_pair) << endl;
	cout << get<1>(LR_pair) << endl;
	cout << "" << endl;

	// If a pair has been found, form a pointcloud of all pointclouds in the vicinity of the wingtips:

	int ind_L = get<0>(LR_pair);
	int ind_R = get<1>(LR_pair);

	arma::Mat<double> wing_L_pcl;
	arma::Mat<double> wing_R_pcl;

	if (ind_L != -1) {
		wing_L_pcl = wing_pcls[ind_L];
		wing_R_pcl = wing_pcls[ind_R];

		arma::Col<double> tip_L = {wing_prop[ind_L](0,9),wing_prop[ind_L](1,9),wing_prop[ind_L](2,9)};
		arma::Col<double> tip_R = {wing_prop[ind_R](0,9),wing_prop[ind_R](1,9),wing_prop[ind_R](2,9)};

		for (int i=0; i<N_c; i++) {
			arma::Col<double> tip_i = {wing_prop[i](0,9),wing_prop[i](1,9),wing_prop[i](2,9)};
			//arma::Col<double> root_i = {wing_prop[i](0,8),wing_prop[i](1,8),wing_prop[i](2,8)};
			arma::Col<double> root_i = {wing_prop[i](0,11),wing_prop[i](1,11),wing_prop[i](2,11)};
			if (i != ind_L) {
				double delta_angle_tip = arma::dot(tip_i,tip_L)/(arma::norm(tip_i,2.0)*arma::norm(tip_L,2.0));
				double tip_angle_L = -PI;
				if (delta_angle_tip >= -1.0 && delta_angle_tip <= 1.0) {
					tip_angle_L = acos(delta_angle_tip);
				}
				double delta_angle_root = arma::dot(root_i,tip_L)/(arma::norm(root_i,2.0)*arma::norm(tip_L,2.0));
				double root_angle_L = -PI;
				if (delta_angle_root >= -1.0 && delta_angle_root <= 1.0) {
					root_angle_L = acos(delta_angle_root);
				}
				double center_dist = wing_prop[i](3,11);
				if (tip_angle_L >= 0.0 && tip_angle_L <= cone_angle) {
					if (root_angle_L >= 0.0 && root_angle_L <= cone_angle) {
						if (center_dist > ((1.0-cone_height)*wing_length)) {
							wing_L_pcl = arma::join_rows(wing_L_pcl,wing_pcls[i]);
						}
					}
				}
			}
			if (i != ind_R) {
				double delta_angle_tip = arma::dot(tip_i,tip_R)/(arma::norm(tip_i,2.0)*arma::norm(tip_R,2.0));
				double tip_angle_R = -PI;
				if (delta_angle_tip >= -1.0 && delta_angle_tip <= 1.0) {
					tip_angle_R = acos(delta_angle_tip);
				}
				double delta_angle_root = arma::dot(root_i,tip_R)/(arma::norm(root_i,2.0)*arma::norm(tip_R,2.0));
				double root_angle_R = -PI;
				if (delta_angle_root >= -1.0 && delta_angle_root <= 1.0) {
					root_angle_R = acos(delta_angle_root);
				}
				double center_dist = wing_prop[i](3,11);
				if (tip_angle_R >= 0.0 && tip_angle_R <= cone_angle) {
					if (root_angle_R >= 0.0 && root_angle_R <= cone_angle) {
						if (center_dist > ((1.0-cone_height)*wing_length)) {
							wing_R_pcl = arma::join_rows(wing_R_pcl,wing_pcls[i]);
						}
					}
				}
			}
		}
	}
	else {
		wing_L_pcl.zeros(7,1);
		wing_R_pcl.zeros(7,1);
	}

	wing_pcl_LR.push_back(wing_L_pcl);
	wing_pcl_LR.push_back(wing_R_pcl);

	return wing_pcl_LR;
}

/*
arma::Mat<double> InitState::FindWingOrientation(arma::Mat<double> M_vector, arma::Mat<double> &wing_pcl_in, int L_or_R) {

	int N_segs = 5;

	arma::Mat<double> M_out;
	M_out.zeros(4,8);

	// Get x,y,z coordinates from pcl:

	double theta_avg = 0.0;

	if (wing_pcl_in.n_cols>10 && arma::sum(M_vector.row(3)) != 0) {

		arma::Mat<double> wing_pcl;

		wing_pcl.ones(4,wing_pcl_in.n_cols);

		wing_pcl.row(0) = wing_pcl_in.row(1);
		wing_pcl.row(1) = wing_pcl_in.row(2);
		wing_pcl.row(2) = wing_pcl_in.row(3);

		// Project pcl into wing reference frame:

		arma::Mat<double> M_wing;
		M_wing.eye(4,4);

		// Transpose of the matrix

		M_wing(0,0) = M_vector(0,0);
		M_wing(0,1) = M_vector(1,0);
		M_wing(0,2) = M_vector(2,0);
		M_wing(0,3) = -M_vector(0,3);
		M_wing(1,0) = M_vector(0,1);
		M_wing(1,1) = M_vector(1,1);
		M_wing(1,2) = M_vector(2,1);
		M_wing(1,3) = -M_vector(1,3);
		M_wing(2,0) = M_vector(0,2);
		M_wing(2,1) = M_vector(1,2);
		M_wing(2,2) = M_vector(2,2);
		M_wing(2,3) = -M_vector(2,3);

		arma::Mat<double> wing_pcl_ref = M_wing*wing_pcl;

		// Perform svd on wing_pcl_ref:

		arma::Row<double> angle_y;
		angle_y.zeros(N_segs);

		arma::Row<double> chord_length;
		chord_length.zeros(N_segs);

		arma::Row<int> N_seg_points;
		N_seg_points.zeros(N_segs);

		if (L_or_R==0) {

			double y_tip = wing_pcl_ref.row(1).max();

			// Split the wing in N segments:
			for (int i=0; i<N_segs; i++) {

				// Select y-slice elements
				arma::uvec slice_y_ids = arma::find((wing_pcl_ref.row(1) > (i*(y_tip/((N_segs+1)*1.0)))) && (wing_pcl_ref.row(1) < ((i+1)*(y_tip/((N_segs+1)*1.0)))));

				if (slice_y_ids.n_rows > 2) {

					arma::Mat<double> slice_points_y = wing_pcl_ref.cols(slice_y_ids);

					// Find the maximum radius from the mid point of the section

					arma::Row<double> R1;
					R1.zeros(slice_y_ids.n_rows);

					for (int j=0; j<slice_y_ids.n_rows; j++) {
						R1(j) = sqrt(pow(slice_points_y(0,j),2.0)+pow(slice_points_y(2,j),2.0));
					}

					int max_R1 = R1.index_max();

					arma::Col<double> R1_point = slice_points_y.col(max_R1);

					// Find the maximum radius from R1:

					arma::Row<double> R2;
					R2.zeros(slice_y_ids.n_rows);

					for (int j=0; j<slice_y_ids.n_rows; j++) {
						R2(j) = sqrt(pow(slice_points_y(0,j)-R1_point(0),2.0)+pow(slice_points_y(2,j)-R1_point(2),2.0));
					}

					int max_R2 = R2.index_max();

					arma::Col<double> R2_point = slice_points_y.col(max_R2);

					// Calculate the angle between the x-axis and the vector R1_point-R2_point:

					double theta = atan2(R1_point(2)-R2_point(2),R1_point(0)-R2_point(0));

					if (isfinite(theta)) {
						angle_y(i) = theta;
						chord_length(i) = sqrt(pow(R1_point(0)-R2_point(0),2.0)+pow(R1_point(2)-R2_point(2),2.0));
					}
				}
				/*
				if (slice_y_ids.n_rows > 10) {

					// Perform eigenvalue decomposition:

					arma::Mat<double> slice_points_y = wing_pcl_ref.cols(slice_y_ids);

					arma::Mat<double> X(3,slice_y_ids.n_rows);

					X.row(0) = slice_points_y.row(0);
					X.row(1) = slice_points_y.row(1);
					X.row(2) = slice_points_y.row(2);

					arma::mat U;
					arma::vec s;
					arma::mat V;

					arma::svd(U,s,V,X);

					arma::Col<double> eig_vec_1 = U.col(0);

					double theta = atan2(eig_vec_1(2),eig_vec_1(0));

					if (isfinite(theta)) {
						angle_y(i) = theta;
						N_seg_points(i) = slice_y_ids.n_rows;
						//chord_length(i) = sqrt(pow(R1_point(0)-R2_point(0),2.0)+pow(R1_point(2)-R2_point(2),2.0));
					}
				}
			}
		}
		else {

			double y_tip = wing_pcl_ref.row(1).min();

			// Split the wing in N segments:
			for (int i=0; i<N_segs; i++) {

				// Select y-slice elements
				arma::uvec slice_y_ids = arma::find((wing_pcl_ref.row(1) < (i*(y_tip/((N_segs+1)*1.0)))) && (wing_pcl_ref.row(1) > ((i+1)*(y_tip/((N_segs+1)*1.0)))));

				if (slice_y_ids.n_rows > 2) {

					arma::Mat<double> slice_points_y = wing_pcl_ref.cols(slice_y_ids);

					// Find the maximum radius from the mid point of the section

					arma::Row<double> R1;
					R1.zeros(slice_y_ids.n_rows);

					for (int j=0; j<slice_y_ids.n_rows; j++) {
						R1(j) = sqrt(pow(slice_points_y(0,j),2.0)+pow(slice_points_y(2,j),2.0));
					}

					int max_R1 = R1.index_max();

					arma::Col<double> R1_point = slice_points_y.col(max_R1);

					// Find the maximum radius from R1:

					arma::Row<double> R2;
					R2.zeros(slice_y_ids.n_rows);

					for (int j=0; j<slice_y_ids.n_rows; j++) {
						R2(j) = sqrt(pow(slice_points_y(0,j)-R1_point(0),2.0)+pow(slice_points_y(2,j)-R1_point(2),2.0));
					}

					int max_R2 = R2.index_max();

					arma::Col<double> R2_point = slice_points_y.col(max_R2);

					// Calculate the angle between the x-axis and the vector R1_point-R2_point:

					double theta = atan2(R1_point(2)-R2_point(2),R1_point(0)-R2_point(0));

					if (isfinite(theta)) {
						angle_y(i) = theta;
						chord_length(i) = sqrt(pow(R1_point(0)-R2_point(0),2.0)+pow(R1_point(2)-R2_point(2),2.0));
					}
				}
				/*
				if (slice_y_ids.n_rows > 10) {

					// Perform eigenvalue decomposition:

					arma::Mat<double> slice_points_y = wing_pcl_ref.cols(slice_y_ids);

					arma::Mat<double> X(3,slice_y_ids.n_rows);

					X.row(0) = slice_points_y.row(0);
					X.row(1) = slice_points_y.row(1);
					X.row(2) = slice_points_y.row(2);

					arma::mat U;
					arma::vec s;
					arma::mat V;

					arma::svd(U,s,V,X);

					arma::Col<double> eig_vec_1 = U.col(0);

					double theta = atan2(eig_vec_1(2),eig_vec_1(0));

					if (isfinite(theta)) {
						angle_y(i) = theta;
						N_seg_points(i) = slice_y_ids.n_rows;
						//chord_length(i) = sqrt(pow(R1_point(0)-R2_point(0),2.0)+pow(R1_point(2)-R2_point(2),2.0));
					}
				}

			}
		}

		cout << angle_y << endl;
		cout << N_seg_points << endl;

		// Determine the average orientation angle:
		double cos_2_theta_avg = 0.0;
		double chord_length_sum = 0.0;
		for (int i=0; i<N_segs; i++) {
			if (chord_length(i) > 0.0) {
				cos_2_theta_avg += chord_length(i)*0.5*acos(cos(2.0*angle_y(i)));
				chord_length_sum += chord_length(i);
			}
		}

		cout << cos_2_theta_avg << endl;

		double theta_avg = 0.0;

		if (chord_length_sum>0.0) {
			theta_avg = cos_2_theta_avg/chord_length_sum;
		}

		/*
		double cos_2_theta_avg = 0.0;
		int N_seg_points_sum = 0;

		for (int i=0; i<N_segs; i++) {
			if (N_seg_points(i) > 10) {
				cos_2_theta_avg += N_seg_points(i)*0.5*acos(cos(2.0*angle_y(i)));
				N_seg_points_sum += N_seg_points(i);
			}
		}

		cout << cos_2_theta_avg << endl;

		double theta_avg = 0.0;

		if (N_seg_points_sum>10) {
			theta_avg = cos_2_theta_avg/(1.0*N_seg_points_sum);
		}

		cout << theta_avg << endl;

		// Rotate the pointcloud by theta_avg around the y-axis:
		arma::Mat<double> M_theta1;
		M_theta1.eye(4,4);
		M_theta1(0,0) = cos(theta_avg);
		M_theta1(0,2) = -sin(theta_avg);
		M_theta1(2,0) = sin(theta_avg);
		M_theta1(2,2) = cos(theta_avg);

		cout << "M theta 1" << endl;
		cout << M_theta1 << endl;

		arma::Mat<double> M_theta2;
		M_theta2.eye(4,4);
		M_theta2(0,0) = cos(theta_avg+PI);
		M_theta2(0,2) = -sin(theta_avg+PI);
		M_theta2(2,0) = sin(theta_avg+PI);
		M_theta2(2,2) = cos(theta_avg+PI);

		cout << "M theta 2" << endl;
		cout << M_theta2 << endl;

		// Return M_out, wing_grid and orient_ind:

		M_out(0,0) = M_vector(0,0);
		M_out(0,1) = M_vector(0,1);
		M_out(0,2) = M_vector(0,2);
		M_out(0,3) = M_vector(0,3);
		M_out(1,0) = M_vector(1,0);
		M_out(1,1) = M_vector(1,1);
		M_out(1,2) = M_vector(1,2);
		M_out(1,3) = M_vector(1,3);
		M_out(2,0) = M_vector(2,0);
		M_out(2,1) = M_vector(2,1);
		M_out(2,2) = M_vector(2,2);
		M_out(2,3) = M_vector(2,3);
		M_out(3,3) = 1.0;

		M_out.submat(0,0,3,3) = M_out.submat(0,0,3,3)*M_theta1;

		M_out(0,4) = M_vector(0,0);
		M_out(0,5) = M_vector(0,1);
		M_out(0,6) = M_vector(0,2);
		M_out(0,7) = M_vector(0,3);
		M_out(1,4) = M_vector(1,0);
		M_out(1,5) = M_vector(1,1);
		M_out(1,6) = M_vector(1,2);
		M_out(1,7) = M_vector(1,3);
		M_out(2,4) = M_vector(2,0);
		M_out(2,5) = M_vector(2,1);
		M_out(2,6) = M_vector(2,2);
		M_out(2,7) = M_vector(2,3);
		M_out(3,7) = 1.0;

		M_out.submat(0,4,3,7) = M_out.submat(0,4,3,7)*M_theta2;

		cout << M_out << endl;

	}
	else {

		M_out(0,0) = M_vector(0,0);
		M_out(0,1) = M_vector(0,1);
		M_out(0,2) = M_vector(0,2);
		M_out(0,3) = M_vector(0,3);
		M_out(1,0) = M_vector(1,0);
		M_out(1,1) = M_vector(1,1);
		M_out(1,2) = M_vector(1,2);
		M_out(1,3) = M_vector(1,3);
		M_out(2,0) = M_vector(2,0);
		M_out(2,1) = M_vector(2,1);
		M_out(2,2) = M_vector(2,2);
		M_out(2,3) = M_vector(2,3);
		M_out(3,3) = 1.0;

		M_out(0,4) = M_vector(0,8);
		M_out(0,5) = M_vector(0,9);
		M_out(0,6) = M_vector(0,10);
		M_out(0,7) = M_vector(0,11);
		M_out(1,4) = M_vector(1,8);
		M_out(1,5) = M_vector(1,9);
		M_out(1,6) = M_vector(1,10);
		M_out(1,7) = M_vector(1,11);
		M_out(2,4) = M_vector(2,8);
		M_out(2,5) = M_vector(2,9);
		M_out(2,6) = M_vector(2,10);
		M_out(2,7) = M_vector(2,11);
		M_out(3,7) = 1.0;

	}

	return M_out;

}
*/

vector<arma::Mat<double>> InitState::FindWingRefFrame(arma::Mat<double> M_body, arma::Mat<double> wing_L_pcl, arma::Mat<double> wing_R_pcl) {

	// Fit oriented bounding boxes to the pointclouds:

	arma::Mat<double> M_L(4,16); // 4 possible orientations left wing

	if (arma::sum(wing_L_pcl.row(1)) != 0.0) {
		struct bbox bbox_L = InitState::BoundingBox(wing_L_pcl);
		arma::Mat<double> c_points_L = InitState::FindRootTip(M_body, bbox_L);
		arma::Col<double> root_L(3);
		arma::Col<double> tip_L(3);
		root_L(0) = 0.25*(c_points_L(0,0)+c_points_L(0,1)+c_points_L(0,2)+c_points_L(0,3));
		root_L(1) = 0.25*(c_points_L(1,0)+c_points_L(1,1)+c_points_L(1,2)+c_points_L(1,3));
		root_L(2) = 0.25*(c_points_L(2,0)+c_points_L(2,1)+c_points_L(2,2)+c_points_L(2,3));
		tip_L(0) = 0.25*(c_points_L(0,4)+c_points_L(0,5)+c_points_L(0,6)+c_points_L(0,7));
		tip_L(1) = 0.25*(c_points_L(1,4)+c_points_L(1,5)+c_points_L(1,6)+c_points_L(1,7));
		tip_L(2) = 0.25*(c_points_L(2,4)+c_points_L(2,5)+c_points_L(2,6)+c_points_L(2,7));

		for (int i=0; i<4; i++) {
			arma::Mat<double> base(3,3);
			arma::Col<double> x_vec(3);
			arma::Col<double> y_vec(3);
			arma::Col<double> z_vec(3);

			x_vec(0) = c_points_L(0,i)-root_L(0);
			x_vec(1) = c_points_L(1,i)-root_L(1);
			x_vec(2) = c_points_L(2,i)-root_L(2);
			base.col(0) = x_vec/arma::norm(x_vec,2);
			y_vec(0) = tip_L(0)-root_L(0);
			y_vec(1) = tip_L(1)-root_L(1);
			y_vec(2) = tip_L(2)-root_L(2);
			base.col(1) = y_vec/arma::norm(y_vec,2);
			z_vec = arma::cross(x_vec,y_vec);
			base.col(2) = z_vec/arma::norm(z_vec,2);

			M_L(0,i*4) 	 = base(0,0);
			M_L(0,i*4+1) = base(0,1);
			M_L(0,i*4+2) = base(0,2);
			M_L(0,i*4+3) = root_L(0);
			M_L(1,i*4) 	 = base(1,0);
			M_L(1,i*4+1) = base(1,1);
			M_L(1,i*4+2) = base(1,2);
			M_L(1,i*4+3) = root_L(1);
			M_L(2,i*4) 	 = base(2,0);
			M_L(2,i*4+1) = base(2,1);
			M_L(2,i*4+2) = base(2,2);
			M_L(2,i*4+3) = root_L(2);
			M_L(3,i*4) 	 = 0.0;
			M_L(3,i*4+1) = 0.0;
			M_L(3,i*4+2) = 0.0;
			M_L(3,i*4+3) = 1.0;
		}
	}
	else {
		M_L.zeros(4,16);
	}

	arma::Mat<double> M_R(4,16); // 4 possible orientations right wing

	if (arma::sum(wing_R_pcl.row(1)) != 0.0) {
		struct bbox bbox_R = InitState::BoundingBox(wing_R_pcl);
		arma::Mat<double> c_points_R = InitState::FindRootTip(M_body, bbox_R);
		arma::Col<double> root_R(3);
		arma::Col<double> tip_R(3);
		root_R(0) = 0.25*(c_points_R(0,0)+c_points_R(0,1)+c_points_R(0,2)+c_points_R(0,3));
		root_R(1) = 0.25*(c_points_R(1,0)+c_points_R(1,1)+c_points_R(1,2)+c_points_R(1,3));
		root_R(2) = 0.25*(c_points_R(2,0)+c_points_R(2,1)+c_points_R(2,2)+c_points_R(2,3));
		tip_R(0) = 0.25*(c_points_R(0,4)+c_points_R(0,5)+c_points_R(0,6)+c_points_R(0,7));
		tip_R(1) = 0.25*(c_points_R(1,4)+c_points_R(1,5)+c_points_R(1,6)+c_points_R(1,7));
		tip_R(2) = 0.25*(c_points_R(2,4)+c_points_R(2,5)+c_points_R(2,6)+c_points_R(2,7));

		for (int i=0; i<4; i++) {
			arma::Mat<double> base(3,3);
			arma::Col<double> x_vec(3);
			arma::Col<double> y_vec(3);
			arma::Col<double> z_vec(3);

			x_vec(0) = c_points_R(0,i)-root_R(0);
			x_vec(1) = c_points_R(1,i)-root_R(1);
			x_vec(2) = c_points_R(2,i)-root_R(2);
			//base.row(0) = arma::trans(x_vec/arma::norm(x_vec,2));
			base.col(0) = x_vec/arma::norm(x_vec,2);
			y_vec(0) = -tip_R(0)+root_R(0);
			y_vec(1) = -tip_R(1)+root_R(1);
			y_vec(2) = -tip_R(2)+root_R(2);
			base.col(1) = y_vec/arma::norm(y_vec,2);
			z_vec = arma::cross(x_vec,y_vec);
			base.col(2) = z_vec/arma::norm(z_vec,2);

			M_R(0,i*4) 	 = base(0,0);
			M_R(0,i*4+1) = base(0,1);
			M_R(0,i*4+2) = base(0,2);
			M_R(0,i*4+3) = root_R(0);
			M_R(1,i*4) 	 = base(1,0);
			M_R(1,i*4+1) = base(1,1);
			M_R(1,i*4+2) = base(1,2);
			M_R(1,i*4+3) = root_R(1);
			M_R(2,i*4) 	 = base(2,0);
			M_R(2,i*4+1) = base(2,1);
			M_R(2,i*4+2) = base(2,2);
			M_R(2,i*4+3) = root_R(2);
			M_R(3,i*4) 	 = 0.0;
			M_R(3,i*4+1) = 0.0;
			M_R(3,i*4+2) = 0.0;
			M_R(3,i*4+3) = 1.0;
		}
	}
	else {
		M_R.zeros(4,16);
	}

	vector<arma::Mat<double>> output_vec;

	output_vec.push_back(M_L);
	output_vec.push_back(M_R);

	return output_vec;
}

arma::Mat<double> InitState::FindBodyRefFrame(struct bbox &body_box, arma::Mat<double> &body_pcl) {

	// I don't really like this, it assumes some properties of the body which are probably not universal

	arma::Mat<double> M_body = {{body_box.R_box[0][0],body_box.R_box[0][1],body_box.R_box[0][2],body_box.box_center[0]},
			{body_box.R_box[1][0],body_box.R_box[1][1],body_box.R_box[1][2],body_box.box_center[1]},
			{body_box.R_box[2][0],body_box.R_box[2][1],body_box.R_box[2][2],body_box.box_center[2]},
			{0.0, 0.0, 0.0 ,1.0}};

	arma::Mat<double> M_body_T = {{body_box.R_box[0][0],body_box.R_box[1][0],body_box.R_box[2][0]},
			{body_box.R_box[0][1],body_box.R_box[1][1],body_box.R_box[2][1]},
			{body_box.R_box[0][2],body_box.R_box[1][2],body_box.R_box[2][2]}};

	// Convert points to body reference frame:

 	int N_points = body_pcl.n_cols;

 	arma::Mat<double> body_points(3,N_points);

 	body_points.row(0) = body_pcl.row(1);
 	body_points.row(1) = body_pcl.row(2);
 	body_points.row(2) = body_pcl.row(3);

 	arma::Col<double> body_center = {body_box.box_center[0], body_box.box_center[1], body_box.box_center[2]};

 	body_points.each_col() -= body_center;

 	arma::Mat<double> body_points_T = M_body_T*body_points;

 	arma::Mat<double> seg_x1;
 	arma::Mat<double> seg_x2;
 	arma::Mat<double> seg_y1;
 	arma::Mat<double> seg_y2;

 	seg_x1 = body_points_T.cols(arma::find(body_points_T.row(0)<((1.0/2.0)*body_box.box_corners[0][0])));
 	seg_x2 = body_points_T.cols(arma::find(body_points_T.row(0)>((1.0/2.0)*body_box.box_corners[0][2])));
 	seg_y1 = body_points_T.cols(arma::find(body_points_T.row(1)<((1.0/4.0)*body_box.box_corners[1][0])));
 	seg_y2 = body_points_T.cols(arma::find(body_points_T.row(1)>((1.0/4.0)*body_box.box_corners[1][4])));

 	arma::Col<double> cg_x1 = arma::mean(seg_x1,1);
 	arma::Col<double> cg_x2 = arma::mean(seg_x2,1);
 	arma::Col<double> cg_y1 = arma::mean(seg_y1,1);
 	arma::Col<double> cg_y2 = arma::mean(seg_y2,1);

 	double Ix_1 = 0.0;
 	double Ix_2 = 0.0;
 	double Iy_1 = 0.0;
 	double Iy_2 = 0.0;

 	for (int i=0; i<seg_x1.n_cols; i++) {
 		Ix_1 = Ix_1+pow(seg_x1(1,i)-cg_x1(1),2.0)+pow(seg_x1(2,i)-cg_x1(2),2.0);
 	}

 	for (int i=0; i<seg_x2.n_cols; i++) {
 		Ix_2 = Ix_2+pow(seg_x2(1,i)-cg_x2(1),2.0)+pow(seg_x2(2,i)-cg_x2(2),2.0);
 	}

	for (int i=0; i<seg_y1.n_cols; i++) {
 		Iy_1 = Iy_1+pow(seg_y1(1,i),2.0);
 	}

 	for (int i=0; i<seg_y2.n_cols; i++) {
 		Iy_2 = Iy_2+pow(seg_y2(1,i),2.0);
 	}

 	//cout << M_body << endl;

 	if (Ix_1 > Ix_2) {
 		// Abdomen is located on negative x-axis: switch y and z axes
 		M_body = {{M_body(0,0),M_body(0,2),M_body(0,1),M_body(0,3)},
 				{M_body(1,0),M_body(1,2),M_body(1,1),M_body(1,3)},
 				{M_body(2,0),M_body(2,2),M_body(2,1),M_body(2,3)},
 				{M_body(3,0),M_body(3,2),M_body(3,1),M_body(3,3)}};
 	}
 	else if (Ix_1 < Ix_2) {
 		// Abdomen is located on the positive x-axis: rotate by 180 degrees and switch y and z axes
 		M_body = {{-M_body(0,0),M_body(0,2),M_body(0,1),M_body(0,3)},
 				{-M_body(1,0),M_body(1,2),M_body(1,1),M_body(1,3)},
 				{-M_body(2,0),M_body(2,2),M_body(2,1),M_body(2,3)},
 				{-M_body(3,0),M_body(3,2),M_body(3,1),M_body(3,3)}};
 	}
 	else {
 		// Don't know where the abdomen is located: switch y and z axes
 		M_body = {{M_body(0,0),M_body(0,2),M_body(0,1),M_body(0,3)},
 				{M_body(1,0),M_body(1,2),M_body(1,1),M_body(1,3)},
 				{M_body(2,0),M_body(2,2),M_body(2,1),M_body(2,3)},
 				{M_body(3,0),M_body(3,2),M_body(3,1),M_body(3,3)}};
 	}

 	if (Iy_1 > Iy_2) {
 		// c.g. is located at the bottom, z-axis is oriented correctly
 	}
 	else if (Iy_1 < Iy_2) {
 		// c.g is located at the top, rotate 180 degrees around x-axis
 		M_body = {{M_body(0,0),M_body(0,1),-M_body(0,2),M_body(0,3)},
 				{M_body(1,0),M_body(1,1),-M_body(1,2),M_body(1,3)},
 				{M_body(2,0),M_body(2,1),-M_body(2,2),M_body(2,3)},
 				{M_body(3,0),M_body(3,1),-M_body(3,2),M_body(3,3)}};
 	}
 	else {
 		// Don't know where the c.g. is located: keep current M_body
 	}

 	arma::Col<double> x_axis = {M_body(0,0),M_body(1,0),M_body(2,0)};
 	arma::Col<double> z_axis = {M_body(0,2),M_body(1,2),M_body(2,2)};

 	arma::Col<double> y_axis_M = {M_body(0,1),M_body(1,1),M_body(2,1)};
 	arma::Col<double> y_axis_cross = arma::cross(x_axis,z_axis);

 	double y_dot = arma::dot(y_axis_M,y_axis_cross)/(arma::norm(y_axis_M,2)*arma::norm(y_axis_cross,2));

 	if (y_dot > 0.5) {
 		// Switch sign y-axis:
 		M_body = {{M_body(0,0),-M_body(0,1),M_body(0,2),M_body(0,3)},
 				{M_body(1,0),-M_body(1,1),M_body(1,2),M_body(1,3)},
 				{M_body(2,0),-M_body(2,1),M_body(2,2),M_body(2,3)},
 				{M_body(3,0),-M_body(3,1),M_body(3,2),M_body(3,3)}};
 	}

	return M_body;
}

arma::Mat<double> InitState::FindRootTip(arma::Mat<double> M_body, struct bbox &seg_box) {

	arma::Mat<double> rot_body_T = {{M_body(0,0),M_body(1,0),M_body(2,0),0.0},
			{M_body(0,1),M_body(1,1),M_body(2,1),0.0},
			{M_body(0,2),M_body(1,2),M_body(2,2),0.0}};

	arma::Mat<double> rot_mat = {{seg_box.R_box[0][0], seg_box.R_box[0][1], seg_box.R_box[0][2], seg_box.box_center[0]},
			{seg_box.R_box[1][0], seg_box.R_box[1][1], seg_box.R_box[1][2], seg_box.box_center[1]},
			{seg_box.R_box[2][0], seg_box.R_box[2][1], seg_box.R_box[2][2], seg_box.box_center[2]},
			{0.0, 0.0, 0.0, 1.0}};

	arma::Col<double> body_center = {M_body(0,3), M_body(1,3), M_body(2,3), 1.0};

	arma::Mat<double> corners_seg(4,8);
	arma::Row<double> corner_dist(8);

	for (int i=0; i<8; i++) {
		arma::Col<double> temp_corner = {seg_box.box_corners[0][i], seg_box.box_corners[1][i], seg_box.box_corners[2][i], 1.0};
		corners_seg.col(i) = rot_mat*temp_corner;
		corner_dist(i) = sqrt(pow(corners_seg(0,i)-body_center(0),2.0)+pow(corners_seg(1,i)-body_center(1),2.0)+pow(corners_seg(2,i)-body_center(2),2.0));
	}

	// Select the 4 corners which are closest to the body center and the 4 corners which are the furthest away:

	arma::uvec c_sorted = arma::sort_index(corner_dist);

	arma::Mat<double> c_points(4,12);

	// Sorted corners + distances

	c_points(0,0) = corners_seg(0,c_sorted(0));
	c_points(1,0) = corners_seg(1,c_sorted(0));
	c_points(2,0) = corners_seg(2,c_sorted(0));
	c_points(3,0) = corner_dist(c_sorted(0));

	c_points(0,1) = corners_seg(0,c_sorted(1));
	c_points(1,1) = corners_seg(1,c_sorted(1));
	c_points(2,1) = corners_seg(2,c_sorted(1));
	c_points(3,1) = corner_dist(c_sorted(1));

	c_points(0,2) = corners_seg(0,c_sorted(2));
	c_points(1,2) = corners_seg(1,c_sorted(2));
	c_points(2,2) = corners_seg(2,c_sorted(2));
	c_points(3,2) = corner_dist(c_sorted(2));

	c_points(0,3) = corners_seg(0,c_sorted(3));
	c_points(1,3) = corners_seg(1,c_sorted(3));
	c_points(2,3) = corners_seg(2,c_sorted(3));
	c_points(3,3) = corner_dist(c_sorted(3));

	c_points(0,4) = corners_seg(0,c_sorted(4));
	c_points(1,4) = corners_seg(1,c_sorted(4));
	c_points(2,4) = corners_seg(2,c_sorted(4));
	c_points(3,4) = corner_dist(c_sorted(4));

	c_points(0,5) = corners_seg(0,c_sorted(5));
	c_points(1,5) = corners_seg(1,c_sorted(5));
	c_points(2,5) = corners_seg(2,c_sorted(5));
	c_points(3,5) = corner_dist(c_sorted(5));

	c_points(0,6) = corners_seg(0,c_sorted(6));
	c_points(1,6) = corners_seg(1,c_sorted(6));
	c_points(2,6) = corners_seg(2,c_sorted(6));
	c_points(3,6) = corner_dist(c_sorted(6));

	c_points(0,7) = corners_seg(0,c_sorted(7));
	c_points(1,7) = corners_seg(1,c_sorted(7));
	c_points(2,7) = corners_seg(2,c_sorted(7));
	c_points(3,7) = corner_dist(c_sorted(7));

	// root point

	c_points(0,8) = 0.25*(corners_seg(0,c_sorted(0))+corners_seg(0,c_sorted(1))+corners_seg(0,c_sorted(2))+corners_seg(0,c_sorted(3)))-body_center(0);
	c_points(1,8) = 0.25*(corners_seg(1,c_sorted(0))+corners_seg(1,c_sorted(1))+corners_seg(1,c_sorted(2))+corners_seg(1,c_sorted(3)))-body_center(1);
	c_points(2,8) = 0.25*(corners_seg(2,c_sorted(0))+corners_seg(2,c_sorted(1))+corners_seg(2,c_sorted(2))+corners_seg(2,c_sorted(3)))-body_center(2);
	c_points(3,8) = sqrt(pow(c_points(0,8),2.0)+pow(c_points(1,8),2.0)+pow(c_points(2,8),2.0));

	// tip point

	c_points(0,9) = 0.25*(corners_seg(0,c_sorted(4))+corners_seg(0,c_sorted(5))+corners_seg(0,c_sorted(6))+corners_seg(0,c_sorted(7)))-body_center(0);
	c_points(1,9) = 0.25*(corners_seg(1,c_sorted(4))+corners_seg(1,c_sorted(5))+corners_seg(1,c_sorted(6))+corners_seg(1,c_sorted(7)))-body_center(1);
	c_points(2,9) = 0.25*(corners_seg(2,c_sorted(4))+corners_seg(2,c_sorted(5))+corners_seg(2,c_sorted(6))+corners_seg(2,c_sorted(7)))-body_center(2);
	c_points(3,9) = sqrt(pow(c_points(0,9),2.0)+pow(c_points(1,9),2.0)+pow(c_points(2,9),2.0));

	// angular orientation w.r.t. body ref frame and volume
	arma::Col<double> tip_body_ref = rot_body_T*c_points.col(9);
	double xsi = atan2(tip_body_ref(1),tip_body_ref(2));
	double theta = atan2(tip_body_ref(0),sqrt(pow(tip_body_ref(1),2)+pow(tip_body_ref(2),2)));
	double seg_L = sqrt(pow((c_points(0,9)-c_points(0,8)),2)+pow((c_points(1,9)-c_points(1,8)),2)+pow((c_points(2,9)-c_points(2,8)),2));
	c_points(0,10) = xsi;
	c_points(1,10) = theta;
	c_points(2,10) = seg_L;
	c_points(3,10) = seg_box.volume;

	// center point
	c_points(0,11) = seg_box.box_center[0]-body_center(0);
	c_points(1,11) = seg_box.box_center[1]-body_center(1);
	c_points(2,11) = seg_box.box_center[2]-body_center(2);
	c_points(3,11) = sqrt(pow(c_points(0,11),2.0)+pow(c_points(1,11),2.0)+pow(c_points(2,11),2.0));

	// Project a point one wing length away from the wing tip along the y-axis of the

	return c_points;
}

vector<tuple<struct bbox, arma::Mat<double>>> InitState::FindBoundingBoxes(arma::Mat<double> &pcl_in) {

	// Find the bounding boxes for all the bounding boxes in the image:
	arma::Row<double> pcl_intensity = pcl_in.row(0);
	arma::Row<double> segment_id = arma::unique(pcl_intensity);

	double body_id = segment_id.min();

	int N_segments = segment_id.n_cols;

	arma::Mat<double> body_pcl;
	struct bbox body_box;
	vector<arma::Mat<double>> segment_pcl;
	arma::Mat<double> seg_pcl;
	vector<struct bbox> segment_box;

	for (int i=0; i<N_segments; i++) {
		if (segment_id(i)==body_id) {
			arma::uvec seg_indices = arma::find(pcl_intensity == segment_id(i));
			body_pcl = pcl_in.cols(seg_indices);
			body_box = InitState::BoundingBox(body_pcl);
		}
		else {
			arma::uvec seg_indices = arma::find(pcl_intensity == segment_id(i));
			if (seg_indices.n_rows > 10) {
				seg_pcl = pcl_in.cols(seg_indices);
				segment_pcl.push_back(seg_pcl);
				segment_box.push_back(InitState::BoundingBox(seg_pcl));
			}
		}
	}

	// Return bounding boxes with body bounding box as the first item:
	vector<tuple<struct bbox, arma::Mat<double>>> bbox_list;
	for (int j=0; j<(segment_box.size()+1); j++) {
		if (j==0) {
			bbox_list.push_back(make_tuple(body_box,body_pcl));
		}
		else {
			bbox_list.push_back(make_tuple(segment_box[j-1],segment_pcl[j-1]));
		}
	}

	return bbox_list;
}

bbox InitState::BoundingBox(arma::Mat<double> &pcl_in) {

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>());
	*cloud_in = InitState::Convert_Mat_2_PCL_XYZ(pcl_in);

	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
  	feature_extractor.setInputCloud (cloud_in);
  	feature_extractor.compute ();

  	// Oriented Bounding Box properties

  	vector<float> moment_of_inertia;
  	vector<float> eccentricity;
	pcl::PointXYZ min_point_OBB;
	pcl::PointXYZ max_point_OBB;
	pcl::PointXYZ position_OBB;
	Eigen::Matrix3f rotational_matrix_OBB;
  	float major_value, middle_value, minor_value;
  	Eigen::Vector3f major_vector, middle_vector, minor_vector;
  	Eigen::Vector3f mass_center;

  	feature_extractor.getMomentOfInertia(moment_of_inertia);
	feature_extractor.getEccentricity(eccentricity);
	feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
	feature_extractor.getEigenValues(major_value, middle_value, minor_value);
	feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
	feature_extractor.getMassCenter(mass_center);

	Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
  	Eigen::Quaternionf quat(rotational_matrix_OBB);

  	// Return bounding box structure

  	struct bbox bounding_box;

  	bounding_box.box_center[0] = (double) position_OBB.x;
  	bounding_box.box_center[1] = (double) position_OBB.y;
  	bounding_box.box_center[2] = (double) position_OBB.z;
  	bounding_box.box_corners[0][0] = (double) min_point_OBB.x;
  	bounding_box.box_corners[1][0] = (double) min_point_OBB.y;
  	bounding_box.box_corners[2][0] = (double) min_point_OBB.z;
	bounding_box.box_corners[0][1] = (double) min_point_OBB.x;
  	bounding_box.box_corners[1][1] = (double) min_point_OBB.y;
  	bounding_box.box_corners[2][1] = (double) max_point_OBB.z;
  	bounding_box.box_corners[0][2] = (double) max_point_OBB.x;
  	bounding_box.box_corners[1][2] = (double) min_point_OBB.y;
  	bounding_box.box_corners[2][2] = (double) max_point_OBB.z;
  	bounding_box.box_corners[0][3] = (double) max_point_OBB.x;
  	bounding_box.box_corners[1][3] = (double) min_point_OBB.y;
  	bounding_box.box_corners[2][3] = (double) min_point_OBB.z;
  	bounding_box.box_corners[0][4] = (double) min_point_OBB.x;
  	bounding_box.box_corners[1][4] = (double) max_point_OBB.y;
  	bounding_box.box_corners[2][4] = (double) min_point_OBB.z;
  	bounding_box.box_corners[0][5] = (double) min_point_OBB.x;
  	bounding_box.box_corners[1][5] = (double) max_point_OBB.y;
  	bounding_box.box_corners[2][5] = (double) max_point_OBB.z;
  	bounding_box.box_corners[0][6] = (double) max_point_OBB.x;
  	bounding_box.box_corners[1][6] = (double) max_point_OBB.y;
  	bounding_box.box_corners[2][6] = (double) max_point_OBB.z;
  	bounding_box.box_corners[0][7] = (double) max_point_OBB.x;
  	bounding_box.box_corners[1][7] = (double) max_point_OBB.y;
  	bounding_box.box_corners[2][7] = (double) min_point_OBB.z;
	bounding_box.q_box[0] = (double) quat.w();
	bounding_box.q_box[1] = (double) quat.x();
	bounding_box.q_box[2] = (double) quat.y();
	bounding_box.q_box[3] = (double) quat.z();
	bounding_box.R_box[0][0] = (double) rotational_matrix_OBB(0,0);
	bounding_box.R_box[0][1] = (double) rotational_matrix_OBB(0,1);
	bounding_box.R_box[0][2] = (double) rotational_matrix_OBB(0,2);
	bounding_box.R_box[1][0] = (double) rotational_matrix_OBB(1,0);
	bounding_box.R_box[1][1] = (double) rotational_matrix_OBB(1,1);
	bounding_box.R_box[1][2] = (double) rotational_matrix_OBB(1,2);
	bounding_box.R_box[2][0] = (double) rotational_matrix_OBB(2,0);
	bounding_box.R_box[2][1] = (double) rotational_matrix_OBB(2,1);
	bounding_box.R_box[2][2] = (double) rotational_matrix_OBB(2,2);
	bounding_box.mass_center[0] = (double)  mass_center(0);
	bounding_box.mass_center[1] = (double)  mass_center(1);
	bounding_box.mass_center[2] = (double)  mass_center(2);
	bounding_box.eigen_values[0] = (double) major_value;
	bounding_box.eigen_values[1] = (double) middle_value;
	bounding_box.eigen_values[2] = (double) minor_value;
	bounding_box.eigen_vectors[0][0] = (double) major_vector(0);
	bounding_box.eigen_vectors[0][1] = (double) major_vector(1);
	bounding_box.eigen_vectors[0][2] = (double) major_vector(2);
	bounding_box.eigen_vectors[1][0] = (double) middle_vector(0);
	bounding_box.eigen_vectors[1][1] = (double) middle_vector(1);
	bounding_box.eigen_vectors[1][2] = (double) middle_vector(2);
	bounding_box.eigen_vectors[2][0] = (double) minor_vector(0);
	bounding_box.eigen_vectors[2][1] = (double) minor_vector(1);
	bounding_box.eigen_vectors[2][2] = (double) minor_vector(2);
	bounding_box.moment_of_inertia[0] = (double) moment_of_inertia[0];
	bounding_box.moment_of_inertia[1] = (double) moment_of_inertia[1];
	bounding_box.moment_of_inertia[2] = (double) moment_of_inertia[2];
	bounding_box.eccentricity[0] = (double) eccentricity[0];
	bounding_box.eccentricity[1] = (double) eccentricity[1];
	bounding_box.eccentricity[2] = (double) eccentricity[2];
	bounding_box.volume = (max_point_OBB.x-min_point_OBB.x)*(max_point_OBB.y-min_point_OBB.y)*(max_point_OBB.z-min_point_OBB.z);

	return bounding_box;
}

np::ndarray InitState::ReturnBBoxSinglePCL(frames &frame_in) {

	arma::Mat<double> pcl_in = InitState::ConvertVectorPCLArma(frame_in.single_frame_pcl);

	vector<tuple<struct bbox, arma::Mat<double>>> bbox_list = InitState::FindBoundingBoxes(pcl_in);

	int N_boxes = bbox_list.size();

	p::tuple shape = p::make_tuple(24,N_boxes);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_boxes; i++) {
		arma::Mat<double> rot_mat = {{get<0>(bbox_list[i]).R_box[0][0], get<0>(bbox_list[i]).R_box[0][1], get<0>(bbox_list[i]).R_box[0][2], get<0>(bbox_list[i]).box_center[0]},
			{get<0>(bbox_list[i]).R_box[1][0], get<0>(bbox_list[i]).R_box[1][1], get<0>(bbox_list[i]).R_box[1][2], get<0>(bbox_list[i]).box_center[1]},
			{get<0>(bbox_list[i]).R_box[2][0], get<0>(bbox_list[i]).R_box[2][1], get<0>(bbox_list[i]).R_box[2][2], get<0>(bbox_list[i]).box_center[2]},
			{0.0, 0.0, 0.0, 1.0}};
		for (int j=0; j<8; j++) {
			arma::Col<double> corner_point = {get<0>(bbox_list[i]).box_corners[0][j], get<0>(bbox_list[i]).box_corners[1][j],
				get<0>(bbox_list[i]).box_corners[2][j], 1.0};
			arma::Col<double> new_corner_point = rot_mat*corner_point;

			array_out[j*3][i] = new_corner_point(0);
			array_out[j*3+1][i] = new_corner_point(1);
			array_out[j*3+2][i] = new_corner_point(2);
		}
	}

	return array_out;
}

np::ndarray InitState::ReturnSinglePCL(frames &frame_in) {

	vector<tuple<int,double,double,double,double,double,double>> pcl_out = frame_in.single_frame_pcl;

	int N_points = pcl_out.size();

	p::tuple shape = p::make_tuple(7,N_points);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_points; i++) {
		array_out[0][i] = get<0>(pcl_out[i]);
		array_out[1][i] = get<1>(pcl_out[i]);
		array_out[2][i] = get<2>(pcl_out[i]);
		array_out[3][i] = get<3>(pcl_out[i]);
		array_out[4][i] = get<4>(pcl_out[i]);
		array_out[5][i] = get<5>(pcl_out[i]);
		array_out[6][i] = get<6>(pcl_out[i]);
	}

	return array_out;
}

np::ndarray InitState::ReturnBBoxBLR(frames &frame_in) {

	p::tuple shape = p::make_tuple(24,3);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<3; i++) {
		arma::Mat<double> pcl_now = frame_in.pcl_init_single_frame[i];

		if (pcl_now.n_cols>1) {

			struct bbox bbox_now = InitState::BoundingBox(pcl_now);

			arma::Mat<double> rot_mat = {{bbox_now.R_box[0][0], bbox_now.R_box[0][1],bbox_now.R_box[0][2], bbox_now.box_center[0]},
				{bbox_now.R_box[1][0], bbox_now.R_box[1][1], bbox_now.R_box[1][2], bbox_now.box_center[1]},
				{bbox_now.R_box[2][0], bbox_now.R_box[2][1], bbox_now.R_box[2][2], bbox_now.box_center[2]},
				{0.0, 0.0, 0.0, 1.0}};

			for (int j=0; j<8; j++) {
				arma::Col<double> corner_point = {bbox_now.box_corners[0][j],bbox_now.box_corners[1][j],bbox_now.box_corners[2][j], 1.0};
				arma::Col<double> new_corner_point = rot_mat*corner_point;

				array_out[j*3][i] = new_corner_point(0);
				array_out[j*3+1][i] = new_corner_point(1);
				array_out[j*3+2][i] = new_corner_point(2);
			}

		}
		else {
			for (int j=0; j<8; j++) {
				array_out[j*3][i] = 0.0;
				array_out[j*3+1][i] = 0.0;
				array_out[j*3+2][i] = 0.0;
			}
		}
	}

	return array_out;
}

np::ndarray InitState::ReturnBLRPCL(frames &frame_in) {

	arma::Mat<double> pcl_body = frame_in.pcl_init_single_frame[0];
	arma::Mat<double> pcl_wing_L = frame_in.pcl_init_single_frame[1];
	arma::Mat<double> pcl_wing_R = frame_in.pcl_init_single_frame[2];

	int N_points_body = pcl_body.n_cols;
	int N_points_wing_L = pcl_wing_L.n_cols;
	int N_points_wing_R = pcl_wing_R.n_cols;

	if (N_points_body < 2) {
		N_points_body = 0;
	}
	if (N_points_wing_L < 2) {
		N_points_wing_L = 0;
	}
	if (N_points_wing_R < 2) {
		N_points_wing_R = 0;
	}

	p::tuple shape = p::make_tuple(7,N_points_body+N_points_wing_L+N_points_wing_R);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_points_body; i++) {
		array_out[0][i] = pcl_body(0,i);
		array_out[1][i] = pcl_body(1,i);
		array_out[2][i] = pcl_body(2,i);
		array_out[3][i] = pcl_body(3,i);
		array_out[4][i] = pcl_body(4,i);
		array_out[5][i] = pcl_body(5,i);
		array_out[6][i] = pcl_body(6,i);
	}

	for (int j=0; j<N_points_wing_L; j++) {
		array_out[0][j+N_points_body] = pcl_wing_L(0,j);
		array_out[1][j+N_points_body] = pcl_wing_L(1,j);
		array_out[2][j+N_points_body] = pcl_wing_L(2,j);
		array_out[3][j+N_points_body] = pcl_wing_L(3,j);
		array_out[4][j+N_points_body] = pcl_wing_L(4,j);
		array_out[5][j+N_points_body] = pcl_wing_L(5,j);
		array_out[6][j+N_points_body] = pcl_wing_L(6,j);
	}

	for (int k=0; k<N_points_wing_R; k++) {
		array_out[0][k+N_points_body+N_points_wing_L] = pcl_wing_R(0,k);
		array_out[1][k+N_points_body+N_points_wing_L] = pcl_wing_R(1,k);
		array_out[2][k+N_points_body+N_points_wing_L] = pcl_wing_R(2,k);
		array_out[3][k+N_points_body+N_points_wing_L] = pcl_wing_R(3,k);
		array_out[4][k+N_points_body+N_points_wing_L] = pcl_wing_R(4,k);
		array_out[5][k+N_points_body+N_points_wing_L] = pcl_wing_R(5,k);
		array_out[6][k+N_points_body+N_points_wing_L] = pcl_wing_R(6,k);
	}

	return array_out;

}

np::ndarray InitState::ReturnMBody(frames &frame_in) {

	arma::Mat<double> M_body = frame_in.M_init_single_frame[0];

	p::tuple shape = p::make_tuple(4,4);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	if (M_body(3,3)==1.0) {
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				array_out[i][j] = M_body(i,j);
			}
		}
	}
	else {
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				if (i==j) {
					array_out[i][j] = 1.0;
				}
				else {
					array_out[i][j] = 0.0;
				}
			}
		}

	}

	return array_out;
}

np::ndarray InitState::ReturnMWingL(frames &frame_in) {

	arma::Mat<double> M_wing_L = frame_in.M_init_single_frame[1];

	p::tuple shape = p::make_tuple(4,4);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	if (M_wing_L(3,3)==1.0) {
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				array_out[i][j] = M_wing_L(i,j);
			}
		}
	}

	return array_out;
}

np::ndarray InitState::ReturnMWingR(frames &frame_in) {

	arma::Mat<double> M_wing_R = frame_in.M_init_single_frame[2];

	p::tuple shape = p::make_tuple(4,4);
	np::dtype dtype = np::dtype::get_builtin<double>();
	np::ndarray array_out = np::zeros(shape,dtype);

	if (M_wing_R(3,3)==1.0) {
		for (int i=0; i<4; i++) {
			for (int j=0; j<4; j++) {
				array_out[i][j] = M_wing_R(i,j);
			}
		}
	}

	return array_out;
}

pcl::PointCloud<pcl::PointXYZ> InitState::Convert_Mat_2_PCL_XYZ(arma::Mat<double> &pcl_mat) {

	// Convert an armadillo matrix to a PointCloud vector

	pcl::PointCloud<pcl::PointXYZ> pcl_vec;

	int N_points = pcl_mat.n_cols;

	for (int i=0; i<N_points; i++) {
		pcl::PointXYZ point;
		point.x = pcl_mat(1,i);
		point.y = pcl_mat(2,i);
		point.z = pcl_mat(3,i);

		pcl_vec.push_back(point);
	}

	return pcl_vec;
}

arma::Mat<double> InitState::ConvertVectorPCLArma(vector<tuple<int,double,double,double,double,double,double>> &pcl_in) {

	int N_points = pcl_in.size();

	arma::Mat<double> pcl_out(7,N_points);

	for (int i=0; i<N_points; i++) {
		pcl_out(0,i) = (double) get<0>(pcl_in[i]);
		pcl_out(1,i) = get<1>(pcl_in[i]);
		pcl_out(2,i) = get<2>(pcl_in[i]);
		pcl_out(3,i) = get<3>(pcl_in[i]);
		pcl_out(4,i) = get<4>(pcl_in[i]);
		pcl_out(5,i) = get<5>(pcl_in[i]);
		pcl_out(6,i) = get<6>(pcl_in[i]);
	}

	return pcl_out;
}


/******************************************************************************

  Implementation of the complex propagation technique used in surface
  reconstruction.

  Copyright (c) 2012, 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef COMPLEX_PROPAGATOR_HPP_D90ED351_6A45_4523_85F3_DA99F52B87C2
#define COMPLEX_PROPAGATOR_HPP_D90ED351_6A45_4523_85F3_DA99F52B87C2

#include <vector>
#include <algorithm>
#include <functional>
#include <boost/assert.hpp>
#include <boost/noncopyable.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "bo/core/raw_image_2d.hpp"
#include "bo/core/mesh.hpp"
#include "bo/core/kdtree.hpp"
#include "bo/math/functions.hpp"
#include "bo/math/pca.hpp"
#include "bo/math/mean.hpp"
#include "bo/distances/distances_3d.hpp"
#include "bo/surfaces/detail/total_propagation.hpp"
#include "bo/surfaces/detail/propagation_result.hpp"
#include "bo/surfaces/detail/arched_strip.hpp"

namespace bo {
namespace surfaces {

template <typename RealType>
class ComplexPropagator: public boost::noncopyable
{
public:
    typedef ComplexPropagator<RealType> SelfType;
    typedef boost::shared_ptr<SelfType> Ptr;
    typedef std::vector<Ptr> Ptrs;

    typedef Vector<RealType, 3> Point3D;

    // Types for input data.
    typedef std::vector<Point3D> Points3D;
    typedef boost::shared_ptr<Points3D> Points3DPtr;
    typedef boost::shared_ptr<const Points3D> Points3DConstPtr;
    typedef std::vector<Points3DConstPtr> Points3DConstPtrs;

    typedef std::vector<RealType> Weights;

    // Used by factory functions.
    typedef RawImage2D<RealType> Image2D;
    typedef bo::Mesh<RealType> Mesh;

private:
    typedef std::pointer_to_binary_function<const Point3D&, const Point3D&, RealType> Metric;
    typedef KDTree<3, Point3D, std::pointer_to_binary_function<const Point3D&,
            std::size_t, RealType> > Tree;
    typedef boost::shared_ptr<Tree> TreePtr;
    typedef std::vector<TreePtr> TreePtrs;

    typedef detail::TotalPropagation<RealType, Tree> PropagationDirector;
    typedef detail::PropagationResult<RealType> PropagationResult;
    typedef typename PropagationResult::PropagatedContour PropagatedContour;
    typedef boost::shared_ptr<PropagatedContour> PropagatedContourPtr;

public:
    // Factory functions. Create an instance of the class from either of the supported
    // input with the provided parameters.
    static Ptr create(const Points3D& plane, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius);

    static Ptrs create(const Points3DConstPtrs& planes, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius);

    static Ptr from_mesh(const Mesh& mesh, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius);

    static Ptr from_raw_image(const Image2D& data, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius);

    // A special factory. Creates a set of instances one per given plane. Additionally
    // passes a set of neighbours with corresponding weights to each instance. These
    // planes are used later in the calculation of the tangential component of the
    // propagation, making it dependent of the neighbour planes. This may improve the result.
    static Ptrs create(const Points3DConstPtrs& planes, const Weights& neighbour_weights,
        RealType delta_min, RealType delta_max, RealType inertial_weight,
        RealType centrifugal_weight, RealType tangential_radius);

    // Performs propagation in either one or two steps depending on whether the
    // contour has a hole.
    void propagate();

    // Accessor functions to the final contour and algorithm markers. Calling these
    // functions makes sense only after calling propagate(). If is_aborted() marker
    // is set to true, the algortihm is most probably stuck (e.g. jumping between two
    // adjacent points or traversing the cosed contour multiple times).
    bool is_aborted() const;
    bool has_hole() const;
    bool is_closed() const;
    PropagatedContourPtr contour() const;

protected:
    ComplexPropagator(RealType delta_min, RealType delta_max, TreePtr tree_ptr,
                       const PropagationDirector& director, const Point3D& start_point);

    // Helper function for KDTree instance.
    static RealType point3D_accessor_(const Point3D& pt, std::size_t k);

    // Helper function creating k-d tree shared_ptr from the given container of points.
    static TreePtr tree_from_plane_(const Points3D& plane);

    // Calculates neighbours and corresponding weights for the given plane. Uses the
    // size of the weights container to determine how many planes "before" and "after"
    // the current should be considered neighbours.
    static void populate_neighbours_and_weights_(std::size_t plane_idx,
            const Points3DConstPtrs& planes, const Weights& neighbour_weights,
            Points3DConstPtrs& out_neighbours, Weights& out_weights);

    // Calculates plane normal for the given container of samples. The samples are
    // assumed to lie in a plane. PCA is used to estimate the vector purpendicular to
    // plane represented by samples.
    static Point3D get_plane_normal_(const Points3D& plane);

    // Projects each of the given points onto the plane represented by its normal and
    // center of mass.
    static Points3DConstPtrs project_neighbour_onto_plane_(const Point3D& center_of_mass,
            const Point3D& normal, const Points3DConstPtrs& neighbours);

    // Tries performing propagation from the start point to the end point.
    // Note that end point is not included in result, while start is always included.
    PropagationResult propagate_(const Point3D& start, const Point3D& end,
                                 Point3D total_prop, std::size_t max_size);

private:
    // Parameters and options of the propagation algorithm.
    const RealType delta_min_;
    const RealType delta_max_;
    const Metric metric_;

    // Algorithm data and helper obejcts.
    const TreePtr tree_ptr_;
    const PropagationDirector propagation_director_;
    const Point3D start_;

    // Markers and final contour.
    bool aborted_;
    bool has_hole_;
    PropagatedContourPtr contour_;
};

// C-tor.
template <typename RealType>
ComplexPropagator<RealType>::ComplexPropagator(RealType delta_min,
                                                 RealType delta_max,
                                                 TreePtr tree_ptr,
                                                 const PropagationDirector& director,
                                                 const Point3D& start_point):
    delta_min_(delta_min), delta_max_(delta_max),
    metric_(std::ptr_fun(distances::euclidean_distance<RealType, 3>)),
    tree_ptr_(tree_ptr),
    propagation_director_(director),
    start_(start_point),
    aborted_(false), has_hole_(false),
    contour_(boost::make_shared<PropagatedContour>())
{
    // If points number is less than 2, propagation cannot be initialized. And
    // because there is no sense in doing propagation for 0 or 1 points, we throw
    // an error instead of returning the plane unchanged.
    if (tree_ptr->size() < 2)
        throw std::logic_error("Cannot run propagation for planes consisting of "
                               "less than 2 vertices.");
}

// Factories. Note that the sum of inertial and centrifugal weights should be in [0; 1].
template <typename RealType>
typename ComplexPropagator<RealType>::Ptr ComplexPropagator<RealType>::create(
        const Points3D& plane, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius)
{
    // Build kd-tree from the given points.
    TreePtr tree_ptr = tree_from_plane_(plane);

    // Create the appropriate propagation director either with or without centrifugal
    // component.
    PropagationDirector direction = math::check_small(centrifugal_weight)
            ? PropagationDirector::create_simple(inertial_weight, tree_ptr,
                    tangential_radius)
            : PropagationDirector::create_with_centrifugal(inertial_weight,
                    centrifugal_weight, tree_ptr, tangential_radius, bo::math::mean(plane));

    // C-tor is declared private, using boost::make_shared gets complicated.
    Ptr ptr(new SelfType(delta_min, delta_max, tree_ptr, direction, plane.front()));
    return ptr;
}

template <typename RealType>
typename ComplexPropagator<RealType>::Ptrs ComplexPropagator<RealType>::create(
        const Points3DConstPtrs& planes, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius)
{
    // Cache values and prepare output container.
    std::size_t planes_count = planes.size();
    Ptrs propagators;
    propagators.reserve(planes_count);

    // Create an instance for each plane.
    for (std::size_t idx = 0; idx < planes_count; ++idx)
    {
        // Create an instance, register neighbours and return.
        Ptr propagator = create(*(planes[idx]), delta_min, delta_max, inertial_weight,
                                centrifugal_weight, tangential_radius);
        propagators.push_back(propagator);
    }

    return propagators;
}

template <typename RealType>
typename ComplexPropagator<RealType>::Ptr ComplexPropagator<RealType>::from_mesh(
        const Mesh& mesh, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius)
{
    return
        create(mesh.get_all_vertices(), delta_min, delta_max, inertial_weight,
               centrifugal_weight, tangential_radius);
}

template <typename RealType>
typename ComplexPropagator<RealType>::Ptr ComplexPropagator<RealType>::from_raw_image(
        const Image2D& data, RealType delta_min, RealType delta_max,
        RealType inertial_weight, RealType centrifugal_weight, RealType tangential_radius)
{
    // Load plane data from an istance of RawImage2D.
    Points3D plane;

    for (std::size_t row = 0; row < data.height(); ++row)
        for (std::size_t col = 0; col < data.width(); ++col)
            if (data(col, row) > 0)
                plane.push_back(Point3D(RealType(col), RealType(row), RealType(0)));

    return
        create(plane, delta_min, delta_max, inertial_weight, centrifugal_weight,
               tangential_radius);
}

template <typename RealType>
typename ComplexPropagator<RealType>::Ptrs ComplexPropagator<RealType>::create(
        const Points3DConstPtrs& planes, const Weights& neighbour_weights,
        RealType delta_min, RealType delta_max, RealType inertial_weight,
        RealType centrifugal_weight, RealType tangential_radius)
{
    // Check neighbours count (in one direction) is not greater than planes count - 1.
    if (neighbour_weights.size() > planes.size() - 1)
        throw std::logic_error("Neighbours count in one direction exceeds reasonable "
                               "limit.");

    // Cache values and prepare output container.
    std::size_t planes_count = planes.size();
    std::size_t radius = neighbour_weights.size();
    Ptrs propagators;
    propagators.reserve(planes_count);

    // Compute neighbours for each plane and create an instance of the class for it.
    for (std::size_t idx = 0; idx < planes_count; ++idx)
    {
        // Cache current plane and prepare output containers.
        Points3DConstPtr plane = planes[idx];

        Points3DConstPtrs neighbours;
        neighbours.reserve(2 * radius);

        Weights weights;
        weights.reserve(2 * radius);

        // Add neighbours and weights to the instantiated containers.
        populate_neighbours_and_weights_(idx, planes, neighbour_weights, neighbours, weights);

        // Cache main plane origin and normal.
        Point3D center_of_mass = bo::math::mean(*plane);
        Point3D norm = get_plane_normal_(*plane);

        // Project all neighbours to the current plane.
        Points3DConstPtrs neighbour_projs = project_neighbour_onto_plane_(center_of_mass,
                norm, neighbours);
        neighbour_projs.reserve(neighbours.size());

        // Compute k-d tree for the current plane.
        TreePtr main_tree_ptr = tree_from_plane_(*plane);

        // Compute k-d trees for projected neighbour planes.
        TreePtrs neighbour_trees;
        neighbour_trees.reserve(neighbour_projs.size());
        for (typename Points3DConstPtrs::const_iterator plane_it =
             neighbour_projs.begin(); plane_it != neighbour_projs.end(); ++plane_it)
            neighbour_trees.push_back(tree_from_plane_(**plane_it));

        // Create the appropriate propagation director with neighbour tangential
        // componentseither and with or without centrifugal component.
        PropagationDirector direction = math::check_small(centrifugal_weight)
                ? PropagationDirector::create_with_neighbours(inertial_weight,
                        main_tree_ptr, tangential_radius, neighbour_trees, weights)
                : PropagationDirector::create_with_neighbours_and_centrifugal(
                        inertial_weight, centrifugal_weight, main_tree_ptr, tangential_radius,
                        center_of_mass, neighbour_trees, weights);

        // C-tor is declared private, using boost::make_shared gets complicated.
        Ptr ptr(new SelfType(delta_min, delta_max, main_tree_ptr, direction, plane->front()));

        propagators.push_back(ptr);
    }

    return propagators;
}

// Public control functions.
template <typename RealType>
void ComplexPropagator<RealType>::propagate()
{
    // Choose initial point and initial propagation. It solely consists of the
    // tangential component, since inertial cannot be defined.
    Point3D initial_prop = propagation_director_.initial(start_);

    // Run propagation. It may bump into a hole or return a circuit.
    PropagationResult attempt1 = propagate_(start_, start_, initial_prop, 1000);

    // If the hole was detected, run propagation in a different direction.
    PropagationResult attempt2;
    if (attempt1.HasHole)
        attempt2 = propagate_(start_, attempt1.Points.back(), - initial_prop, 1000);

    // Glue propagation results together.
    contour_->reserve(attempt1.Points.size() + attempt2.Points.size());
    for (typename PropagatedContour::const_reverse_iterator rit = attempt2.Points.rbegin();
         rit != attempt2.Points.rend(); ++rit)
        contour_->push_back(*rit);
    for (typename PropagatedContour::const_iterator it = attempt1.Points.begin() + 1;
         it != attempt1.Points.end(); ++it)
        contour_->push_back(*it);

    // Set markers.
    aborted_ = attempt1.Aborted || attempt2.Aborted;
    has_hole_ = attempt1.HasHole;
}

template <typename RealType> inline
bool ComplexPropagator<RealType>::is_aborted() const
{
    return aborted_;
}

template <typename RealType> inline
bool ComplexPropagator<RealType>::has_hole() const
{
    return has_hole_;
}

template <typename RealType> inline
bool ComplexPropagator<RealType>::is_closed() const
{
    return !(this->has_hole());
}

template <typename RealType> inline
typename ComplexPropagator<RealType>::PropagatedContourPtr
ComplexPropagator<RealType>::contour() const
{
    return contour_;
}

// Private static helper functions.
template <typename RealType> inline
RealType ComplexPropagator<RealType>::point3D_accessor_(const Point3D &pt, std::size_t k)
{
    return pt[k];
}

template <typename RealType> inline
typename ComplexPropagator<RealType>::TreePtr
ComplexPropagator<RealType>::tree_from_plane_(const Points3D& plane)
{
    // Build kd-tree from the given points.
    // TODO: provide k-d tree with current metric?
    TreePtr tree_ptr = boost::make_shared<Tree>(plane.begin(), plane.end(),
                                                std::ptr_fun(point3D_accessor_));
    return tree_ptr;
}

template <typename RealType>
typename ComplexPropagator<RealType>::Point3D
ComplexPropagator<RealType>::get_plane_normal_(const Points3D& plane)
{
    // Employ PCA to estimate plane normal.
    typedef math::PCA<RealType, 3> PCAEngine;
    typedef typename PCAEngine::Result PCAResult;
    PCAEngine pca;
    PCAResult result = pca(plane);
    Point3D normal = result.template get<1>()[0];

    return normal;
}

template <typename RealType>
typename ComplexPropagator<RealType>::Points3DConstPtrs
ComplexPropagator<RealType>::project_neighbour_onto_plane_(const Point3D& center_of_mass,
        const Point3D& normal, const Points3DConstPtrs& neighbours)
{
    // Prepare output container.
    Points3DConstPtrs neighbour_projs;
    neighbour_projs.reserve(neighbours.size());

    // Project all neighbours to the current plane.
    for (typename Points3DConstPtrs::const_iterator neighbour =
         neighbours.begin(); neighbour != neighbours.end(); ++neighbour)
    {
        // Prepare container for the projected plane.
        Points3DPtr plane_proj = boost::make_shared<Points3D>();
        plane_proj->reserve((*neighbour)->size());

        // Project all points from the neighbour to the current plane.
        for (typename Points3D::const_iterator point_it = (*neighbour)->begin();
             point_it != (*neighbour)->end(); ++point_it)
        {
            plane_proj->push_back(distances::project_point_onto_plane(*point_it,
                    center_of_mass, normal));
        }

        neighbour_projs.push_back(plane_proj);
    }

    return neighbour_projs;
}

template <typename RealType>
void ComplexPropagator<RealType>::populate_neighbours_and_weights_(std::size_t plane_idx,
        const Points3DConstPtrs& planes, const Weights& neighbour_weights,
        Points3DConstPtrs& out_neighbours, Weights& out_weights)
{
    std::size_t planes_count = planes.size();
    std::size_t radius = neighbour_weights.size();

    // Try add neighbours before the current idx. We add radius items at most,
    // but less if no neighbours are available (close to container front).
    std::size_t neighbour_idx = plane_idx - 1;
    std::size_t added_count = 0;
    std::size_t maxval = std::size_t(-1); // dirty hack to workaround int behaviour for size_t.
    while ((neighbour_idx < maxval) && (added_count < radius))
    {
        out_neighbours.push_back(planes[neighbour_idx]);
        out_weights.push_back(neighbour_weights[added_count]);
        ++added_count;
        --neighbour_idx;
    }

    // Try add neighbours after the current idx. We add radius items at most,
    // but less if no neighbours are available (close to container end).
    neighbour_idx = plane_idx + 1;
    added_count = 0;
    while ((neighbour_idx < planes_count) && (added_count < radius))
    {
        out_neighbours.push_back(planes[neighbour_idx]);
        out_weights.push_back(neighbour_weights[added_count]);
        ++added_count;
        ++neighbour_idx;
    }

    BOOST_ASSERT((out_neighbours.size() == out_weights.size()) && "Weights quantity doesn't"
                 "correspond to planes number.");
}

template <typename RealType>
typename ComplexPropagator<RealType>::PropagationResult
ComplexPropagator<RealType>::propagate_(const Point3D& start, const Point3D& end,
                                         Point3D total_prop, std::size_t max_size)
{
    typedef detail::ArchedStrip<RealType, 3> ArchedStrip;

    PropagationResult retvalue;
    retvalue.Points.push_back(start);

    // Flag for so-called "smooth-ending". It is necessary to keep the distance
    // between the points in the end phase as close to delta_min as possible,
    // despite a delta_max point has been already found.
    bool end_detected = false;

    Point3D current = start;
    do
    {
        // Restrict total length (to prevent looping).
        if (retvalue.Points.size() > max_size)
        {
            retvalue.Aborted = true;
            break;
        }

        // Search for the propagation candidate. It should lie on the arced strip
        // bounded by delta_min and delta_max circumferences and a plane containing
        // current point and normal to propagation vector.
        Point3D phantom_candidate = current + total_prop * delta_min_;

        // TODO: get rid of max radius.
        std::pair<typename Tree::const_iterator, RealType> candidate_data =
                tree_ptr_->find_nearest_if(phantom_candidate, delta_max_,
                    ArchedStrip(current, delta_min_, delta_max_, total_prop, metric_));
        RealType candidate_distance = candidate_data.second;
        Point3D candidate = *(candidate_data.first);

        // Check if we bump into a hole.
        if (candidate_distance >= delta_max_)
        {
            retvalue.HasHole = true;
            break;
        }

        // Check if the candidate "sees" the end point "in front".
        if (!end_detected)
        {
            retvalue.Points.push_back(candidate);
            if ((delta_max_ >= metric_(end, candidate)) &&
                (total_prop * (end - candidate)) > 0)
                end_detected = true;
        }
        else
        {
            RealType cur_dist = (end - retvalue.Points.back()).euclidean_norm();
            RealType candidate_dist1 = (candidate - retvalue.Points.back()).euclidean_norm();
            RealType candidate_dist2 = (end - candidate).euclidean_norm();

            if (delta_min_ > metric_(end, candidate))
            {
                retvalue.Points.push_back(candidate);
                candidate = end;
            }
            else if ((cur_dist > candidate_dist1) && (cur_dist > candidate_dist2))
            {
                retvalue.Points.push_back(candidate);
            }
            else
            {
                candidate = end;
            }
        }

        // Update algortihm's state.
        Point3D previous = current;
        current = candidate;
        total_prop = propagation_director_.next(current, previous);

    } while (current != end);

    return retvalue;
}

} // namespace surfaces
} // namespace bo

#endif // COMPLEX_PROPAGATOR_HPP_D90ED351_6A45_4523_85F3_DA99F52B87C2