#pragma once
#define BOOST_CHRONO_HEADER_ONLY 1
#include <boost/chrono.hpp>
#include <string>
using namespace boost::chrono;

// DataType: variable type: double, int32_t, int64_t, etc.
// Ratio: std::ratio<a,b>
// Size: final value is mean of <Size> last samples
// example: Timer<double, 1, 1> measure time in second with floating precision
// Timer<int64_t, 1000, 1> measure time in miliseconds

template <typename DataType, long long int Base, int Size>
class Timer {
    // steady_clocksteady_clock::time_point t1;
    boost::chrono::high_resolution_clock::time_point t1;
    boost::chrono::high_resolution_clock::time_point t2;
    DataType array[Size] = {0};
    int cnt {0};
    DataType val;
    std::string del;
public:
    Timer(){
            t1 = boost::chrono::high_resolution_clock::now();
            t2 = boost::chrono::high_resolution_clock::now();
    }
    DataType operator ()(){
        t2 = boost::chrono::high_resolution_clock::now();
        duration<DataType, boost::ratio<1, Base>> delta = boost::chrono::duration_cast<duration<DataType, boost::ratio<1, Base>>>(t2 - t1);
        t1 = boost::chrono::high_resolution_clock::now();
        array[cnt] = delta.count();
        DataType sum = 0;
        for(auto it : array)
            sum += it;
        sum /= Size;

        val = (DataType)sum;
        cnt = (cnt + 1)%Size;
        return val;
    }

    std::string getString(){
        std::stringstream ss;
        ss<<val;
        return ss.str();
    }
    void start(){
        t1 = boost::chrono::high_resolution_clock::now();
    }
    void end(){
        t2 = boost::chrono::high_resolution_clock::now();
        duration<DataType, boost::ratio<1, Base>> delta = boost::chrono::duration_cast<duration<DataType, boost::ratio<1, Base>>>(t2 - t1);
        array[cnt] = delta.count();
        DataType sum = 0;
        for(auto it : array)
            sum += it;
        sum /= Size;

        val = (DataType)sum;
        cnt = (cnt + 1)%Size;
    }
    DataType get(){
        return val;
    }
};

template <typename DataType, long long int Base>
class Timer<DataType, Base, 1> {
    boost::chrono::high_resolution_clock::time_point t1;
    boost::chrono::high_resolution_clock::time_point t2;
    DataType val;
public:
    Timer(){
        t1 = boost::chrono::high_resolution_clock::now();
        t2 = boost::chrono::high_resolution_clock::now();
    }
    DataType operator ()(){
        t2 = boost::chrono::high_resolution_clock::now();
        duration<DataType, boost::ratio<1, Base>> delta = boost::chrono::duration_cast<duration<DataType, boost::ratio<1, Base>>>(t2 - t1);
        t1 = boost::chrono::high_resolution_clock::now();
        val = delta.count();
        return val;
    }

    std::string getString(){
        std::stringstream ss;
        ss<<val;
        return ss.str();
    }
    void start(){
        t1 = boost::chrono::high_resolution_clock::now();
    }
    DataType middle(){
        t2 = boost::chrono::high_resolution_clock::now();
        duration<DataType, boost::ratio<1, Base>> delta = boost::chrono::duration_cast<duration<DataType, boost::ratio<1, Base>>>(t2 - t1);

        val = delta.count();
        return val;
    }
    DataType end(){
        t2 = boost::chrono::high_resolution_clock::now();
        duration<DataType, boost::ratio<1, Base>> delta = boost::chrono::duration_cast<duration<DataType, boost::ratio<1, Base>>>(t2 - t1);

        val = delta.count();
        return val;
    }
    DataType get(){
        return val;
    }
};


#include "bezier_local_planner/bezier_local_planner.h"
#include <Eigen/Core>
#include <chrono>

namespace bezier_local_planner
{
BezierLocalPlanner::BezierLocalPlanner(base_local_planner::CostmapModel* costmap_model,
                                       const std::vector<geometry_msgs::Point>& footprint_spec, double inscribed_radius,
                                       double circumscribed_radius, const BezierConfig& cfg, ros::NodeHandle nh)
  : costmap_model_(costmap_model)
  , footprint_spec_(footprint_spec)
  , inscribed_radius_(inscribed_radius)
  , circumscribed_radius_(circumscribed_radius)
  , cfg_(&cfg)
{
  initializeCandidateTargets();

  feasible_candidate_paths_pub_ = nh.advertise<nav_msgs::Path>("feasible_candidate_paths", 1);
  all_candidate_paths_pub_ = nh.advertise<nav_msgs::Path>("all_candidate_paths", 1);
  tracked_path_pub_ = nh.advertise<nav_msgs::Path>("tracked_path", 1);
}

BezierLocalPlanner::~BezierLocalPlanner()
{
}

void BezierLocalPlanner::initializeCandidateTargets()
{
  double dy = 1.0 / 10;
  for (int i = 0; i <= 20; i++)
  {
    Pose2D pose;
    pose.y += (-1.0 + i * dy);
    candidate_targets_.push_back(pose);
  }
}

bool BezierLocalPlanner::computeVelocityCommands(const Velocity& robot_vel, const Pose2D& robot_pose,
                                                 const std::vector<Pose2D>& global_plan,
                                                 unsigned char const* const* costmap, int size_x, int size_y,
                                                 double resolution, double origin_x, double origin_y, Velocity& cmd_vel)
{
  return true;
}

bool BezierLocalPlanner::isPathFeasible(const std::vector<Pose2D>& path)
{
  // Number of poses along the path that should be verified
  int look_ahead_idx = (int)path.size() - 1;

  for (int i = 0; i <= look_ahead_idx; ++i)
  {
    if (costmap_model_->footprintCost(path[i].x, path[i].y, path[i].theta, footprint_spec_, inscribed_radius_,
                                      circumscribed_radius_) == -1)
    {
      return false;
    }
    // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than
    // the specified threshold and interpolates in that case. (if obstacles are pushing two consecutive poses away, the
    // center between two consecutive poses might coincide with the obstacle ;-)!
    if (i < look_ahead_idx)
    {
      double delta_rot = NormalizeAngle(path[i + 1].theta - path[i].theta);
      Eigen::Vector2d delta_dist(path[i + 1].x - path[i].x, path[i + 1].y - path[i].y);
      if (fabs(delta_rot) > M_PI || delta_dist.norm() > inscribed_radius_)
      {
        int n_additional_samples =
            std::max(std::ceil(fabs(delta_rot) / M_PI), std::ceil(delta_dist.norm() / inscribed_radius_)) - 1;

        Eigen::Vector2d intermediate_position(path[i].x, path[i].y);
        double intermediate_theta = path[i].theta;
        for (int step = 0; step < n_additional_samples; ++step)
        {
          intermediate_position = intermediate_position + delta_dist / (n_additional_samples + 1.0);
          intermediate_theta = NormalizeAngle(intermediate_theta + delta_rot / (n_additional_samples + 1.0));
          if (costmap_model_->footprintCost(intermediate_position.x(), intermediate_position.y(), intermediate_theta,
                                            footprint_spec_, inscribed_radius_, circumscribed_radius_) == -1)
          {
            return false;
          }
        }
      }
    }
  }
  return true;
}

void BezierLocalPlanner::publishBezierPaths(const std::vector<std::vector<Pose2D> >& bezier_paths,
                                            ros::Publisher publisher)
{
  if (bezier_paths.empty())
    return;

  nav_msgs::Path gui_path;
  gui_path.header.frame_id = cfg_->map_frame;
  gui_path.header.stamp = ros::Time::now();
  geometry_msgs::PoseStamped pose;

  for (int i = 0; i < (int)bezier_paths.size(); i++)
  {
    for (int j = 0; j < (int)bezier_paths[i].size(); j++)
    {
      pose.pose.position.x = bezier_paths[i][j].x;
      pose.pose.position.y = bezier_paths[i][j].y;
      pose.pose.orientation = tf::createQuaternionMsgFromYaw(bezier_paths[i][j].theta);
      gui_path.poses.push_back(pose);
    }

    for (int j = (int)bezier_paths[i].size() - 1; j >= 0; j--)
    {
      pose.pose.position.x = bezier_paths[i][j].x;
      pose.pose.position.y = bezier_paths[i][j].y;
      pose.pose.orientation = tf::createQuaternionMsgFromYaw(bezier_paths[i][j].theta);
      gui_path.poses.push_back(pose);
    }
  }

  publisher.publish(gui_path);
}

void BezierLocalPlanner::publishBezierPath(const std::vector<Pose2D>& bezier_path, ros::Publisher publisher)
{
  nav_msgs::Path gui_path;
  gui_path.header.frame_id = cfg_->map_frame;
  gui_path.header.stamp = ros::Time::now();
  geometry_msgs::PoseStamped pose;

  for (int i = 0; i < (int)bezier_path.size(); i++)
  {
    pose.pose.position.x = bezier_path[i].x;
    pose.pose.position.y = bezier_path[i].y;
    pose.pose.orientation = tf::createQuaternionMsgFromYaw(bezier_path[i].theta);
    gui_path.poses.push_back(pose);
  }

  publisher.publish(gui_path);
}

}  // namespace bezier_local_planner


/**
 * True-of-date ECI coordinates. This is just another xyz coordinate.
 * Most of the magic happens in converts.hpp and conversion_matrices.hpp
 *
 * Copyright 2013 Bruce Ide
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <Eigen/Core>
#include "xyz_coordinate.hpp"

#ifndef _HPP_TOD_ECI
#define _HPP_TOD_ECI

namespace fr {

  namespace coordinates {

    class tod_eci : public xyz_coordinate {

    public:
      typedef xyz_coordinate super;
      tod_eci(const double &x, const double &y, const double &z) : super(x,y,z)
      {
      }

      tod_eci(const tod_eci &copy) : super(copy.get_x(), copy.get_y(), copy.get_z())
      {
      }

      virtual ~tod_eci()
      {
      }

      /**
       * Interpolate between two points. Takes time observed at the current
       * point, another tod_eci and the time observed at that point,
       * and a time between the two points.
       */

      virtual tod_eci interpolate(const double &time_now, const tod_eci &next_point, const double &time_then, const double &time_between)
      {
	Eigen::Vector3d vec_now = get_xyz();
	Eigen::Vector3d next_vec = next_point.get_xyz();
	Eigen::Vector3d interim = interpolate_3x3(vec_now, time_now, next_vec, time_then, time_between);
	tod_eci retval(interim(0), interim(1), interim(2));
	return retval;
      }

    };

  }
}

#endif


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISJOINT_SEGMENT_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISJOINT_SEGMENT_BOX_HPP

#include <cstddef>
#include <utility>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/calculation_type.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/algorithms/detail/disjoint/segment_box.hpp>

#include <boost/geometry/strategies/disjoint.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

namespace boost { namespace geometry { namespace strategy { namespace disjoint
{

// NOTE: This may be temporary place for this or corresponding strategy
// It seems to be more appropriate to implement the opposite of it
// e.g. intersection::segment_box because in disjoint() algorithm
// other strategies that are used are intersection and covered_by strategies.
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
struct segment_box_geographic
{
public:
    typedef Spheroid model_type;

    inline segment_box_geographic()
        : m_spheroid()
    {}

    explicit inline segment_box_geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Segment, typename Box>
    struct point_in_geometry_strategy
        : services::default_strategy
            <
                typename point_type<Segment>::type,
                Box
            >
    {};

    template <typename Segment, typename Box>
    static inline typename point_in_geometry_strategy<Segment, Box>::type
        get_point_in_geometry_strategy()
    {
        typedef typename point_in_geometry_strategy<Segment, Box>::type strategy_type;

        return strategy_type();
    }

    template <typename Segment, typename Box>
    inline bool apply(Segment const& segment, Box const& box) const
    {
        geometry::strategy::azimuth::geographic
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            > azimuth_geographic(m_spheroid);

        return geometry::detail::disjoint::disjoint_segment_box_sphere_or_spheroid
                <geographic_tag>::apply(segment, box, azimuth_geographic);
    }

private:
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Linear, typename Box, typename LinearTag>
struct default_strategy<Linear, Box, LinearTag, box_tag, 1, 2,
                        geographic_tag, geographic_tag>
{
    typedef segment_box_geographic<> type;
};

template <typename Box, typename Linear, typename LinearTag>
struct default_strategy<Box, Linear, box_tag, LinearTag, 2, 1,
                        geographic_tag, geographic_tag>
{
    typedef segment_box_geographic<> type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}}} // namespace boost::geometry::strategy::disjoint

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISJOINT_SEGMENT_BOX_HPP


#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <boost/circular_buffer.hpp>
#include <range/v3/all.hpp>
#include <cstdint>
#include <initializer_list>
#include <iterator>
#include <bit>
#include <functional>
#include <array>
namespace views = ranges::views;
using Buffer = boost::circular_buffer<std::uint8_t>;
class Knothash
{
  Buffer ring_;
  static inline constexpr int N = 256;
  void reverse(int start, int len)
  {
    for (auto i = 0; i < len / 2; ++i) {
      int b = (start + i) % ring_.size();
      int r = (start + len - 1 - i) % ring_.size();
      std::swap(ring_[b], ring_[r]);
    }
  }

public:
  Knothash() : ring_(N)
  {
  }
  std::array<std::uint8_t, 16> operator()(std::string_view input)
  {
    ranges::copy(views::ints | views::take(N), std::back_inserter(ring_));
    int skiplen_ = 0;
    int curpos_ = 0;
    auto v = input | views::transform([](char c) { return static_cast<int>(c); });
    int suffix[] = { 17, 31, 73, 47, 23 };
    auto sequencelen_ = views::concat(v, suffix);
    for (auto i = 0; i < 64; ++i) {
      for (auto l : sequencelen_) {
        reverse(curpos_, l);
        curpos_ += l + skiplen_;
        ++skiplen_;
      }
    }
    std::array<std::uint8_t, 16> result{};
    auto hexstring = ring_ | views::chunk(16) | views::transform([](auto range) {
      auto r = ranges::accumulate(
        range, 0u, std::bit_xor{});
      return r;
    });
    ranges::copy(hexstring, result.begin());
    return result;
  }
  std::string str(std::string_view s)
  {
    auto r = (*this)(s);
    auto hexstring = r | views::transform([](auto t) {
      std::string result;
      char hex[] = "0123456789abcdef";
      result += hex[(t / 16)];
      result += hex[(t % 16)];
      return result;
    });
    std::string sres;
    return ranges::accumulate(hexstring, sres);
  }
};

struct KnotPopCount
{
  template<class T>
  int operator()(const T &t)
  {
    auto r = hash_(t);
    return ranges::accumulate(r, 0, {}, [](auto a) {
      int n = std::popcount(a);
      return n;
    });
  }

private:
  Knothash hash_;
};

template<class T>
int knohhash_popCount(const T &t) noexcept
{
  return std::transform_reduce(t.begin(), t.end(), 0, std::plus{}, KnotPopCount{});
}
int main()
{
  Knothash hash_;
  auto range = views::ints | views::take(128) | views::transform([str = "stpzcrnm"](int i) {
    return str + std::string("-") + std::to_string(i);
  }) | views::common;
  auto n = knohhash_popCount(range);

  std::cout << n << std::endl;
}


#include <Eigen/Core>
#include "ear/common/helpers/make_unique.hpp"
#include "ear/dsp/delay_buffer.hpp"
#include "ear/exceptions.hpp"

namespace ear {
  namespace dsp {

    /// A multi-channel delay buffer, templated over the type of sample.
    class DelayBufferImpl {
     public:
      /// @param nchannels number of input and output channels
      /// @param nsamples length of the delay
      DelayBufferImpl(size_t nchannels, size_t nsamples)
          : delaymem(Eigen::MatrixXf::Zero(nsamples, nchannels)) {}

      /// Process an arbitrary number of samples. \p input and \p output have
      /// \c nchannels channels and \p nsamples samples.
      void process(size_t nsamples, const float *const *input,
                   float *const *output) {
        Eigen::Index nchannels = delaymem.cols();
        Eigen::Index delay = delaymem.rows();

        for (Eigen::Index channel = 0; channel < nchannels; channel++) {
          for (Eigen::Index sample = 0; sample < nsamples + delay; sample++) {
            // transfer from:
            //    [delaymem; input]
            // to:
            //    [output; delaymem]
            float value = sample < delay ? delaymem(sample, channel)
                                         : input[channel][sample - delay];

            if (sample < nsamples)
              output[channel][sample] = value;
            else
              delaymem(sample - nsamples, channel) = value;
          }
        }
      }

      /// Get the delay in samples.
      int get_delay() const { return (int)delaymem.rows(); }

     private:
      Eigen::MatrixXf delaymem;
    };

    DelayBuffer::DelayBuffer(size_t nchannels, size_t nsamples)
        : impl(std::make_unique<DelayBufferImpl>(nchannels, nsamples)) {}

    void DelayBuffer::process(size_t nsamples, const float *const *input,
                              float *const *output) {
      impl->process(nsamples, input, output);
    }

    int DelayBuffer::get_delay() const { return impl->get_delay(); }

    DelayBuffer::~DelayBuffer() = default;

  }  // namespace dsp
}  // namespace ear


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2011.
// 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)

#include <mpllibs/metamonad/list.hpp>
#include <mpllibs/metamonad/return_.hpp>
#include <mpllibs/metamonad/bind.hpp>
#include <mpllibs/metamonad/lambda.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/lambda_c.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/equal.hpp>
#include <boost/mpl/assert.hpp>

using boost::mpl::list;

using mpllibs::metamonad::lambda_c;
using namespace mpllibs::metamonad::name;

namespace
{
  typedef lambda_c<t, list<t, t> > twice;
}

BOOST_AUTO_TEST_CASE(test_list_monad)
{
  using mpllibs::metamonad::return_;
  using mpllibs::metamonad::bind;
  using mpllibs::metamonad::list_tag;

  using boost::mpl::equal;

  // test_return
  BOOST_MPL_ASSERT((equal<return_<list_tag, int13>, list<int13> >));

  // test_bind
  BOOST_MPL_ASSERT((
    equal<
      bind<list_tag, list<int13, int11>, twice>::type,
      list<int13, int13, int11, int11>
    >
  ));
}


#include <boost/test/unit_test.hpp>

#include "../test_utils.hh"
#include "../fixture.hh"

namespace {

template<class T>
void trim_to_banded(auto matrix, scylla_blas::index_t KL, scylla_blas::index_t KU) {
    using namespace scylla_blas;

    for (index_t i = 1; i <= matrix->row_count; i++) {
        scylla_blas::vector_segment<T> old_row = matrix->get_row(i);
        scylla_blas::vector_segment<T> new_row;
        for (auto &val : old_row) {
            auto new_val = val;

            /* Skip values that are too far (we have to insert 0 due to our block clearing limitations) */
            if (val.index > i + KU) new_val.value = 0;
            if (val.index < i - KL) new_val.value = 0;

            new_row.emplace_back(new_val);
        }
        matrix->update_row(i, new_row);
    }
}

}

//BOOST_FIXTURE_TEST_CASE(mixed_operations, mixed_fixture)
//{
//    using namespace scylla_blas;
//    {
//        /* FLOAT */
//
//        print_matrix(*float_BxA);
//        print_vector(*float_A);
//        std::cout << "sgemv(float_BxA, float_A) = ";
//        float_B->clear_all();
//        print_vector(scheduler->sgemv(scylla_blas::NoTrans, 1, *float_BxA, *float_A, 0, *float_B));
//
//        float_B->clear_all();
//        print_matrix(*float_AxB);
//        std::cout << "sgemv(float_AxB^T, float_A) = ";
//        print_vector(scheduler->sgemv(scylla_blas::Trans, 1, *float_AxB, *float_A, 0, *float_B));
//
//        float_B->clear_all();
//        index_type KL = 1, KU = 2;
//        trim_to_banded<float>(float_BxA, KL, KU);
//        print_matrix(*float_BxA);
//        std::cout << "banded sgbmv(float_BxA, float_A) = ";
//        print_vector(scheduler->sgbmv(scylla_blas::NoTrans, KL, KU, 1, *float_BxA, *float_A, 0, *float_B));
//
//        float_B->clear_all();
//        trim_to_banded<float>(float_AxB, KL, KU);
//        print_matrix(*float_AxB);
//        std::cout << "banded sgbmv(float_AxB^T, float_A) = ";
//        print_vector(scheduler->sgbmv(scylla_blas::Trans, KL, KU, 1, *float_AxB, *float_A, 0, *float_B));
//    }
//
//    {
//        /* DOUBLE */
//
//        print_matrix(*double_BxA);
//        print_vector(*double_A);
//        std::cout << "dgemv(double_BxA, double_A) = ";
//        double_B->clear_all();
//        print_vector(scheduler->dgemv(scylla_blas::NoTrans, 1, *double_BxA, *double_A, 0, *double_B));
//
//        double_B->clear_all();
//        print_matrix(*double_AxB);
//        std::cout << "dgemv(double_AxB^T, double_A) = ";
//        print_vector(scheduler->dgemv(scylla_blas::Trans, 1, *double_AxB, *double_A, 0, *double_B));
//
//        index_type KL = 1, KU = 2;
//
//        double_B->clear_all();
//        trim_to_banded<double>(double_BxA, KL, KU);
//        print_matrix(*double_BxA);
//        std::cout << "banded sgbmv(double_BxA, double_A) = ";
//        print_vector(scheduler->dgbmv(scylla_blas::NoTrans, KL, KU, 1, *double_BxA, *double_A, 0, *double_B));
//
//        double_B->clear_all();
//        trim_to_banded<double>(double_AxB, KL, KU);
//        print_matrix(*double_AxB);
//        std::cout << "banded sgbmv(double_AxB^T, double_A) = ";
//        print_vector(scheduler->dgbmv(scylla_blas::Trans, KL, KU, 1, *double_AxB, *double_A, 0, *double_B));
//    }
//
//    print_vector(*float_A);
//    print_vector(*float_B);
//    float_AxB->clear_all();
//    std::cout  << "float_A^T * float_B = ";
//    print_matrix(scheduler->sger(1, *float_A, *float_B, *float_AxB));
//
//    print_vector(*double_A);
//    print_vector(*double_B);
//    double_AxB->clear_all();
//    std::cout  << "double_A^T * double_B = ";
//    print_matrix(scheduler->dger(1, *double_A, *double_B, *double_AxB));
//}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-reduction
    This function object returns the returns the dot product of the two arguments

    @par Header <boost/simd/function/dot.hpp>

    @par Example:

      @snippet edot.cpp dot

    @par Possible output:

      @snippet edot.txt dot

  **/
  scalar_of_t<Value> dot(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/dot.hpp>
#include <boost/simd/function/simd/dot.hpp>

#endif


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing signnz capabilities

    Returns the sign of x. I.e. -1 or 1, according
    x is negative or positive.

    This function never returns @ref Zero (zero is considered positive for integers).

    For floating point numbers the bit of sign is taken into account and so
    we always have signnz(-z) == -signnz(z)).

    The result for @ref Nan entry is undefined

    @par Semantic:

    @code
    T r = signnz(x);
    @endcode

    is similar to:

    @code
    T r = is_nan(x) ? Nan<T>() : (is_negative(x) ? T(-1) : T(1));
    @endcode

    @see Mzero,  sign, is_negative, is_positive

  **/
  const boost::dispatch::functor<tag::signnz_> signnz = {};
} }
#endif

#include <boost/simd/function/scalar/signnz.hpp>
#include <boost/simd/function/simd/signnz.hpp>

#endif


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <exception>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/algorithm/string.hpp>

#include <CGAL/Random.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Simple_cartesian.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/iterator.h>

#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path_traits.h>
#include <CGAL/Surface_mesh_shortest_path/Surface_mesh_shortest_path.h>

#define UNUSED(X) (void)sizeof(X)

namespace po = boost::program_options;

typedef boost::timer::nanosecond_type nanosecond_type;

enum Kernel_type
{
  KERNEL_UNKNOWN,
  KERNEL_IPICK,
  KERNEL_EPICK,
  KERNEL_EPECK,
};

template <class IntType>
class SampledValue
{
private:
  IntType m_minimum;
  IntType m_maximum;
  IntType m_sum;
  IntType m_numSamples;

public:
  SampledValue()
  {
    reset();
  }

  void reset()
  {
    m_minimum = IntType(0);
    m_maximum = IntType(0);
    m_sum = IntType(0);
    m_numSamples = IntType(0);
  }

  IntType minimum() const
  {
    return m_minimum;
  }

  IntType maximum() const
  {
    return m_maximum;
  }

  IntType sum() const
  {
    return m_sum;
  }

  IntType average_integer() const
  {
    return m_sum / m_numSamples;
  }

  IntType average_remainder() const
  {
    return m_sum % m_numSamples;
  }

  double average_float() const
  {
    return double(average_integer()) + (double(average_remainder()) / double(m_numSamples));
  }

  void add_sample(IntType sample)
  {
    if (m_numSamples == 0)
    {
      m_minimum = sample;
      m_maximum = sample;
    }
    else
    {
      m_minimum = (std::min)(m_minimum, sample);
      m_maximum = (std::max)(m_maximum, sample);
    }
    m_sum += sample;
    ++m_numSamples;
  }
};

struct Benchmark_data
{
  size_t numVertices;
  size_t numFaces;
  size_t numEdges;

  SampledValue<boost::timer::nanosecond_type> constructionTime;
  SampledValue<boost::timer::nanosecond_type> queryTime;

#if !defined(NDEBUG)
  SampledValue<size_t> peakMemoryUsage;
#endif

  void reset()
  {
    numVertices = 0;
    numEdges = 0;
    numFaces = 0;
    constructionTime.reset();
    queryTime.reset();
#if !defined(NDEBUG)
    peakMemoryUsage.reset();
#endif
  }
};

void print_results(std::ostream& stream, const std::string& filename, const Benchmark_data& outData)
{
  stream << "Filename | " << filename << std::endl;
  stream << "Num Vertices | " << outData.numVertices << std::endl;
  stream << "Num Edges | " << outData.numEdges << std::endl;
  stream << "Num Faces | " << outData.numFaces << std::endl;

  stream << "Construction  | " << outData.constructionTime.average_float() / 1.0e9 << " | " << double(outData.constructionTime.minimum()) / 1.0e9 << " | " << double(outData.constructionTime.maximum()) / 1.0e9 << " |" << std::endl;
  stream << "Query         | " << 1.0 / (outData.queryTime.average_float() / 1.0e9) << " | " << double(outData.queryTime.minimum()) / 1.0e9 << " | " << double(outData.queryTime.maximum()) / 1.0e9 << " |" << std::endl;
#if !defined(NDEBUG)
  stream << "Memory (Peak) | " << outData.peakMemoryUsage.average_float() / 1.0e6 << " | " << double(outData.peakMemoryUsage.minimum()) / 1.0e6 << " | " << double(outData.peakMemoryUsage.maximum()) / 1.0e6 << " |" << std::endl;
#endif
}

template <class Traits>
typename Traits::Barycentric_coordinates random_coordinates(CGAL::Random& rand)
{
  typedef typename Traits::FT FT;
  typename Traits::Construct_barycentric_coordinates construct_barycentric_coordinates;
  FT u = rand.uniform_real(FT(0), FT(1));
  FT v = rand.uniform_real(FT(0), FT(1) - u);
  return construct_barycentric_coordinates(u, v, FT(1) - u - v);
}

template <class Kernel>
void run_benchmarks(CGAL::Random& rand, size_t numTrials, size_t numSources, size_t numQueries, const std::string& polyhedronFile, Benchmark_data& outData)
{
  typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron_3;
  typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Polyhedron_3> Traits;
  typedef typename Traits::Barycentric_coordinates Barycentric_coordinates;
  typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;
  typedef typename Surface_mesh_shortest_path::Face_location Face_location;
  typedef typename Surface_mesh_shortest_path::FT FT;
  typedef boost::graph_traits<Polyhedron_3> Graph_traits;
  typedef typename Graph_traits::face_descriptor face_descriptor;
  typedef typename Graph_traits::face_iterator face_iterator;

  std::ifstream inFile(polyhedronFile.c_str());

  if (!inFile)
  {
    throw std::runtime_error(std::string("Model file \"") + polyhedronFile + std::string("\" does not exist."));
  }

  Polyhedron_3 polyhedron;

  inFile >> polyhedron;
  inFile.close();

  CGAL::set_halfedgeds_items_id(polyhedron);

  boost::timer::cpu_timer timer;
  outData.reset();

  outData.numVertices = num_vertices(polyhedron);
  outData.numEdges = num_edges(polyhedron);
  outData.numFaces = num_faces(polyhedron);

  face_iterator startFace, endFace;
  boost::tie(startFace, endFace) = faces(polyhedron);

  std::vector<face_descriptor> allFaces;

  for (face_iterator currentFace = startFace; currentFace != endFace; ++currentFace)
  {
    allFaces.push_back(*currentFace);
  }

  Traits traits;
  Surface_mesh_shortest_path shortestPaths(polyhedron, traits);

  for (size_t i = 0; i < numTrials; ++i)
  {
    std::vector<Face_location> sourcePoints;

    while (sourcePoints.size() < numSources)
    {
      face_descriptor sourceFace = allFaces[rand.get_int(0, allFaces.size())];
      Barycentric_coordinates sourceLocation = random_coordinate<Traits>(rand);
      sourcePoints.push_back(Face_location(sourceFace, sourceLocation));
    }

    shortestPaths.clear();
    shortestPaths.add_source_points(sourcePoints.begin(), sourcePoints.end());

    timer.start();
    shortestPaths.build_sequence_tree();
    timer.stop();

    boost::timer::cpu_times elapsed = timer.elapsed();

    outData.constructionTime.add_sample(elapsed.wall);

#if !defined(NDEBUG)
    outData.peakMemoryUsage.add_sample((std::max)(shortestPaths.peak_memory_usage(), shortestPaths.current_memory_usage()));
#endif

    for (size_t j = 0; j < numQueries; ++j)
    {
      face_descriptor sourceFace = allFaces[rand.get_int(0, allFaces.size())];
      Barycentric_coordinates sourceLocation = random_coordinate<Traits>(rand);

      timer.start();
      FT distance = shortestPaths.shortest_distance_to_source_points(sourceFace, sourceLocation).first;
      timer.stop();
      UNUSED(distance);

      boost::timer::cpu_times elapsed = timer.elapsed();

      outData.queryTime.add_sample(elapsed.wall);
    }
  }
}

Kernel_type parse_kernel_type(const std::string& s)
{
  if (boost::iequals(s, "ipick"))
  {
    return KERNEL_IPICK;
  }
  else if (boost::iequals(s, "epick"))
  {
    return KERNEL_EPICK;
  }
  else if (boost::iequals(s, "epeck"))
  {
    return KERNEL_EPECK;
  }
  else
  {
    return KERNEL_UNKNOWN;
  }
}

int main(int argc, char* argv[])
{
  namespace po = boost::program_options;
  po::options_description options;

  options.add_options()
    ("help,h", "Display help message")
    ("polyhedron,p", po::value<std::string>(), "Polyhedron input file")
    ("randomseed,r", po::value<size_t>()->default_value(0), "Randomization seed value")
    ("trials,t", po::value<size_t>()->default_value(20), "Number of trials to run")
    ("numpoints,n", po::value<size_t>()->default_value(1), "Number of source points per trial")
    ("queries,q", po::value<size_t>()->default_value(100), "Number of queries to run per trial")
    ("kernel,k", po::value<std::string>()->default_value("epick"), "Kernel to use.  One of \'ipick\', \'epick\', \'epeck\'")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    std::cout << options << std::endl;
  }
  else if (vm.count("polyhedron"))
  {
    Benchmark_data results;

    CGAL::Random rand(vm["randomseed"].as<size_t>());
    size_t numTrials = vm["trials"].as<size_t>();
    size_t numPoints = vm["numpoints"].as<size_t>();
    size_t numQueries = vm["queries"].as<size_t>();
    std::string polyhedronFile = vm["polyhedron"].as<std::string>();

    try
    {
      switch (parse_kernel_type(vm["kernel"].as<std::string>()))
      {
        case KERNEL_IPICK:
          run_benchmarks<CGAL::Simple_cartesian<double> >(rand, numTrials, numPoints, numQueries, polyhedronFile, results);
          break;
        case KERNEL_EPICK:
          run_benchmarks<CGAL::Exact_predicates_inexact_constructions_kernel>(rand, numTrials, numPoints, numQueries, polyhedronFile, results);
          break;
        case KERNEL_EPECK:
          run_benchmarks<CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt>(rand, numTrials, numPoints, numQueries, polyhedronFile, results);
          break;
        default:
          std::cerr << "Invalid kernel type: \"" << vm["kernel"].as<std::string>() << "\"" << std::endl;
          return EXIT_FAILURE;
      }
    }
    catch (std::exception& e)
    {
      std::cerr << "Runtime error: " << e.what() << std::endl;
      return EXIT_FAILURE;
    }

    print_results(std::cout, polyhedronFile, results);
  }
  else
  {
    std::cerr << "Please specify a polyhedron to use.  Use option --help for more details." << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


#ifndef LWTNN_FAST_INPUT_PREPROCESSOR_HH
#define LWTNN_FAST_INPUT_PREPROCESSOR_HH

#include "lwtnn/lightweight_network_config.hh"
#include "lwtnn/generic/eigen_typedefs.hh"

#include <Eigen/Dense>
#include <vector>

namespace lwt {

namespace generic {

  // ______________________________________________________________________
  // input preprocessor (handles normalization and packing into Eigen)

  template<typename T>
  class FastInputPreprocessor
  {
  public:
    FastInputPreprocessor(const std::vector<Input>& inputs,
                          const std::vector<std::string>& order);
    VectorX<T> operator()(const VectorX<T>&) const;
  private:
    // input transformations
    VectorX<T> m_offsets;
    VectorX<T> m_scales;
    std::vector<size_t> m_indices;
  };

  template<typename T>
  class FastInputVectorPreprocessor
  {
  public:
    FastInputVectorPreprocessor(const std::vector<Input>& inputs,
                                const std::vector<std::string>& order);
    MatrixX<T> operator()(const MatrixX<T>&) const;
  private:
    // input transformations
    VectorX<T> m_offsets;
    VectorX<T> m_scales;
    std::vector<size_t> m_indices;
  };
}

}

#include "FastInputPreprocessor.tcc"

#endif


// Copyright 2019 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SOURCES_UTILS_LOG_COSH_HPP
#define SOURCES_UTILS_LOG_COSH_HPP

#include <Eigen/Core>

#include "common_types.hpp"

namespace netket {

namespace detail {
#ifdef NETKET_USE_SLEEF
Complex SumLogCoshBias_avx2(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> input,
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> bias) noexcept;
Complex SumLogCosh_avx2(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> input) noexcept;
#endif
Complex SumLogCoshBias_generic(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> input,
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> bias) noexcept;
Complex SumLogCosh_generic(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> input) noexcept;
double SumLogCosh_generic(
    Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> input) noexcept;
}  // namespace detail

/// Returns `∑log(cosh(inputᵢ + biasᵢ))`
inline Complex SumLogCoshBias(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> input,
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>> bias) noexcept {
#ifdef NETKET_USE_SLEEF
  if (__builtin_cpu_supports("avx2")) {
    return detail::SumLogCoshBias_avx2(input, bias);
  } else {
    return detail::SumLogCoshBias_generic(input, bias);
  }
#else
  return detail::SumLogCoshBias_generic(input, bias);
#endif
}

inline double SumLogCoshBias(
    Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> input,
    Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> bias) noexcept {
    // TODO Add SLEEF implementation
    return (Eigen::abs(input.array()) +
                Eigen::log1p(Eigen::exp(-2. *
                  Eigen::abs((input + bias).array()))))
      .sum();
}

/// Returns `∑log(cosh(inputᵢ))`
inline Complex SumLogCosh(
    Eigen::Ref<const Eigen::Matrix<Complex, Eigen::Dynamic, 1>>
        input) noexcept {
#ifdef NETKET_USE_SLEEF
  if (__builtin_cpu_supports("avx2")) {
    return detail::SumLogCosh_avx2(input);
  } else {
    return detail::SumLogCosh_generic(input);
  }
#else
  return detail::SumLogCosh_generic(input);
#endif
}

// For real values Sum Log cosh is evaluated as
// |x| + log(1+exp(-2*|x|))
inline double SumLogCosh(
    Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> input) noexcept {
  return (Eigen::abs(input.array()) +
          Eigen::log1p(Eigen::exp(-2. * Eigen::abs(input.array()))))
      .sum();
}

inline void SumLogCosh(Eigen::Ref<const MatrixXd> input,
                       Eigen::Ref<VectorXcd> output) noexcept {
#pragma omp parallel for schedule(static)
  for (auto i = 0; i < output.size(); i++) {
    output(i) = SumLogCosh(input.row(i));
  }
}

inline void SumLogCosh(Eigen::Ref<const MatrixXcd> input,
                       Eigen::Ref<VectorXcd> output) noexcept {
#pragma omp parallel for schedule(static)
  for (auto i = 0; i < output.size(); i++) {
    output(i) = SumLogCosh(input.row(i));
  }
}

inline void SumLogCoshReIm(Eigen::Ref<const MatrixXd> inputr,
                           Eigen::Ref<const MatrixXd> inputi,
                           Eigen::Ref<VectorXcd> output) noexcept {
  constexpr std::complex<double> I_(0, 1);

#pragma omp parallel for schedule(static)
  for (auto i = 0; i < output.size(); i++) {
    output(i) = SumLogCosh(inputr.row(i)) + I_ * SumLogCosh(inputi.row(i));
  }
}

}  // namespace netket

#endif  // SOURCES_UTILS_LOG_COSH_HPP


#ifndef TESTDEATHCLOCKODESYSTEM_HPP_
#define TESTDEATHCLOCKODESYSTEM_HPP_

#include <cxxtest/TestSuite.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include "DeathClockOdeSystem.hpp"
#include "CvodeAdaptor.hpp"
#include "Timer.hpp"
#include "OutputFileHandler.hpp"

//This test is always run sequentially (never in parallel)
#include "FakePetscSetup.hpp"

class TestDeathClockOdeSystem : public CxxTest::TestSuite
{
public:

    void TestDeathClockOdeSystemSetup()
    {
#ifdef CHASTE_CVODE
        DeathClockOdeSystem ode_system;

        // Set parameters
        ode_system.SetParameter("number of neighbours", 5);
        ode_system.SetParameter("number of neighbours g2", 2);
        ode_system.SetParameter("number of neighbours apoptosis", 1);

        ode_system.SetParameter("shared edge length", 50);
        ode_system.SetParameter("shared edge length g2", 20);
        ode_system.SetParameter("shared edge length apoptosis", 10);

        ode_system.SetParameter("base rate", 0.75);

        ode_system.SetParameter("neighbours constant", .76);
        ode_system.SetParameter("neighbours g2 constant", .77);
        ode_system.SetParameter("neighbours apoptosis constant", .78);
        ode_system.SetParameter("normalised neighbours g2 constant", .79);
        ode_system.SetParameter("normalised neighbours apoptosis constant", .80);

        ode_system.SetParameter("shared edge constant", .076);
        ode_system.SetParameter("shared edge g2 constant", .077);
        ode_system.SetParameter("shared edge apoptosis constant", .078);
        ode_system.SetParameter("normalised shared edge g2 constant", .079);
        ode_system.SetParameter("normalised shared edge apoptosis constant", .080);

        double h_value = 1.0;
        CvodeAdaptor cvode_solver;
        OdeSolution solutions;

        // Check initial conditions
        std::vector<double> initial_conditions = ode_system.GetInitialConditions();

        TS_ASSERT_DELTA(initial_conditions[0], 0.0, 1e-6);

        Timer::Reset();
        solutions = cvode_solver.Solve(&ode_system, initial_conditions, 0.0, 100.0, h_value, h_value);
        Timer::Print("1. Cvode for 100 hours");

        // Test solutions are OK for a small time increase...
        int end = solutions.rGetSolutions().size() - 1;
        // Tests the simulation is ending at the right time
        TS_ASSERT_DELTA(solutions.rGetTimes()[end], 100, 1e-2);

        // Decent results
        TS_ASSERT_DELTA(solutions.rGetSolutions()[end][0], 1351.36, 1e-4);
#else
        std::cout << "CVODE is not enabled. " << std::endl;
        std::cout << "If required please install and alter your hostconfig "
            "settings to switch on chaste support." << std::endl;
#endif //CHASTE_CVODE
    }

    void TestArchiving()
    {
#ifdef CHASTE_CVODE
        OutputFileHandler handler("archive", false);
        std::string archive_filename = handler.GetOutputDirectoryFullPath() + "death_clock_ode.arch";

        {
            std::vector<double> state_variables;
            state_variables.push_back(3.0);

            DeathClockOdeSystem ode_system(state_variables);

            ode_system.SetDefaultInitialCondition(0, 3.25);

            std::vector<double> initial_conditions = ode_system.GetInitialConditions();

            // These are the initial conditions hard-coded in the constructor.
            TS_ASSERT_EQUALS(initial_conditions.size(), 1u);
            TS_ASSERT_DELTA(initial_conditions[0], 3.25, 1e-6);

            ode_system.SetParameter("base rate", 10.0);

            ode_system.SetParameter("neighbours constant", 10.1);
            ode_system.SetParameter("neighbours g2 constant", 10.2);
            ode_system.SetParameter("neighbours apoptosis constant", 10.3);
            ode_system.SetParameter("normalised neighbours g2 constant", 10.4);
            ode_system.SetParameter("normalised neighbours apoptosis constant", 10.5);

            ode_system.SetParameter("shared edge constant", 20.1);
            ode_system.SetParameter("shared edge g2 constant", 20.2);
            ode_system.SetParameter("shared edge apoptosis constant", 20.3);
            ode_system.SetParameter("normalised shared edge g2 constant", 20.4);
            ode_system.SetParameter("normalised shared edge apoptosis constant", 20.5);

            // Create an output archive
            std::ofstream ofs(archive_filename.c_str());
            boost::archive::text_oarchive output_arch(ofs);

            // Archive ODE system
            AbstractOdeSystem* const p_const_ode_system = &ode_system;
            output_arch << p_const_ode_system;
        }

        {
            AbstractOdeSystem* p_ode_system;

            // Create an input archive
            std::ifstream ifs(archive_filename.c_str(), std::ios::binary);
            boost::archive::text_iarchive input_arch(ifs);

            // Restore from the archive
            input_arch >> p_ode_system;

            // Check that archiving worked correctly
            std::vector<double> initial_conditions = p_ode_system->GetInitialConditions();

            TS_ASSERT_EQUALS(initial_conditions.size(), 1u);
            TS_ASSERT_DELTA(initial_conditions[0], 0.0, 1e-6);

            double base_rate = p_ode_system->GetParameter("base rate");

            double neighbours_constant = p_ode_system->GetParameter("neighbours constant");
            double neighbours_g2_constant =
                p_ode_system->GetParameter("neighbours g2 constant");
            double neighbours_apoptosis_constant =
                p_ode_system->GetParameter("neighbours apoptosis constant");
            double normalised_neighbours_g2_constant =
                p_ode_system->GetParameter("normalised neighbours g2 constant");
            double normalised_neighbours_apoptosis_constant =
                p_ode_system->GetParameter("normalised neighbours apoptosis constant");

            double shared_edge_constant = p_ode_system->GetParameter("shared edge constant");
            double shared_edge_g2_constant =
                p_ode_system->GetParameter("shared edge g2 constant");
            double shared_edge_apoptosis_constant =
                p_ode_system->GetParameter("shared edge apoptosis constant");
            double normalised_shared_edge_g2_constant =
                p_ode_system->GetParameter("normalised shared edge g2 constant");
            double normalised_shared_edge_apoptosis_constant =
                p_ode_system->GetParameter("normalised shared edge apoptosis constant");

            TS_ASSERT_DELTA(base_rate, 10.0, 1e-3);

            TS_ASSERT_DELTA(neighbours_constant, 10.1, 1e-3);
            TS_ASSERT_DELTA(neighbours_g2_constant, 10.2, 1e-3);
            TS_ASSERT_DELTA(neighbours_apoptosis_constant, 10.3, 1e-3);
            TS_ASSERT_DELTA(normalised_neighbours_g2_constant, 10.4, 1e-3);
            TS_ASSERT_DELTA(normalised_neighbours_apoptosis_constant, 10.5, 1e-3);

            TS_ASSERT_DELTA(shared_edge_constant, 20.1, 1e-3);
            TS_ASSERT_DELTA(shared_edge_g2_constant, 20.2, 1e-3);
            TS_ASSERT_DELTA(shared_edge_apoptosis_constant, 20.3, 1e-3);
            TS_ASSERT_DELTA(normalised_shared_edge_g2_constant, 20.4, 1e-3);
            TS_ASSERT_DELTA(normalised_shared_edge_apoptosis_constant, 20.5, 1e-3);

            // Get state variables
            double var1 = p_ode_system->GetStateVariable(0);

            TS_ASSERT_DELTA(var1, 3.0, 1e-3);

            // Tidy up
            delete p_ode_system;
        }
#endif //CHASTE_CVODE
    }

    void TestSetStateVariables()
    {
#ifdef CHASTE_CVODE

        std::vector<double> state_vars;
        state_vars.push_back(1.0);
        DeathClockOdeSystem ode_system(state_vars);

        TS_ASSERT_EQUALS(ode_system.GetStateVariable(0), 1.0);

#endif //CHASTE_CVODE
   }
};

#endif // TESTDEATHCLOCKODESYSTEM_HPP_


/**
 * \ file RLSFilter.cpp
 */

#include <ATK/Adaptive/RLSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

#include <gtest/gtest.h>

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

constexpr gsl::index PROCESSSIZE = 1024 * 64;

TEST(RLSFilter, destructor_test)
{
  ASSERT_NO_THROW(std::make_unique<ATK::RLSFilter<double>>(100));
}

TEST(RLSFilter, size_negative_test)
{
  ATK::RLSFilter<double> filter(100);
  ASSERT_THROW(filter.set_size(0), ATK::RuntimeError);
}

TEST(RLSFilter, size_set_test)
{
  ATK::RLSFilter<double> filter(100);
  filter.set_size(10);
  ASSERT_EQ(filter.get_size(), 10);
}

TEST(RLSFilter, memory_negative_test)
{
  ATK::RLSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(0), ATK::RuntimeError);
}

TEST(RLSFilter, memory_test)
{
  ATK::RLSFilter<double> filter(100);
  filter.set_memory(0.5);
  ASSERT_EQ(filter.get_memory(), 0.5);
}

TEST(RLSFilter, memory_positive1_test)
{
  ATK::RLSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(1), ATK::RuntimeError);
}

TEST(RLSFilter, learning_set_test)
{
  ATK::RLSFilter<double> filter(100);
  ASSERT_EQ(filter.get_learning(), true);
  filter.set_learning(false);
  ASSERT_EQ(filter.get_learning(), false);
}

TEST(RLSFilter, memory_99_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.99);
  filter.set_learning(false);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  filter.set_input_port(0, &generator, 0);
  checker.set_input_port(0, &filter, 0);

  checker.process(PROCESSSIZE);

  ASSERT_NE(filter.get_w(), nullptr);
}

TEST(RLSFilter, constant_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.99);
  filter.set_learning(false);
  Eigen::Matrix<double, Eigen::Dynamic, 1> w(1);
  w << 1;
  filter.set_w(w.data());

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  checker.set_input_port(0, &sum, 0);

  filter.process(1); // to start everything, the filter is supposed to be one sample late
  checker.process(PROCESSSIZE);
}

TEST(RLSFilter, learning_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(0.9999);
  filter.set_learning(true);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  output.set_input_port(0, &sum, 0);

  output.process(PROCESSSIZE);

  for(std::int64_t i = 100; i < PROCESSSIZE; ++i) // let the RLS filter start learning first
  {
    ASSERT_NEAR(std::abs(outdata[i]), 0, 0.1);
  }
}

TEST(RLSFilter, learning_training_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::SimpleSinusGeneratorFilter<double> reference;
  reference.set_output_sampling_rate(48000);
  reference.set_amplitude(-1);
  reference.set_frequency(1000);

  ATK::RLSFilter<double> filter(10);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(0.9999);
  filter.set_learning(true);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  sum.set_input_port(0, &reference, 0);
  sum.set_input_port(1, &filter, 0);
  output.set_input_port(0, &sum, 0);

  output.process(1000);
  filter.set_learning(false);
  output.process(PROCESSSIZE - 1000);

  for (std::int64_t i = 100; i < PROCESSSIZE; ++i) // let the RLS filter start learning first
  {
    ASSERT_NEAR(std::abs(outdata[i]), 0, 0.1);
  }

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_SIGNGAM_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_SIGNGAM_HPP_INCLUDED

#include <nt2/euler/functions/signgam.hpp>
#include <nt2/include/functions/scalar/is_nan.hpp>
#include <nt2/include/functions/scalar/is_lez.hpp>
#include <nt2/include/functions/scalar/is_flint.hpp>
#include <nt2/include/functions/scalar/is_odd.hpp>
#include <nt2/include/functions/scalar/floor.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/nan.hpp>

#include <boost/simd/sdk/config.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( signgam_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef result_type type;
#ifdef BOOST_SIMD_NO_INVALIDS
      if (nt2::is_lez(a0))
      {
        if (nt2::is_flint(a0))
          return nt2::Nan<type>();
        else
          return nt2::One<type>()-bool(nt2::is_odd(nt2::floor(a0)))*nt2::Two<A0>();
      }
#else
      if (nt2::is_lez(a0))
      {
        if (nt2::is_flint(a0)||nt2::is_inf(a0))
          return nt2::Nan<type>();
        else
          return nt2::One<type>()-bool(nt2::is_odd(nt2::floor(a0)))*nt2::Two<A0>();
      }
      else if (nt2::is_nan(a0)) return a0;
#endif
      return nt2::One<type>();
    }
  };
} }

#endif


#include <cstdio>
#include <cstdlib>

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <map>

#include <boost/config.hpp>
#include <boost/tokenizer.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

static void die(const char *reason) {
  fprintf(stderr, "%s\n", reason);
  exit(1);
}

template <typename DistanceMap>
class bacon_number_recorder : public boost::default_bfs_visitor {
  DistanceMap d;
 public:
  bacon_number_recorder(DistanceMap dist) : d(dist) {}

  template <typename Edge, typename Graph>
  void tree_edge(Edge e, const Graph & g) const {
    typename boost::graph_traits<Graph>::vertex_descriptor
      u = boost::source(e, g), v = boost::target(e, g);
    d[v] = d[u] + 1;
  }
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::property<boost::vertex_name_t, std::string>,
  boost::property<boost::edge_name_t, std::string>> Graph;

typedef boost::property_map<Graph, boost::vertex_name_t>::type actor_name_map_t;
typedef boost::property_map<Graph, boost::edge_name_t>::type movie_name_map_t;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef std::map<std::string, Vertex> NameVertexMap;

template <typename DistanceMap>
bacon_number_recorder<DistanceMap> rec_num(DistanceMap d) {
  return bacon_number_recorder<DistanceMap>(d);
}

int main(int argc, char *argv[]) {
  std::ifstream datafile(argc >= 2 ? argv[1] : "./kevin-bacon.dat");
  if(!datafile) die("No datafile given.");

  Graph g;
  actor_name_map_t actor_name = boost::get(boost::vertex_name, g);
  movie_name_map_t connecting_movie = boost::get(boost::edge_name, g);

  NameVertexMap actors;

  for(std::string line; std::getline(datafile, line); ) {
    boost::char_delimiters_separator<char> sep(false, "", ";");
    boost::tokenizer<> line_toks(line, sep);
    boost::tokenizer<>::iterator i = line_toks.begin();
    std::string actors_name = *i++;

    NameVertexMap::iterator pos;
    bool inserted;
    Vertex u, v;

    boost::tie(pos, inserted) = actors.insert(std::make_pair(actors_name, Vertex()));
    if(inserted) {
      u = boost::add_vertex(g);
      actor_name[u] = actors_name;
      pos->second = u;
    } else u = pos->second;

    std::string movie_name = *i++;

    boost::tie(pos, inserted) = actors.insert(std::make_pair(*i, Vertex()));
    if(inserted) {
      v = boost::add_vertex(g);
      actor_name[v] = *i;
      pos->second = v;
    } else v = pos->second;

    boost::graph_traits<Graph>::edge_descriptor e;
    boost::tie(e, inserted) = boost::add_edge(u, v, g);

    if(inserted) connecting_movie[e] = movie_name;
  }

  std::vector<int> bacon_number(boost::num_vertices(g));

  Vertex src = actors["Kevin Bacon"];
  bacon_number[src] = 0;

  boost::breadth_first_search(g, src, boost::visitor(rec_num(&bacon_number[0])));

  boost::graph_traits<Graph>::vertex_iterator i, end;
  for(boost::tie(i, end) = boost::vertices(g); i != end; ++i) {
    std::cout << actor_name[*i] << " has a Bacon number of " <<
      bacon_number[*i] << std::endl;
  }

  return EXIT_SUCCESS;
}


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

The MIT License (MIT)

Copyright (c) 2013-2017 Joel E. Merritt

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.

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

//  This module defines the FunctionObject class whose objects represent
//  each function r in the Gauss-Newton non-linear least squares solver
//  implementation described in main.cpp.
//
//  Each function object is constructed to return either the screen x or
//  screen y error term for a given point. This error term is the
//  distance in pixels between the observed screen position of the point
//  and the computed screen position of the point, given the camera
//  described by the beta vector. These error terms are used as feedback
//  for the Gauss-Newton solver.

#include "function_object.h"

#include <vector>
#include <Eigen/Dense>

#define DEG_TO_RAD (M_PI / 180.0)

//
//  Member Function: FunctionObject::xp
//
//  Return the screen coordinate x (_xp) stored in the object by the
//  constructor.
//
//  No input parameters
//

double FunctionObject::xp() const
{
  return _xp;
}

//
//  Member Function: FunctionObject::yp
//
//  Return the screen coordinate y (_yp) stored in the object by the
//  constructor.
//
//  No input parameters
//

double FunctionObject::yp() const
{
  return _yp;
}

//
//  Member Function: FunctionObject::x3d
//
//  Return the 3d coordinate x (_x3d) stored in the object by the
//  constructor.
//
//  No input parameters
//

double FunctionObject::x3d() const
{
  return _x3d;
}

//
//  Member Function: FunctionObject::y3d
//
//  Return the 3d coordinate y (_y3d) stored in the object by the
//  constructor.
//
//  No input parameters
//

double FunctionObject::y3d() const
{
  return _y3d;
}

//
//  Member Function: FunctionObject::z3d
//
//  Return the 3d coordinate z (_z3d) stored in the object by the
//  constructor.
//
//  No input parameters
//

double FunctionObject::z3d() const
{
  return _z3d;
}

//
//  Member Function: FunctionObject::operator()
//
//  If an even valued index (_indx) has been stored in the object by the
//  constructor, return the difference between the point's x screen
//  coordinate and the x screen backprojection coordinate of the point's
//  3d coordinates backprojected by the camera specified by the input
//  beta parameter vector. Otherwise, return the difference between the
//  point's y screen coordinate and the y screen backprojection coordinate.
//
//  Input parameters:
//  beta -- vector containing camera parameters to optimize
//

double FunctionObject::operator()(Eigen::VectorXd const &beta) const
{
  double ret;

  if(_indx % 2 == 0)
  {
    ret = _xp - EvalBackProject(beta);
  }
  else
  {
    ret = _yp - EvalBackProject(beta);
  }

  return ret;
}

//
// Member Function: FunctionObject::EvalBackProject
//

//  If an even valued index (_indx) has been stored in the object by the
//  constructor, return the x screen backprojection coordinate of the
//  point's 3d coordinates backprojected by the camera specified by the
//  input beta parameter vector. Otherwise, return the y screen
//  backprojection coordinate.
//
//  Input parameters:
//  beta -- vector containing camera parameters to optimize
//

double FunctionObject::EvalBackProject(Eigen::VectorXd const &beta) const
{
  double k = beta[0];
  double tx = beta[1];
  double ty = beta[2];
  double tz = beta[3];
  double rx = beta[4] * DEG_TO_RAD;
  double ry = beta[5] * DEG_TO_RAD;
  double rz = beta[6] * DEG_TO_RAD;

  Eigen::Affine3d rya = Eigen::Affine3d(Eigen::AngleAxisd(-ry,
      Eigen::Vector3d::UnitY()));
  Eigen::Affine3d rxa = Eigen::Affine3d(Eigen::AngleAxisd(-rx,
  Eigen::Vector3d::UnitX()));
  Eigen::Affine3d rza = Eigen::Affine3d(Eigen::AngleAxisd(-rz,
      Eigen::Vector3d::UnitZ()));
  Eigen::Affine3d t1(Eigen::Translation3d(-tx, -ty, -tz));

  Eigen::Matrix4d m4 = (rza * rxa * rya * t1).matrix();
  Eigen::Vector3d pn(_x3d, _y3d, _z3d);
  Eigen::Vector4d pnh = pn.homogeneous();
  Eigen::Vector4d pnh2 = m4 * pnh;
  Eigen::Vector3d pn2 = pnh2.hnormalized();

  //  Handle case when point is behind camera.
  if(pn2[2] > 0.0)
  {
    return 1.e10;
  }

  double ret;
  if(_indx % 2 == 0)
  {
    ret = k * pn2[0] / -pn2[2];
  }
  else
  {
    ret = k * pn2[1] / -pn2[2];
  }

  return ret;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_ORDERED_LOGISTIC_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_ORDERED_LOGISTIC_LOG_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/prob/ordered_logistic_lpmf.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the (natural) log probability of the specified integer
     * outcome given the continuous location and specified cutpoints
     * in an ordered logistic model.
     *
     *
Typically the continous location
     * will be the dot product of a vector of regression coefficients
     * and a vector of predictors for the outcome.
     *
     * @deprecated use ordered_logistic_lpmf
     *
     * @tparam propto True if calculating up to a proportion.
     * @tparam T_loc Location type.
     * @tparam T_cut Cut-point type.
     * @param y Outcome.
     * @param lambda Location.
     * @param c Positive increasing vector of cutpoints.
     * @return Log probability of outcome given location and
     * cutpoints.
     *
     * @throw std::domain_error If the outcome is not between 1 and
     * the number of cutpoints plus 2; if the cutpoint vector is
     * empty; if the cutpoint vector contains a non-positive,
     * non-finite value; or if the cutpoint vector is not sorted in
     * ascending order.
     */
    template <bool propto, typename T_lambda, typename T_cut>
    typename boost::math::tools::promote_args<T_lambda, T_cut>::type
    ordered_logistic_log(int y, const T_lambda& lambda,
                         const Eigen::Matrix<T_cut, Eigen::Dynamic, 1>& c) {
      return ordered_logistic_lpmf<propto, T_lambda, T_cut>(y, lambda, c);
    }

    /**
     * @deprecated use ordered_logistic_lpmf
     */
    template <typename T_lambda, typename T_cut>
    typename boost::math::tools::promote_args<T_lambda, T_cut>::type
    ordered_logistic_log(int y, const T_lambda& lambda,
                         const Eigen::Matrix<T_cut, Eigen::Dynamic, 1>& c) {
      return ordered_logistic_lpmf<T_lambda, T_cut>(y, lambda, c);
    }

  }
}
#endif


#include <boost/hana/integral.hpp>

int main() {
    auto xs = <%= list(
        (0..x).map{ |i| "decltype(boost::hana::int_<#{i}>)" },
        (0..x).map{ |i| "boost::hana::int_<#{i}>" }
    ) %>;

    auto parts = partition(xs, [](auto x) {
        return x % boost::hana::int_<2> == boost::hana::int_<0>;
    });
}


#ifndef IMAGE_REPROJECTION_PLUGINS_PLANE_SURFACE_MODEL_HPP
#define IMAGE_REPROJECTION_PLUGINS_PLANE_SURFACE_MODEL_HPP

#include <string>

#include <image_reprojection/surface_model.hpp>
#include <image_reprojection_plugins/PlaneStamped.h>
#include <topic_tools/shape_shifter.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <opencv2/core/core.hpp>

namespace image_reprojection_plugins {

class PlaneSurfaceModel : public image_reprojection::SurfaceModel {
public:
  PlaneSurfaceModel() {}

  virtual ~PlaneSurfaceModel() {}

  virtual void update(const topic_tools::ShapeShifter &surface) {
    const PlaneStampedConstPtr plane(surface.instantiate< PlaneStamped >());
    CV_Assert(plane);
    update(*plane);
  }

  void update(const PlaneStamped &plane) {
    CV_Assert(plane.normal.x != 0. || plane.normal.y != 0. || plane.normal.z != 0.);

    boost::unique_lock< boost::shared_mutex > write_lock(mutex_);

    frame_id_ = plane.header.frame_id;
    point_ = cv::Vec3f(plane.point.x, plane.point.y, plane.point.z);
    normal_ = cv::Vec3f(plane.normal.x, plane.normal.y, plane.normal.z);
  }

  virtual std::string getFrameId() const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    return frame_id_;
  }

private:
  virtual void onInit() {}

  virtual void onIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                              cv::Mat &dst, cv::Mat &mask) const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    multirayPlaneIntersection(src_origin, src_direction, dst, mask);
  }

  void multirayPlaneIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                                 cv::Mat &dst, cv::Mat &mask) const {
    dst.create(src_direction.size(), CV_32FC3);
    for (int x = 0; x < src_direction.size().width; ++x) {
      for (int y = 0; y < src_direction.size().height; ++y) {
        unsigned char &m(mask.at< unsigned char >(y, x));
        const cv::Vec3f &sd(src_direction.at< cv::Vec3f >(y, x));
        cv::Vec3f &d(dst.at< cv::Vec3f >(y, x));
        m = (m != 0 && rayPlaneIntersection(src_origin, sd, d)) ? 1 : 0;
      }
    }
  }

  bool rayPlaneIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                            cv::Vec3f &dst) const {
    // position of ray origin with respect to principle point of plane
    const cv::Vec3f r(src_origin - point_);

    // intersection point (p) can be described as
    //    p = r + t * d
    //    (t >= 0)
    // where d is ray direction

    // calculate dot(d, n) for convenience
    const double ddn(src_direction.dot(normal_));
    if (ddn == 0.) { // means ray and plane are parallel
      return false;
    }
    const double ddn_sign(ddn > 0. ? 1. : -1.);
    const double ddn_norm(ddn > 0. ? ddn : -ddn);

    // t = -dot(r, n) / dot(d, n)
    const double srdn(ddn_sign * r.dot(normal_));
    if (srdn > 0.) { // means t < 0
      return false;
    }

    // intersection point with respect to plane coordinate frame
    dst = src_origin - srdn * src_direction / ddn_norm;
    return true;
  }

private:
  mutable boost::shared_mutex mutex_;
  std::string frame_id_;
  cv::Vec3f point_, normal_;
};

} // namespace image_reprojection_plugins

#endif


//------------------------------------------------------------------------------
// \file Functors_tests.cpp
//------------------------------------------------------------------------------
#include "Categories/Functors/Functor.h"

#include <algorithm> // std::move, std::transform
#include <array>
#include <boost/test/unit_test.hpp>
#include <cstring>
#include <iostream>
#include <iterator> // std::back_inserter
#include <list>
#include <memory>
#include <optional>
#include <string>
#include <vector>

using Categories::Functors::Functor;
using Categories::Functors::transform;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Functors)
BOOST_AUTO_TEST_SUITE(Functors_tests)

BOOST_AUTO_TEST_SUITE(FunctorExamples)

// cf. https://www.fpcomplete.com/blog/2012/07/the-functor-pattern-in-c
BOOST_AUTO_TEST_SUITE(FromFPCompleteBlog)

std::unique_ptr<int> lifted_length(std::unique_ptr<std::string> s)
{
  return std::unique_ptr<int>(new int((*s).length()));
}

template <class A, class B>
std::unique_ptr<B> fmap(std::function<B(A)> f, std::unique_ptr<A> a)
{
  return std::unique_ptr<B>(new B(f(*a)));
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TypeConstructionByUniquePtrs)
{
  std::unique_ptr<std::string> u_ptr =
    std::make_unique<std::string>("demo.txt");

  BOOST_TEST_REQUIRE(*u_ptr == "demo.txt");

  // cf. https://stackoverflow.com/questions/30905487/how-can-i-pass-stdunique-ptr-into-a-function
  // Move smart pointer into the function argument.
  std::unique_ptr<int> length_ptr = lifted_length(std::move(u_ptr));

  BOOST_TEST(*length_ptr == 8);

  std::function<int(std::string)> string_length = [](std::string s) -> int
  {
    return std::strlen(s.c_str());
  };

  std::unique_ptr<std::string> u_ptr1 =
    std::make_unique<std::string>("demo1.txt");

  auto length_ptr1 = fmap(string_length, std::move(u_ptr1));

  BOOST_TEST(*length_ptr1 == 9);
}

std::function<int(std::string)> string_length = [](std::string s) -> int
{
  return std::strlen(s.c_str());
};

std::vector<int> lifted_length(std::vector<std::string> strings)
{
  std::vector<int> lengths;
  std::transform(
    strings.begin(),
    strings.end(),
    // std::back_inserter is convenience function template that constructs a
    // std::back_insert_iterator for container c with type deduced from type of
    // argument
    // <iterator>
    // template <class Container>
    // std::back_insert_iterator<Container> back_inserter(Container& c);
    std::back_inserter(lengths),
    string_length);
  return lengths;
}

template <class A, class B>
std::vector<B> fmap(std::function<B(A)> f, std::vector<A> as)
{
  std::vector<B> bs;
  std::transform(as.begin(), as.end(), std::back_inserter(bs), f);
  return bs;
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FunctorsOnStdVectorIntroductoryExample)
{
  const std::vector<std::string> strings {"a", "bb", "ccc", "dddd"};

  const auto lengths = lifted_length(strings);

  BOOST_TEST(lengths[0] == 1);
  BOOST_TEST(lengths[1] == 2);
  BOOST_TEST(lengths[2] == 3);
  BOOST_TEST(lengths[3] == 4);

  const auto lengths_1 = fmap(string_length, strings);

  BOOST_TEST(lengths_1[0] == 1);
  BOOST_TEST(lengths_1[1] == 2);
  BOOST_TEST(lengths_1[2] == 3);
  BOOST_TEST(lengths_1[3] == 4);
}

std::function<int()> lifted_length(std::function<std::string()> fStr)
{
  return [fStr]()
  {
    return string_length(fStr());
  };
}

template <class A, class B>
std::function<B()> fmap(std::function<B(A)> f, std::function<A()> fA)
{
  return [f, fA]()
  {
    return f(fA());
  };
}

template <class C, class A, class B>
std::function<B(C)> fmap(std::function<B(A)> f, std::function<A(C)> rdA)
{
  return [f, rdA](C cfg)
  {
    return f(rdA(cfg));
  };
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FunctorsOnFunctionObjectsExamples)
{
  std::function<std::string()> example_string_function_object =
    []() -> std::string
    {
      return "Example static string function object";
    };

  const auto resulting_length = lifted_length(example_string_function_object);

  BOOST_TEST(resulting_length() == 37);

  const auto mapped_string_length =
    fmap(string_length, example_string_function_object);

  BOOST_TEST(mapped_string_length() == 37);
}

BOOST_AUTO_TEST_SUITE_END() // FromFPCompleteBlog

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FunctorDemonstration)
{
  {
    std::list<int> a {1, 2, 3};

    // <int, int> implies "int -> int" type annotation
    auto f = Functor<int, int> ([](int x) { return 2 * x;});
    auto g = Functor<int, int> ([](int x) { return 10 * x;});
    auto z = Functor<int, int> ([](int x) { return x + 1;});

    // Function composition preserving
    auto result1 = g(f(a));
    auto result2 = f(g(a));

    BOOST_TEST(result1 == result2);

    std::array<int, 3> expected {20, 40, 60};
    {
      int i {0};
      for (const auto& ele : result1)
      {
        BOOST_TEST(ele == expected[i]);
        i++;
      }
    }
  }

  BOOST_TEST(true);
}

template <typename T>
auto add_x(const T x)
{
  return [x](const T y)
    {
      return x + y;
    };
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateStdOptionalTransform)
{
  {
    // Test out add_x
    BOOST_TEST(add_x(42)(69) == 111);

    std::optional<int> a {69};
    const auto a_final {transform(a, add_x(42))};
    BOOST_TEST(a_final.value() == 111);
  }
}

BOOST_AUTO_TEST_SUITE_END() // FunctorExamples

BOOST_AUTO_TEST_SUITE_END() // Functors_tests
BOOST_AUTO_TEST_SUITE_END() // Functors
BOOST_AUTO_TEST_SUITE_END() // Categories


#include "KnowledgeComparitor.h"

#include <boost/algorithm/string.hpp>

/* implementation of KnowledgeComparitor.h */
namespace XAIPasS {

/**
     * returns true iff inequality is true
     */
bool KnowledgeComparitor::inequalityTrue(const KnowledgeItem &a, const std::vector<KnowledgeItem> &modelFunctions){

    double lhs = KnowledgeComparitor::evaluateExpression(a.ineq.LHS, modelFunctions);
    double rhs = KnowledgeComparitor::evaluateExpression(a.ineq.RHS, modelFunctions);

    switch(a.ineq.comparison_type) {
    case ComparisonType::GREATER:   return lhs>rhs;
    case ComparisonType::GREATEREQ: return lhs>=rhs;
    case ComparisonType::LESS:      return lhs<rhs;
    case ComparisonType::LESSEQ:    return lhs<=rhs;
    case ComparisonType::EQUALS:    return lhs==rhs;
    }
    std::cout<<" Knowledge Comapritor does not recognise comparison type."<<std::endl;
    return false;
}

/**
     * evaluates the prefix expression
     */
double KnowledgeComparitor::evaluateExpression(const ExprComposite &expr, const std::vector<KnowledgeItem> &modelFunctions) {

    std::vector<ExprBase> tokens = expr.tokens;

    bool pending_operand = false;
    std::vector<ExprBase> operator_stack;
    std::vector<ExprBase> operand_stack;
    for(int i=0;i<tokens.size();i++) {
        ExprBase token = tokens[i];
        switch(token.expr_type) {
        case ExpressionType::OPERATOR:
            operator_stack.push_back(token);
            pending_operand = false;
            break;
        case ExpressionType::CONSTANT:
        case ExpressionType::FUNCT:
        case ExpressionType::SPECIAL:

            if(pending_operand) {
                while (operand_stack.size()>0) {
                    ExprBase lhs = token;
                    ExprBase rhs = operand_stack.back();
                    operand_stack.pop_back();
                    ExprBase op = operator_stack.back();
                    operator_stack.back();
                    double value = KnowledgeComparitor::evaluateOperation(
                                KnowledgeComparitor::getValue(lhs, modelFunctions),
                                KnowledgeComparitor::getValue(rhs, modelFunctions),
                                op);
                    token.expr_type = ExpressionType::CONSTANT;
                    token.constant = value;
                }
            }
            pending_operand = true;
            operand_stack.push_back(token);
            break;
        }
    }

    ExprBase result = operand_stack.back();
    operand_stack.pop_back();
    return getValue(result, modelFunctions);
}

/*
     * evaluate numeric expression between two ExprBase messages.
     */
double KnowledgeComparitor::evaluateOperation(const double &lhs, const double &rhs, const ExprBase &op) {

    switch(op.op) {
    case Operators::ADD: return lhs+rhs;
    case Operators::SUB: return lhs-rhs;
    case Operators::MUL: return lhs*rhs;
    case Operators::DIV: return lhs/rhs;
    case Operators::UMINUS: return -lhs;
    }
    std::cout<<" Knowledge Comapritor does not recognise operator type."<<std::endl;
    return 0;
}

/*
     * Find numeric value of an ExprBase message, i.e. evaluate the value of PDDL function
     */
double KnowledgeComparitor::getValue(const ExprBase &expr, const std::vector<KnowledgeItem> &modelFunctions) {
    switch(expr.expr_type) {
    case ExpressionType::CONSTANT:
        return expr.constant;
    case ExpressionType::FUNCT:
        std::vector<KnowledgeItem>::const_iterator fit = modelFunctions.begin();
        for(; fit!=modelFunctions.end(); fit++) {
            if(expr.function.name == fit->attribute_name && expr.function.typed_parameters.size() == fit->values.size()) {
                // parameters
                bool match = true;
                for(size_t i=0;i<fit->values.size();i++) {
                    if("" != expr.function.typed_parameters[i].value && !(boost::iequals(expr.function.typed_parameters[i].value, fit->values[i].value))) {
                        match = false;
                    }
                }
                if(match) {
                    return fit->function_value;
                }
            }
        }
        return 0;
    }
    std::cout<<" Knowledge Comapritor cannot get value for base expression."<<std::endl;
    return 0;
}

/**
     * returns true iff a matches the knowledge in b.
     */
bool KnowledgeComparitor::containsKnowledge(const KnowledgeItem &a, const KnowledgeItem &b) {

    int matches = 0;
    if(a.knowledge_type != b.knowledge_type) return false;

    switch(a.knowledge_type) {

    case KnowledgeItemType::INSTANCE:
    {
        // check instance knowledge
        if(0!=a.instance_type.compare(b.instance_type)) return false;
        if(a.instance_name!="" && !boost::iequals(a.instance_name, b.instance_name)) return false;
        return true;
    }
        break;

    case KnowledgeItemType::FUNCTION:

        // compare functions (use _inequalityTrue_ to check function values)

        // check function or fact label and parameters all match
    case KnowledgeItemType::FACT:
    {
        // label
        if(a.attribute_name=="") return true;
        if(!boost::iequals(a.attribute_name, b.attribute_name)) return false;

        // negative fact
        if(a.is_negative != b.is_negative) return false;

        // parameters
        if(a.values.size() != b.values.size()) return false;
        for(size_t i=0;i<a.values.size();i++) {
            if("" == a.values[i].value) {
                ++matches;
            } else if(boost::iequals(a.values[i].value, b.values[i].value)) {
                ++matches;
            }
        }
        return (matches == a.values.size());
    }
        break;
    }
}

/**
     * returns true is the knowledge item contains the instance, as instance or attribute parameter.
     */
bool KnowledgeComparitor::containsInstance(const KnowledgeItem &a, std::string &name) {

    if(0==a.instance_name.compare(name))
        return true;

    for(size_t i=0;i<a.values.size();i++) {
        if(boost::iequals(a.values[i].value, name))
            return true;
    }

    return false;
}

} // close namespace


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <iterator>

#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>

#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>

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

BOOST_AUTO_TEST_CASE(value_type)
{
    BOOST_STATIC_ASSERT((
        boost::is_same<
            boost::compute::counting_iterator<int>::value_type,
            int
        >::value
    ));
    BOOST_STATIC_ASSERT((
        boost::is_same<
            boost::compute::counting_iterator<float>::value_type,
            float
        >::value
    ));
}

BOOST_AUTO_TEST_CASE(distance)
{
    BOOST_CHECK_EQUAL(
        std::distance(
            boost::compute::make_counting_iterator(0),
            boost::compute::make_counting_iterator(10)
        ),
        std::ptrdiff_t(10)
    );
    BOOST_CHECK_EQUAL(
        std::distance(
            boost::compute::make_counting_iterator(5),
            boost::compute::make_counting_iterator(10)
        ),
        std::ptrdiff_t(5)
    );
    BOOST_CHECK_EQUAL(
        std::distance(
            boost::compute::make_counting_iterator(-5),
            boost::compute::make_counting_iterator(5)
        ),
        std::ptrdiff_t(10)
    );
}

BOOST_AUTO_TEST_CASE(copy)
{
    boost::compute::vector<int> vector(10, context);

    boost::compute::copy(
        boost::compute::make_counting_iterator(1),
        boost::compute::make_counting_iterator(11),
        vector.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(
        int, 10, vector,
        (1, 2, 3, 4, 5, 6, 7, 8, 9, 10)
    );
}

BOOST_AUTO_TEST_CASE(iota_with_copy_doctest)
{
//! [iota_with_copy]
using boost::compute::make_counting_iterator;

boost::compute::vector<int> result(5, context);

boost::compute::copy(
    make_counting_iterator(1), make_counting_iterator(6), result.begin(), queue
);

// result == { 1, 2, 3, 4, 5 }
//! [iota_with_copy]

    CHECK_RANGE_EQUAL(int, 5, result, (1, 2, 3, 4, 5));
}

BOOST_AUTO_TEST_SUITE_END()


//  Copyright (c) 2018 Robert Ramey
//
// 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)

#include <iostream>

#include <boost/safe_numerics/checked_result.hpp>
#include <boost/safe_numerics/checked_result_operations.hpp>

int main(){
    using ext_uint = boost::safe_numerics::checked_result<unsigned int>;
    const ext_uint x{4};
    const ext_uint y{3};

    // operation is a success!
    std::cout << "success! x - y = " << x - y;

    // subtraction would result in -1, and invalid result for an unsigned value
    std::cout << "problem: y - x = " << y - x;

    const ext_uint z = y - x;
    std::cout << "z = " << z;
    // sum of two negative overflows is a negative overflow.
    std::cout << "z + z" << z + z;

    return 0;
}


/*
Copyright 2016 Australian Centre for Robotic Vision
*/

#ifndef APC_3D_VISION
#define APC_3D_VISION

#include <stdlib.h>     /* getenv */

#include <cv_bridge/cv_bridge.h>

#include <pcl_conversions/pcl_conversions.h>

// Point cloud library
#include <Eigen/Core>

#include <pcl/common/io.h>  // pcl::copyPointCloud
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/pca.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/segment_differences.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <pcl/surface/mls.h>

#include <boost/thread/thread.hpp>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <string>
#include <vector>
#include <iostream>
#include <map>

// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

class Apc3dVision {
 public:
    class ndt_params_t {  // type name ndt_params_t
     public:
        ndt_params_t(
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _input_cloud,
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _target_cloud)
            : input_cloud(_input_cloud), target_cloud(_target_cloud) {}

        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            input_cloud;
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            target_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        double input_cloud_leaf_size;
        bool is_downsample_input_cloud;
        double target_cloud_leaf_size;
        bool is_downsample_target_cloud;
        double transformationEpsilon;
        double stepSize;
        double resolution;
        int maxIterations;
        bool verbose;
        Eigen::Affine3f transform;
    };

    class icp_params_t {  // type name ndt_params_t
     public:
        icp_params_t(
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _input_cloud,
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _target_cloud)
            : input_cloud(_input_cloud), target_cloud(_target_cloud) {}

        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            input_cloud;
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            target_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        double input_cloud_leaf_size;
        bool is_downsample_input_cloud;
        double target_cloud_leaf_size;
        bool is_downsample_target_cloud;
        Eigen::Affine3f transform;
        double lowest_fitness_score;
        bool verbose;
    };

    class pca_params_t {
     public:
        pca_params_t(
            const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
                _input_cloud) : input_cloud(_input_cloud) {}

        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            input_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        Eigen::Matrix<double, 3, 3> eigenvectors;
        Eigen::Vector4d centroid;
        bool verbose;
    };

    class align_params_t {
     public:
        align_params_t(
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _input_cloud,
            boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _target_cloud)
            : input_cloud(_input_cloud), target_cloud(_target_cloud) {}

        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            input_cloud;
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>>
            target_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        double input_cloud_leaf_size;
        bool is_downsample_input_cloud;
        double target_cloud_leaf_size;
        bool is_downsample_target_cloud;
        double transformationEpsilon;
        double stepSize;
        double resolution;
        int maxIterations;
        bool verbose;
    };

    struct align_prerejective_params_t {
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> input_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> target_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        double leaf_size;
        double normalRadiusSearch;
        double featureRadiusSearch;
        int maxIterations;
        int numberOfSamples;
        int correspondenceRandomness;
        double similarityThreshold;
        double maxCorrespondenceDistance;
        double inlierFraction;
        bool verbose;
    };

    struct segment_differences_params_t {
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> input_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> target_cloud;
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud;
        double distanceThreshold;
        bool verbose;
    };

    Apc3dVision();

    ~Apc3dVision();

    bool load_pcd_file(std::string fileName,
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud);

    bool save_pcd_file(std::string fileName,
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> input_cloud);

    bool approximate_voxel_grid(
        const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ>> input_cloud,
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud,
        double leaf_size, bool verbose);

    bool align_ndt(ndt_params_t *params);

    bool align_icp(icp_params_t *params);

    bool align_icp_tote(icp_params_t *params);

    bool align_pca(pca_params_t *params);

    bool align(align_params_t *params);

    // http://pointclouds.org/documentation/tutorials/alignment_prerejective.php#alignment-prerejective
    bool align_prerejective(align_prerejective_params_t params);

    bool segment_differences(segment_differences_params_t params);

    bool smooth_cloud(
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> cloud,
        boost::shared_ptr<pcl::PointCloud<pcl::PointNormal>> output_cloud,
        float radius);

    bool outlier_removal(
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> cloud,
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> output_cloud,
        double outlierNumberOfSamples, double stdDevMulThresh);

    std::vector<int> get_labels_in_point_cloud(
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZL>> input_cloud);

    bool split_labelled_point_cloud(
        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZL>> input_cloud,
        std::map<int, boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>>
        output_clouds);
};

#endif